From 6b0ed71ae02a56692a80e47bd11baa7e14d9284e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 25 Sep 2012 16:36:33 +0200 Subject: [PATCH 1/8] Simplified magnetometer calibration routine --- apps/commander/commander.c | 179 +++++++++-------------------------- apps/systemlib/param/param.c | 4 +- 2 files changed, 49 insertions(+), 134 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 7535b90469..4c20a2f71e 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -307,30 +307,10 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) const uint64_t calibration_interval_us = 45 * 1000000; unsigned int calibration_counter = 0; - const int peak_samples = 2000; - /* Get rid of 10% */ - const int outlier_margin = (peak_samples) / 10; + const int peak_samples = 500; - float *mag_maxima[3]; - mag_maxima[0] = (float*)malloc(peak_samples * sizeof(float)); - mag_maxima[1] = (float*)malloc(peak_samples * sizeof(float)); - mag_maxima[2] = (float*)malloc(peak_samples * sizeof(float)); - - float *mag_minima[3]; - mag_minima[0] = (float*)malloc(peak_samples * sizeof(float)); - mag_minima[1] = (float*)malloc(peak_samples * sizeof(float)); - mag_minima[2] = (float*)malloc(peak_samples * sizeof(float)); - - /* initialize data table */ - for (int i = 0; i < peak_samples; i++) { - mag_maxima[0][i] = FLT_MIN; - mag_maxima[1][i] = FLT_MIN; - mag_maxima[2][i] = FLT_MIN; - - mag_minima[0][i] = FLT_MAX; - mag_minima[1][i] = FLT_MAX; - mag_minima[2][i] = FLT_MAX; - } + float mag_max[3] = {0, 0, 0}; + float mag_min[3] = {0, 0, 0}; int fd = open(MAG_DEVICE_PATH, 0); struct mag_scale mscale_null = { @@ -357,27 +337,25 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); /* get min/max values */ - /* iterate through full list */ - for (int i = 0; i < peak_samples; i++) { - /* x minimum */ - if (raw.magnetometer_raw[0] < mag_minima[0][i]) - mag_minima[0][i] = raw.magnetometer_ga[0]; - /* y minimum */ - if (raw.magnetometer_raw[1] < mag_minima[1][i]) - mag_minima[1][i] = raw.magnetometer_ga[1]; - /* z minimum */ - if (raw.magnetometer_raw[2] < mag_minima[2][i]) - mag_minima[2][i] = raw.magnetometer_ga[2]; + if (raw.magnetometer_raw[0] < mag_min[0]) { + mag_min[0] = raw.magnetometer_raw[0]; + } + else if (raw.magnetometer_raw[0] > mag_max[0]) { + mag_max[0] = raw.magnetometer_raw[0]; + } - /* x maximum */ - if (raw.magnetometer_raw[0] > mag_maxima[0][i]) - mag_maxima[0][i] = raw.magnetometer_ga[0]; - /* y maximum */ - if (raw.magnetometer_raw[1] > mag_maxima[1][i]) - mag_maxima[1][i] = raw.magnetometer_ga[1]; - /* z maximum */ - if (raw.magnetometer_raw[2] > mag_maxima[2][i]) - mag_maxima[2][i] = raw.magnetometer_ga[2]; + if (raw.magnetometer_raw[1] < mag_min[1]) { + mag_min[1] = raw.magnetometer_raw[1]; + } + else if (raw.magnetometer_raw[1] > mag_max[1]) { + mag_max[1] = raw.magnetometer_raw[1]; + } + + if (raw.magnetometer_raw[2] < mag_min[2]) { + mag_min[2] = raw.magnetometer_raw[2]; + } + else if (raw.magnetometer_raw[2] > mag_max[2]) { + mag_max[2] = raw.magnetometer_raw[2]; } calibration_counter++; @@ -392,67 +370,6 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) status->flag_preflight_mag_calibration = false; state_machine_publish(status_pub, status, mavlink_fd); - /* sort values */ - cal_bsort(mag_minima[0], peak_samples); - cal_bsort(mag_minima[1], peak_samples); - cal_bsort(mag_minima[2], peak_samples); - - cal_bsort(mag_maxima[0], peak_samples); - cal_bsort(mag_maxima[1], peak_samples); - cal_bsort(mag_maxima[2], peak_samples); - - float min_avg[3] = { 0.0f, 0.0f, 0.0f }; - float max_avg[3] = { 0.0f, 0.0f, 0.0f }; - - // printf("start:\n"); - - // for (int i = 0; i < 10; i++) { - // printf("mag min: %8.4f\t%8.4f\t%8.4f\tmax: %8.4f\t%8.4f\t%8.4f\n", - // mag_minima[0][i], - // mag_minima[1][i], - // mag_minima[2][i], - // mag_maxima[0][i], - // mag_maxima[1][i], - // mag_maxima[2][i]); - // usleep(10000); - // } - // printf("-----\n"); - - // for (int i = (peak_samples - outlier_margin)-10; i < (peak_samples - outlier_margin); i++) { - // printf("mag min: %8.4f\t%8.4f\t%8.4f\tmax: %8.4f\t%8.4f\t%8.4f\n", - // mag_minima[0][i], - // mag_minima[1][i], - // mag_minima[2][i], - // mag_maxima[0][i], - // mag_maxima[1][i], - // mag_maxima[2][i]); - // usleep(10000); - // } - - // printf("end\n"); - - /* take average of center value group */ - for (int i = 0; i < (peak_samples - outlier_margin); i++) { - min_avg[0] += mag_minima[0][i+outlier_margin]; - min_avg[1] += mag_minima[1][i+outlier_margin]; - min_avg[2] += mag_minima[2][i+outlier_margin]; - - max_avg[0] += mag_maxima[0][i]; - max_avg[1] += mag_maxima[1][i]; - max_avg[2] += mag_maxima[2][i]; - } - - min_avg[0] /= (peak_samples - outlier_margin); - min_avg[1] /= (peak_samples - outlier_margin); - min_avg[2] /= (peak_samples - outlier_margin); - - max_avg[0] /= (peak_samples - outlier_margin); - max_avg[1] /= (peak_samples - outlier_margin); - max_avg[2] /= (peak_samples - outlier_margin); - - // printf("\nFINAL:\nmag min: %8.4f\t%8.4f\t%8.4f\nmax: %8.4f\t%8.4f\t%8.4f\n", (double)min_avg[0], - // (double)min_avg[1], (double)min_avg[2], (double)max_avg[0], (double)max_avg[1], (double)max_avg[2]); - float mag_offset[3]; /** @@ -467,31 +384,37 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) * offset = (max + min) / 2.0f */ - mag_offset[0] = (max_avg[0] + min_avg[0]) / 2.0f; - mag_offset[1] = (max_avg[1] + min_avg[1]) / 2.0f; - mag_offset[2] = (max_avg[2] + min_avg[2]) / 2.0f; + printf("max 0: %f\n",mag_max[0]); + printf("max 1: %f\n",mag_max[1]); + printf("max 2: %f\n",mag_max[2]); + printf("min 0: %f\n",mag_min[0]); + printf("min 1: %f\n",mag_min[1]); + printf("min 2: %f\n",mag_min[2]); - if (!isfinite(mag_offset[1]) || !isfinite(mag_offset[1]) || !isfinite(mag_offset[2])) { + mag_offset[0] = (mag_max[0] + mag_min[0]) / 2.0f; + mag_offset[1] = (mag_max[1] + mag_min[1]) / 2.0f; + mag_offset[2] = (mag_max[2] + mag_min[2]) / 2.0f; - mavlink_log_critical(mavlink_fd, "[commander] MAG calibration failed (INF/NAN)"); - } else { - /* announce and set new offset */ + printf("mag off 0: %f\n",mag_offset[0]); + printf("mag off 1: %f\n",mag_offset[1]); + printf("mag off 2: %f\n",mag_offset[2]); - // char offset_output[50]; - // sprintf(offset_output, "[commander] mag cal: %8.4f %8.4f %8.4f", (double)mag_offset[0], (double)mag_offset[1], (double)mag_offset[2]); - // mavlink_log_info(mavlink_fd, offset_output); + /* announce and set new offset */ - if (param_set(param_find("SENS_MAG_XOFF"), &(mag_offset[0]))) { - fprintf(stderr, "[commander] Setting X mag offset failed!\n"); - } - - if (param_set(param_find("SENS_MAG_YOFF"), &(mag_offset[1]))) { - fprintf(stderr, "[commander] Setting Y mag offset failed!\n"); - } + // char offset_output[50]; + // sprintf(offset_output, "[commander] mag cal: %8.4f %8.4f %8.4f", (double)mag_offset[0], (double)mag_offset[1], (double)mag_offset[2]); + // mavlink_log_info(mavlink_fd, offset_output); - if (param_set(param_find("SENS_MAG_ZOFF"), &(mag_offset[2]))) { - fprintf(stderr, "[commander] Setting Z mag offset failed!\n"); - } + if (param_set(param_find("SENS_MAG_XOFF"), &(mag_offset[0]))) { + fprintf(stderr, "[commander] Setting X mag offset failed!\n"); + } + + if (param_set(param_find("SENS_MAG_YOFF"), &(mag_offset[1]))) { + fprintf(stderr, "[commander] Setting Y mag offset failed!\n"); + } + + if (param_set(param_find("SENS_MAG_ZOFF"), &(mag_offset[2]))) { + fprintf(stderr, "[commander] Setting Z mag offset failed!\n"); } fd = open(MAG_DEVICE_PATH, 0); @@ -507,14 +430,6 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) warn("WARNING: failed to set scale / offsets for mag"); close(fd); - free(mag_maxima[0]); - free(mag_maxima[1]); - free(mag_maxima[2]); - - free(mag_minima[0]); - free(mag_minima[1]); - free(mag_minima[2]); - /* auto-save to EEPROM */ int save_ret = pm_save_eeprom(false); if(save_ret != 0) { diff --git a/apps/systemlib/param/param.c b/apps/systemlib/param/param.c index a01c170a68..0ab7c0ea30 100644 --- a/apps/systemlib/param/param.c +++ b/apps/systemlib/param/param.c @@ -484,7 +484,7 @@ param_export(int fd, bool only_unsaved) s->unsaved = false; - /* append the appripriate BSON type object */ + /* append the appropriate BSON type object */ switch (param_type(s->param)) { case PARAM_TYPE_INT32: param_get(s->param, &i); @@ -688,4 +688,4 @@ param_foreach(void (*func)(void *arg, param_t param), void *arg, bool only_chang func(arg, param); } -} \ No newline at end of file +} From 5c00ca343fabce95273d15b13da4460935134fd3 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 25 Sep 2012 16:39:15 +0200 Subject: [PATCH 2/8] forgot to remove printfs of magnetometer calibration --- apps/commander/commander.c | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 4c20a2f71e..3ac01ee1bd 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -384,20 +384,20 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) * offset = (max + min) / 2.0f */ - printf("max 0: %f\n",mag_max[0]); - printf("max 1: %f\n",mag_max[1]); - printf("max 2: %f\n",mag_max[2]); - printf("min 0: %f\n",mag_min[0]); - printf("min 1: %f\n",mag_min[1]); - printf("min 2: %f\n",mag_min[2]); +// printf("max 0: %f\n",mag_max[0]); +// printf("max 1: %f\n",mag_max[1]); +// printf("max 2: %f\n",mag_max[2]); +// printf("min 0: %f\n",mag_min[0]); +// printf("min 1: %f\n",mag_min[1]); +// printf("min 2: %f\n",mag_min[2]); mag_offset[0] = (mag_max[0] + mag_min[0]) / 2.0f; mag_offset[1] = (mag_max[1] + mag_min[1]) / 2.0f; mag_offset[2] = (mag_max[2] + mag_min[2]) / 2.0f; - printf("mag off 0: %f\n",mag_offset[0]); - printf("mag off 1: %f\n",mag_offset[1]); - printf("mag off 2: %f\n",mag_offset[2]); +// printf("mag off 0: %f\n",mag_offset[0]); +// printf("mag off 1: %f\n",mag_offset[1]); +// printf("mag off 2: %f\n",mag_offset[2]); /* announce and set new offset */ From abbe998506e4ba49bbf6a9a9ae731b1eec521db6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 25 Sep 2012 21:35:02 +0200 Subject: [PATCH 3/8] ardrone in the air again (workaround: rate controller disabled) --- apps/ardrone_interface/ardrone_interface.c | 11 +++++++++-- apps/ardrone_interface/ardrone_motor_control.c | 9 ++++++--- .../multirotor_att_control_main.c | 16 +++++++++++++--- .../multirotor_attitude_control.c | 7 +++++++ 4 files changed, 35 insertions(+), 8 deletions(-) diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c index 8d77e7502e..f12f9cb474 100644 --- a/apps/ardrone_interface/ardrone_interface.c +++ b/apps/ardrone_interface/ardrone_interface.c @@ -239,14 +239,15 @@ int ardrone_interface_thread_main(int argc, char *argv[]) /* declare and safely initialize all structs */ struct vehicle_status_s state; - memset(&state, 0, sizeof(state)); + //memset(&state, 0, sizeof(state)); struct actuator_controls_s actuator_controls; - memset(&actuator_controls, 0, sizeof(actuator_controls)); + //memset(&actuator_controls, 0, sizeof(actuator_controls)); struct actuator_armed_s armed; armed.armed = false; /* subscribe to attitude, motor setpoints and system state */ int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + int state_sub = orb_subscribe(ORB_ID(vehicle_status)); int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); @@ -328,7 +329,13 @@ int ardrone_interface_thread_main(int argc, char *argv[]) * if in failsafe */ if (armed.armed && !armed.lockdown) { + + + + //printf("AMO_BEF: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",actuator_controls.control[0], actuator_controls.control[1], actuator_controls.control[2], actuator_controls.control[3]); + ardrone_mixing_and_output(ardrone_write, &actuator_controls); + } else { /* Silently lock down motor speeds to zero */ ardrone_write_motor_commands(ardrone_write, 0, 0, 0, 0); diff --git a/apps/ardrone_interface/ardrone_motor_control.c b/apps/ardrone_interface/ardrone_motor_control.c index 787db18773..cbf9600a59 100644 --- a/apps/ardrone_interface/ardrone_motor_control.c +++ b/apps/ardrone_interface/ardrone_motor_control.c @@ -368,6 +368,8 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls float yaw_control = actuators->control[2]; float motor_thrust = actuators->control[3]; + //printf("AMO: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",roll_control, pitch_control, yaw_control, motor_thrust); + const float min_thrust = 0.02f; /**< 2% minimum thrust */ const float max_thrust = 1.0f; /**< 100% max thrust */ const float scaling = 512.0f; /**< 100% thrust equals a value of 512 */ @@ -387,15 +389,16 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls if (motor_thrust <= min_thrust) { motor_thrust = min_thrust; output_band = 0.0f; - + //printf("0 silent\n"); } else if (motor_thrust < startpoint_full_control && motor_thrust > min_thrust) { output_band = band_factor * (motor_thrust - min_thrust); - + //printf("1 starting\n"); } else if (motor_thrust >= startpoint_full_control && motor_thrust < max_thrust - band_factor * startpoint_full_control) { output_band = band_factor * startpoint_full_control; - + //printf("2 working\n"); } else if (motor_thrust >= max_thrust - band_factor * startpoint_full_control) { output_band = band_factor * (max_thrust - motor_thrust); + //printf("3 full\n"); } //add the yaw, nick and roll components to the basic thrust //TODO:this should be done by the mixer diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 21c720096e..70b9d8e28d 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -76,7 +76,7 @@ __EXPORT int multirotor_att_control_main(int argc, char *argv[]); static bool thread_should_exit; static int mc_task; static bool motor_test_mode = false; -static struct actuator_controls_s actuators; + static orb_advert_t actuator_pub; /** @@ -86,6 +86,8 @@ static void *rate_control_thread_main(void *arg) { prctl(PR_SET_NAME, "mc rate control", getpid()); + struct actuator_controls_s actuators; + int gyro_sub = orb_subscribe(ORB_ID(sensor_gyro)); int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); @@ -125,8 +127,8 @@ static void *rate_control_thread_main(void *arg) gyro_lp[1] = gyro_report.y; gyro_lp[2] = gyro_report.z; - multirotor_control_rates(&rates_sp, gyro_lp, &actuators); - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); +// multirotor_control_rates(&rates_sp, gyro_lp, &actuators); +// orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); // } } } @@ -153,6 +155,8 @@ mc_thread_main(int argc, char *argv[]) struct vehicle_rates_setpoint_s rates_sp; memset(&rates_sp, 0, sizeof(rates_sp)); + struct actuator_controls_s actuators; + /* subscribe to attitude, motor setpoints and system state */ int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); @@ -228,6 +232,7 @@ mc_thread_main(int argc, char *argv[]) /* set yaw rate */ rates_sp.yaw = manual.yaw; att_sp.thrust = manual.throttle; + //printf("Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",att_sp.roll_body, att_sp.pitch_body, att_sp.yaw_body, att_sp.thrust); att_sp.timestamp = hrt_absolute_time(); /* STEP 2: publish the result to the vehicle actuators */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); @@ -269,10 +274,15 @@ mc_thread_main(int argc, char *argv[]) /* run attitude controller */ if (state.flag_control_attitude_enabled && !state.flag_control_rates_enabled) { multirotor_control_attitude(&att_sp, &att, NULL, &actuators); +// printf("publish actuator\n"); + +// printf("MAC_PUB: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",actuators.control[0], actuators.control[1], actuators.control[2], actuators.control[3]); + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); } else if (state.flag_control_attitude_enabled && state.flag_control_rates_enabled) { multirotor_control_attitude(&att_sp, &att, &rates_sp, NULL); orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); +// printf("publish attitude\n"); } diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c index 2129915d12..c25e96856a 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -312,6 +312,10 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s actuators->control[3] = motor_thrust; } +// if(motor_skip_counter%20 == 0) +// printf("MAC: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",actuators->control[0], actuators->control[1], actuators->control[2], actuators->control[3]); + + // XXX change yaw rate to yaw pos controller if (rates_sp) { rates_sp->roll = roll_control; @@ -320,5 +324,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s rates_sp->thrust = motor_thrust; } +// if(motor_skip_counter%20 == 0) +// printf("Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",rates_sp->roll, rates_sp->pitch, rates_sp->yaw, rates_sp->thrust); + motor_skip_counter++; } From 201fdbc42c46bc9146a8cbf2434a98792d6d9f50 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 26 Sep 2012 10:11:57 +0200 Subject: [PATCH 4/8] ardrone flying now (still workaround of disabled rates controller) --- .../multirotor_att_control_main.c | 15 ++++++++------- apps/sensors/sensors.cpp | 4 +++- 2 files changed, 11 insertions(+), 8 deletions(-) diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 70b9d8e28d..904872dde2 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -127,8 +127,8 @@ static void *rate_control_thread_main(void *arg) gyro_lp[1] = gyro_report.y; gyro_lp[2] = gyro_report.z; -// multirotor_control_rates(&rates_sp, gyro_lp, &actuators); -// orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + multirotor_control_rates(&rates_sp, gyro_lp, &actuators); + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); // } } } @@ -192,10 +192,10 @@ mc_thread_main(int argc, char *argv[]) /* ready, spawn pthread */ pthread_attr_t rate_control_attr; - pthread_attr_init(&rate_control_attr); - pthread_attr_setstacksize(&rate_control_attr, 2048); - pthread_t rate_control_thread; - pthread_create(&rate_control_thread, &rate_control_attr, rate_control_thread_main, NULL); +// pthread_attr_init(&rate_control_attr); +// pthread_attr_setstacksize(&rate_control_attr, 2048); +// pthread_t rate_control_thread; +// pthread_create(&rate_control_thread, &rate_control_attr, rate_control_thread_main, NULL); while (!thread_should_exit) { @@ -306,7 +306,7 @@ mc_thread_main(int argc, char *argv[]) perf_print_counter(mc_loop_perf); perf_free(mc_loop_perf); - pthread_join(rate_control_thread, NULL); + //pthread_join(rate_control_thread, NULL); fflush(stdout); exit(0); @@ -340,6 +340,7 @@ int multirotor_att_control_main(int argc, char *argv[]) default: fprintf(stderr, "option: -%c\n", ch); usage("unrecognized option"); + break; } } argc -= optioncount; diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index eb22ac8a70..ceb8c4b104 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -968,7 +968,7 @@ Sensors::ppm_poll() _rc.chan[i].scaled = (ppm_buffer[i] - _parameters.trim[i]) / (float)(_parameters.max[i] - _parameters.trim[i]); } else if (ppm_buffer[i] < (_parameters.trim[i] - _parameters.dz[i])) { /* division by zero impossible for trim == min (as for throttle), as this falls in the above if clause */ - _rc.chan[i].scaled = -1.0f + ((ppm_buffer[i] - _parameters.min[i]) / (float)(_parameters.trim[i] - _parameters.min[i])); + _rc.chan[i].scaled = -((_parameters.trim[i] - ppm_buffer[i]) / (float)(_parameters.trim[i] - _parameters.min[i])); } else { /* in the configured dead zone, output zero */ @@ -1029,6 +1029,8 @@ Sensors::ppm_poll() orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc); orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control); + +// printf("SENSORS: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",manual_control.roll, manual_control.pitch, manual_control.yaw, manual_control.throttle); } #endif From ac43a67a0ff8be62504e3398def6b1f6f0719e14 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 26 Sep 2012 14:29:47 +0200 Subject: [PATCH 5/8] ardrone max motor output was slightly to high --- apps/ardrone_interface/ardrone_motor_control.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/ardrone_interface/ardrone_motor_control.c b/apps/ardrone_interface/ardrone_motor_control.c index cbf9600a59..89ed183ccd 100644 --- a/apps/ardrone_interface/ardrone_motor_control.c +++ b/apps/ardrone_interface/ardrone_motor_control.c @@ -372,7 +372,7 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls const float min_thrust = 0.02f; /**< 2% minimum thrust */ const float max_thrust = 1.0f; /**< 100% max thrust */ - const float scaling = 512.0f; /**< 100% thrust equals a value of 512 */ + const float scaling = 500.0f; /**< 100% thrust equals a value of 500 which works, 512 leads to cutoff */ const float min_gas = min_thrust * scaling; /**< value range sent to motors, minimum */ const float max_gas = max_thrust * scaling; /**< value range sent to motors, maximum */ From 7f153098926bf977609c6efb53fa7cb5093564af Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 27 Sep 2012 16:50:20 +0200 Subject: [PATCH 6/8] Calibration should not freeze anymore, ardrone flying but estimator is not able to use calibrated magnetometer data --- apps/ardrone_interface/ardrone_interface.c | 3 +- apps/commander/commander.c | 48 +++++++------------ .../multirotor_att_control_main.c | 10 ++-- 3 files changed, 23 insertions(+), 38 deletions(-) diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c index f0bc0b1e78..8b4b5c400f 100644 --- a/apps/ardrone_interface/ardrone_interface.c +++ b/apps/ardrone_interface/ardrone_interface.c @@ -140,9 +140,8 @@ static int ardrone_open_uart(char *uart_name, struct termios *uart_config_origin int speed = B115200; int uart; - /* open uart */ - printf("[ardrone_interface] UART is %s, baud rate is%d\n",uart_name,speed); + //printf("[ardrone_interface] UART is %s, baud rate is%d\n",uart_name,speed); uart = open(uart_name, O_RDWR | O_NOCTTY); /* Try to set baud rate */ diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 66a981a685..a1c2a15d26 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -312,8 +312,6 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) const uint64_t calibration_interval_us = 45 * 1000000; unsigned int calibration_counter = 0; - const int peak_samples = 500; - float mag_max[3] = {0, 0, 0}; float mag_min[3] = {0, 0, 0}; @@ -366,8 +364,8 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) calibration_counter++; } else { /* any poll failure for 1s is a reason to abort */ - //mavlink_log_info(mavlink_fd, "[commander] mag calibration aborted, please retry."); - //break; + mavlink_log_info(mavlink_fd, "[commander] mag calibration aborted, please retry."); + break; } } @@ -389,20 +387,20 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) * offset = (max + min) / 2.0f */ -// printf("max 0: %f\n",mag_max[0]); -// printf("max 1: %f\n",mag_max[1]); -// printf("max 2: %f\n",mag_max[2]); -// printf("min 0: %f\n",mag_min[0]); -// printf("min 1: %f\n",mag_min[1]); -// printf("min 2: %f\n",mag_min[2]); + printf("max 0: %f\n",mag_max[0]); + printf("max 1: %f\n",mag_max[1]); + printf("max 2: %f\n",mag_max[2]); + printf("min 0: %f\n",mag_min[0]); + printf("min 1: %f\n",mag_min[1]); + printf("min 2: %f\n",mag_min[2]); mag_offset[0] = (mag_max[0] + mag_min[0]) / 2.0f; mag_offset[1] = (mag_max[1] + mag_min[1]) / 2.0f; mag_offset[2] = (mag_max[2] + mag_min[2]) / 2.0f; -// printf("mag off 0: %f\n",mag_offset[0]); -// printf("mag off 1: %f\n",mag_offset[1]); -// printf("mag off 2: %f\n",mag_offset[2]); + printf("mag off 0: %f\n",mag_offset[0]); + printf("mag off 1: %f\n",mag_offset[1]); + printf("mag off 2: %f\n",mag_offset[2]); /* announce and set new offset */ @@ -487,7 +485,7 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) calibration_counter++; } else { /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "[commander] gyro calibration aborted, please retry."); + mavlink_log_info(mavlink_fd, "[commander] gyro calibration aborted, retry"); return; } } @@ -534,9 +532,7 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) mavlink_log_info(mavlink_fd, "[commander] gyro calibration finished"); - // char offset_output[50]; - // sprintf(offset_output, "[commander] gyro cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]); - // mavlink_log_info(mavlink_fd, offset_output); + printf("[commander] gyro cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]); close(sub_sensor_combined); } @@ -544,9 +540,8 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) void do_accel_calibration(int status_pub, struct vehicle_status_s *status) { /* announce change */ - usleep(5000); - mavlink_log_info(mavlink_fd, "[commander] The system should be level and not moved"); + mavlink_log_info(mavlink_fd, "[commander] keep it level and still"); /* set to accel calibration mode */ status->flag_preflight_accel_calibration = true; state_machine_publish(status_pub, status, mavlink_fd); @@ -571,7 +566,6 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale_null)) warn("WARNING: failed to set scale / offsets for accel"); close(fd); - while (calibration_counter < calibration_count) { /* wait blocking for new data */ @@ -585,11 +579,10 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) calibration_counter++; } else { /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "[commander] gyro calibration aborted, please retry."); + mavlink_log_info(mavlink_fd, "[commander] acceleration calibration aborted"); return; } } - accel_offset[0] = accel_offset[0] / calibration_count; accel_offset[1] = accel_offset[1] / calibration_count; accel_offset[2] = accel_offset[2] / calibration_count; @@ -644,18 +637,11 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) if(save_ret != 0) { warn("WARNING: auto-save of params to EEPROM failed"); } - /* exit to gyro calibration mode */ status->flag_preflight_accel_calibration = false; state_machine_publish(status_pub, status, mavlink_fd); - mavlink_log_info(mavlink_fd, "[commander] acceleration calibration finished"); - - // char offset_output[50]; - // sprintf(offset_output, "[commander] accel cal: x:%8.4f y:%8.4f z:%8.4f", (double)accel_offset[0], - // (double)accel_offset[1], (double)accel_offset[2]); - // mavlink_log_info(mavlink_fd, offset_output); - + printf("[commander] accel calibration: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0],(double)accel_offset[1], (double)accel_offset[2]); close(sub_sensor_combined); } @@ -823,7 +809,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* none found */ if (!handled) { //fprintf(stderr, "[commander] refusing unsupported calibration request\n"); - mavlink_log_critical(mavlink_fd, "[commander] CMD refusing unsupported calibration request"); + mavlink_log_critical(mavlink_fd, "[commander] CMD refusing unsup. calib. request"); result = MAV_RESULT_UNSUPPORTED; } } diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 8d648c1944..b00b6bc0ca 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -193,10 +193,10 @@ mc_thread_main(int argc, char *argv[]) /* ready, spawn pthread */ pthread_attr_t rate_control_attr; -// pthread_attr_init(&rate_control_attr); -// pthread_attr_setstacksize(&rate_control_attr, 2048); -// pthread_t rate_control_thread; -// pthread_create(&rate_control_thread, &rate_control_attr, rate_control_thread_main, NULL); + pthread_attr_init(&rate_control_attr); + pthread_attr_setstacksize(&rate_control_attr, 2048); + pthread_t rate_control_thread; + pthread_create(&rate_control_thread, &rate_control_attr, rate_control_thread_main, NULL); while (!thread_should_exit) { @@ -310,7 +310,7 @@ mc_thread_main(int argc, char *argv[]) perf_print_counter(mc_loop_perf); perf_free(mc_loop_perf); - //pthread_join(rate_control_thread, NULL); + pthread_join(rate_control_thread, NULL); fflush(stdout); exit(0); From 2c5c3141057be1f46cb4a33f71e2331ce36b18a7 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 27 Sep 2012 17:08:29 +0200 Subject: [PATCH 7/8] Cleanup of lots of debugging printfs --- apps/ardrone_interface/ardrone_interface.c | 11 ++--------- apps/ardrone_interface/ardrone_motor_control.c | 4 ---- apps/commander/commander.c | 4 ---- .../multirotor_att_control_main.c | 6 ------ .../multirotor_attitude_control.c | 7 ------- apps/sensors/sensors.cpp | 1 - 6 files changed, 2 insertions(+), 31 deletions(-) diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c index 8b4b5c400f..8ed6db6642 100644 --- a/apps/ardrone_interface/ardrone_interface.c +++ b/apps/ardrone_interface/ardrone_interface.c @@ -141,7 +141,6 @@ static int ardrone_open_uart(char *uart_name, struct termios *uart_config_origin int uart; /* open uart */ - //printf("[ardrone_interface] UART is %s, baud rate is%d\n",uart_name,speed); uart = open(uart_name, O_RDWR | O_NOCTTY); /* Try to set baud rate */ @@ -238,15 +237,14 @@ int ardrone_interface_thread_main(int argc, char *argv[]) /* declare and safely initialize all structs */ struct vehicle_status_s state; - //memset(&state, 0, sizeof(state)); + memset(&state, 0, sizeof(state)); struct actuator_controls_s actuator_controls; - //memset(&actuator_controls, 0, sizeof(actuator_controls)); + memset(&actuator_controls, 0, sizeof(actuator_controls)); struct actuator_armed_s armed; armed.armed = false; /* subscribe to attitude, motor setpoints and system state */ int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); - int state_sub = orb_subscribe(ORB_ID(vehicle_status)); int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); @@ -328,11 +326,6 @@ int ardrone_interface_thread_main(int argc, char *argv[]) * if in failsafe */ if (armed.armed && !armed.lockdown) { - - - - //printf("AMO_BEF: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",actuator_controls.control[0], actuator_controls.control[1], actuator_controls.control[2], actuator_controls.control[3]); - ardrone_mixing_and_output(ardrone_write, &actuator_controls); } else { diff --git a/apps/ardrone_interface/ardrone_motor_control.c b/apps/ardrone_interface/ardrone_motor_control.c index e410d3a71a..c68a26df96 100644 --- a/apps/ardrone_interface/ardrone_motor_control.c +++ b/apps/ardrone_interface/ardrone_motor_control.c @@ -388,16 +388,12 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls if (motor_thrust <= min_thrust) { motor_thrust = min_thrust; output_band = 0.0f; - //printf("0 silent\n"); } else if (motor_thrust < startpoint_full_control && motor_thrust > min_thrust) { output_band = band_factor * (motor_thrust - min_thrust); - //printf("1 starting\n"); } else if (motor_thrust >= startpoint_full_control && motor_thrust < max_thrust - band_factor * startpoint_full_control) { output_band = band_factor * startpoint_full_control; - //printf("2 working\n"); } else if (motor_thrust >= max_thrust - band_factor * startpoint_full_control) { output_band = band_factor * (max_thrust - motor_thrust); - //printf("3 full\n"); } //add the yaw, nick and roll components to the basic thrust //TODO:this should be done by the mixer diff --git a/apps/commander/commander.c b/apps/commander/commander.c index a1c2a15d26..1c23c1f9d2 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -404,10 +404,6 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) /* announce and set new offset */ - // char offset_output[50]; - // sprintf(offset_output, "[commander] mag cal: %8.4f %8.4f %8.4f", (double)mag_offset[0], (double)mag_offset[1], (double)mag_offset[2]); - // mavlink_log_info(mavlink_fd, offset_output); - if (param_set(param_find("SENS_MAG_XOFF"), &(mag_offset[0]))) { fprintf(stderr, "[commander] Setting X mag offset failed!\n"); } diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index b00b6bc0ca..ebd9911a36 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -236,7 +236,6 @@ mc_thread_main(int argc, char *argv[]) /* set yaw rate */ rates_sp.yaw = manual.yaw; att_sp.thrust = manual.throttle; - //printf("Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",att_sp.roll_body, att_sp.pitch_body, att_sp.yaw_body, att_sp.thrust); att_sp.timestamp = hrt_absolute_time(); /* STEP 2: publish the result to the vehicle actuators */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); @@ -278,15 +277,10 @@ mc_thread_main(int argc, char *argv[]) /* run attitude controller */ if (state.flag_control_attitude_enabled && !state.flag_control_rates_enabled) { multirotor_control_attitude(&att_sp, &att, NULL, &actuators); -// printf("publish actuator\n"); - -// printf("MAC_PUB: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",actuators.control[0], actuators.control[1], actuators.control[2], actuators.control[3]); - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); } else if (state.flag_control_attitude_enabled && state.flag_control_rates_enabled) { multirotor_control_attitude(&att_sp, &att, &rates_sp, NULL); orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); -// printf("publish attitude\n"); } diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c index c25e96856a..2129915d12 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -312,10 +312,6 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s actuators->control[3] = motor_thrust; } -// if(motor_skip_counter%20 == 0) -// printf("MAC: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",actuators->control[0], actuators->control[1], actuators->control[2], actuators->control[3]); - - // XXX change yaw rate to yaw pos controller if (rates_sp) { rates_sp->roll = roll_control; @@ -324,8 +320,5 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s rates_sp->thrust = motor_thrust; } -// if(motor_skip_counter%20 == 0) -// printf("Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",rates_sp->roll, rates_sp->pitch, rates_sp->yaw, rates_sp->thrust); - motor_skip_counter++; } diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index ceb8c4b104..f81dfa9b8f 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -1030,7 +1030,6 @@ Sensors::ppm_poll() orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc); orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control); -// printf("SENSORS: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",manual_control.roll, manual_control.pitch, manual_control.yaw, manual_control.throttle); } #endif From d206327541f159ac4abd76e66bce3160e8704231 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 27 Sep 2012 18:43:04 +0200 Subject: [PATCH 8/8] Magnetometer calibration fixed --- apps/commander/commander.c | 55 +++++++++++--------------------------- 1 file changed, 15 insertions(+), 40 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 1c23c1f9d2..960c5d3837 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -256,22 +256,6 @@ int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, u return 0; } -static void cal_bsort(float a[], int n) -{ - int i,j,t; - for(i=0;ia[j+1]) { - t=a[j]; - a[j]=a[j+1]; - a[j+1]=t; - } - } - } -} - static const char *parameter_file = "/eeprom/parameters"; static int pm_save_eeprom(bool only_unsaved) @@ -312,8 +296,8 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) const uint64_t calibration_interval_us = 45 * 1000000; unsigned int calibration_counter = 0; - float mag_max[3] = {0, 0, 0}; - float mag_min[3] = {0, 0, 0}; + float mag_max[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX}; + float mag_min[3] = {FLT_MAX, FLT_MAX, FLT_MAX}; int fd = open(MAG_DEVICE_PATH, 0); struct mag_scale mscale_null = { @@ -340,25 +324,25 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); /* get min/max values */ - if (raw.magnetometer_raw[0] < mag_min[0]) { - mag_min[0] = raw.magnetometer_raw[0]; + if (raw.magnetometer_ga[0] < mag_min[0]) { + mag_min[0] = raw.magnetometer_ga[0]; } - else if (raw.magnetometer_raw[0] > mag_max[0]) { - mag_max[0] = raw.magnetometer_raw[0]; + else if (raw.magnetometer_ga[0] > mag_max[0]) { + mag_max[0] = raw.magnetometer_ga[0]; } - if (raw.magnetometer_raw[1] < mag_min[1]) { - mag_min[1] = raw.magnetometer_raw[1]; + if (raw.magnetometer_ga[1] < mag_min[1]) { + mag_min[1] = raw.magnetometer_ga[1]; } - else if (raw.magnetometer_raw[1] > mag_max[1]) { - mag_max[1] = raw.magnetometer_raw[1]; + else if (raw.magnetometer_ga[1] > mag_max[1]) { + mag_max[1] = raw.magnetometer_ga[1]; } - if (raw.magnetometer_raw[2] < mag_min[2]) { - mag_min[2] = raw.magnetometer_raw[2]; + if (raw.magnetometer_ga[2] < mag_min[2]) { + mag_min[2] = raw.magnetometer_ga[2]; } - else if (raw.magnetometer_raw[2] > mag_max[2]) { - mag_max[2] = raw.magnetometer_raw[2]; + else if (raw.magnetometer_ga[2] > mag_max[2]) { + mag_max[2] = raw.magnetometer_ga[2]; } calibration_counter++; @@ -387,20 +371,11 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) * offset = (max + min) / 2.0f */ - printf("max 0: %f\n",mag_max[0]); - printf("max 1: %f\n",mag_max[1]); - printf("max 2: %f\n",mag_max[2]); - printf("min 0: %f\n",mag_min[0]); - printf("min 1: %f\n",mag_min[1]); - printf("min 2: %f\n",mag_min[2]); - mag_offset[0] = (mag_max[0] + mag_min[0]) / 2.0f; mag_offset[1] = (mag_max[1] + mag_min[1]) / 2.0f; mag_offset[2] = (mag_max[2] + mag_min[2]) / 2.0f; - printf("mag off 0: %f\n",mag_offset[0]); - printf("mag off 1: %f\n",mag_offset[1]); - printf("mag off 2: %f\n",mag_offset[2]); + printf("mag off x: %4.4f, y: %4.4f, z: %4.4f\n",(double)mag_offset[0],(double)mag_offset[0],(double)mag_offset[2]); /* announce and set new offset */