diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-07-20 13:47:51 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-07-20 13:47:51 +0200 |
commit | 68ab69de010adbe37b37558824ca013ecd0d569a (patch) | |
tree | 4b50fd7f8350f4baa5d3deed813938d3dd568778 /src/modules/commander | |
parent | da1bf69ce249f22df16e7ccb21d17c08bcbbbedf (diff) | |
download | px4-firmware-68ab69de010adbe37b37558824ca013ecd0d569a.tar.gz px4-firmware-68ab69de010adbe37b37558824ca013ecd0d569a.tar.bz2 px4-firmware-68ab69de010adbe37b37558824ca013ecd0d569a.zip |
moved commander to C++, preparation for better gyro scale calibration respecting the current attitude for more accurate results
Diffstat (limited to 'src/modules/commander')
-rw-r--r-- | src/modules/commander/accelerometer_calibration.cpp (renamed from src/modules/commander/accelerometer_calibration.c) | 59 | ||||
-rw-r--r-- | src/modules/commander/airspeed_calibration.cpp (renamed from src/modules/commander/airspeed_calibration.c) | 6 | ||||
-rw-r--r-- | src/modules/commander/baro_calibration.cpp (renamed from src/modules/commander/baro_calibration.c) | 2 | ||||
-rw-r--r-- | src/modules/commander/calibration_routines.cpp (renamed from src/modules/commander/calibration_routines.c) | 2 | ||||
-rw-r--r-- | src/modules/commander/commander.cpp (renamed from src/modules/commander/commander.c) | 19 | ||||
-rw-r--r-- | src/modules/commander/commander_helper.cpp (renamed from src/modules/commander/commander_helper.c) | 13 | ||||
-rw-r--r-- | src/modules/commander/commander_params.c (renamed from src/modules/commander/commander.h) | 28 | ||||
-rw-r--r-- | src/modules/commander/gyro_calibration.cpp (renamed from src/modules/commander/gyro_calibration.c) | 70 | ||||
-rw-r--r-- | src/modules/commander/mag_calibration.cpp (renamed from src/modules/commander/mag_calibration.c) | 6 | ||||
-rw-r--r-- | src/modules/commander/module.mk | 21 | ||||
-rw-r--r-- | src/modules/commander/rc_calibration.cpp (renamed from src/modules/commander/rc_calibration.c) | 2 | ||||
-rw-r--r-- | src/modules/commander/state_machine_helper.cpp (renamed from src/modules/commander/state_machine_helper.c) | 7 |
12 files changed, 152 insertions, 83 deletions
diff --git a/src/modules/commander/accelerometer_calibration.c b/src/modules/commander/accelerometer_calibration.cpp index bc9d24956..df92d51d2 100644 --- a/src/modules/commander/accelerometer_calibration.c +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -33,7 +33,7 @@ ****************************************************************************/ /** - * @file accelerometer_calibration.c + * @file accelerometer_calibration.cpp * * Implementation of accelerometer calibration. * @@ -113,8 +113,6 @@ #include <sys/prctl.h> #include <math.h> #include <string.h> - - #include <drivers/drv_hrt.h> #include <uORB/topics/sensor_combined.h> #include <drivers/drv_accel.h> @@ -123,6 +121,12 @@ #include <systemlib/err.h> #include <mavlink/mavlink_log.h> +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]); int detect_orientation(int mavlink_fd, int sub_sensor_combined); int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num); @@ -229,7 +233,10 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float sprintf(str, "meas started: %s", orientation_strs[orient]); mavlink_log_info(mavlink_fd, str); read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num); - str_ptr = sprintf(str, "meas result for %s: [ %.2f %.2f %.2f ]", orientation_strs[orient], accel_ref[orient][0], accel_ref[orient][1], accel_ref[orient][2]); + str_ptr = sprintf(str, "meas result for %s: [ %.2f %.2f %.2f ]", orientation_strs[orient], + (double)accel_ref[orient][0], + (double)accel_ref[orient][1], + (double)accel_ref[orient][2]); mavlink_log_info(mavlink_fd, str); data_collected[orient] = true; tune_neutral(); @@ -274,7 +281,9 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { float accel_err_thr = 5.0f; /* still time required in us */ int64_t still_time = 2000000; - struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; + struct pollfd fds[1]; + fds[0].fd = sub_sensor_combined; + fds[0].events = POLLIN; hrt_abstime t_start = hrt_absolute_time(); /* set timeout to 30s */ @@ -342,29 +351,29 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { } } - if ( fabs(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr && - fabs(accel_ema[1]) < accel_err_thr && - fabs(accel_ema[2]) < accel_err_thr ) + if ( fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr && + fabsf(accel_ema[1]) < accel_err_thr && + fabsf(accel_ema[2]) < accel_err_thr ) return 0; // [ g, 0, 0 ] - if ( fabs(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr && - fabs(accel_ema[1]) < accel_err_thr && - fabs(accel_ema[2]) < accel_err_thr ) + if ( fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr && + fabsf(accel_ema[1]) < accel_err_thr && + fabsf(accel_ema[2]) < accel_err_thr ) return 1; // [ -g, 0, 0 ] - if ( fabs(accel_ema[0]) < accel_err_thr && - fabs(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr && - fabs(accel_ema[2]) < accel_err_thr ) + if ( fabsf(accel_ema[0]) < accel_err_thr && + fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr && + fabsf(accel_ema[2]) < accel_err_thr ) return 2; // [ 0, g, 0 ] - if ( fabs(accel_ema[0]) < accel_err_thr && - fabs(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr && - fabs(accel_ema[2]) < accel_err_thr ) + if ( fabsf(accel_ema[0]) < accel_err_thr && + fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr && + fabsf(accel_ema[2]) < accel_err_thr ) return 3; // [ 0, -g, 0 ] - if ( fabs(accel_ema[0]) < accel_err_thr && - fabs(accel_ema[1]) < accel_err_thr && - fabs(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr ) + if ( fabsf(accel_ema[0]) < accel_err_thr && + fabsf(accel_ema[1]) < accel_err_thr && + fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr ) return 4; // [ 0, 0, g ] - if ( fabs(accel_ema[0]) < accel_err_thr && - fabs(accel_ema[1]) < accel_err_thr && - fabs(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr ) + if ( fabsf(accel_ema[0]) < accel_err_thr && + fabsf(accel_ema[1]) < accel_err_thr && + fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr ) return 5; // [ 0, 0, -g ] mavlink_log_info(mavlink_fd, "ERROR: invalid orientation"); @@ -376,7 +385,9 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { * Read specified number of accelerometer samples, calculate average and dispersion. */ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num) { - struct pollfd fds[1] = { { .fd = sensor_combined_sub, .events = POLLIN } }; + struct pollfd fds[1]; + fds[0].fd = sensor_combined_sub; + fds[0].events = POLLIN; int count = 0; float accel_sum[3] = { 0.0f, 0.0f, 0.0f }; diff --git a/src/modules/commander/airspeed_calibration.c b/src/modules/commander/airspeed_calibration.cpp index feaf11aee..df08292e3 100644 --- a/src/modules/commander/airspeed_calibration.c +++ b/src/modules/commander/airspeed_calibration.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file airspeed_calibration.c + * @file airspeed_calibration.cpp * Airspeed sensor calibration routine */ @@ -65,7 +65,9 @@ void do_airspeed_calibration(int mavlink_fd) while (calibration_counter < calibration_count) { /* wait blocking for new data */ - struct pollfd fds[1] = { { .fd = diff_pres_sub, .events = POLLIN } }; + struct pollfd fds[1]; + fds[0].fd = diff_pres_sub; + fds[0].events = POLLIN; int poll_ret = poll(fds, 1, 1000); diff --git a/src/modules/commander/baro_calibration.c b/src/modules/commander/baro_calibration.cpp index a70594794..d7515b3d9 100644 --- a/src/modules/commander/baro_calibration.c +++ b/src/modules/commander/baro_calibration.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file baro_calibration.c + * @file baro_calibration.cpp * Barometer calibration routine */ diff --git a/src/modules/commander/calibration_routines.c b/src/modules/commander/calibration_routines.cpp index 799cd4269..be38ea104 100644 --- a/src/modules/commander/calibration_routines.c +++ b/src/modules/commander/calibration_routines.cpp @@ -33,7 +33,7 @@ ****************************************************************************/ /** - * @file calibration_routines.c + * @file calibration_routines.cpp * Calibration routines implementations. * * @author Lorenz Meier <lm@inf.ethz.ch> diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.cpp index c4dc495f6..253b6295f 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.cpp @@ -36,13 +36,11 @@ ****************************************************************************/ /** - * @file commander.c + * @file commander.cpp * Main system state machine implementation. * */ -#include "commander.h" - #include <nuttx/config.h> #include <pthread.h> #include <stdio.h> @@ -97,12 +95,11 @@ #include "rc_calibration.h" #include "airspeed_calibration.h" -PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0); /**< Go into low-level failsafe after 0 ms */ -//PARAM_DEFINE_INT32(SYS_FAILSAVE_HL, 0); /**< Go into high-level failsafe after 0 ms */ -PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); -PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); -PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); - +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; extern struct system_load_s system_load; @@ -160,8 +157,10 @@ enum { * * The actual stack size should be set in the call * to task_create(). + * + * @ingroup apps */ -__EXPORT int commander_main(int argc, char *argv[]); +extern "C" __EXPORT int commander_main(int argc, char *argv[]); /** * Print the correct usage. diff --git a/src/modules/commander/commander_helper.c b/src/modules/commander/commander_helper.cpp index 199f73e6c..9427bd892 100644 --- a/src/modules/commander/commander_helper.c +++ b/src/modules/commander/commander_helper.cpp @@ -34,7 +34,7 @@ ****************************************************************************/ /** - * @file commander_helper.c + * @file commander_helper.cpp * Commander helper functions implementations */ @@ -56,6 +56,12 @@ #include "commander_helper.h" +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + bool is_multirotor(const struct vehicle_status_s *current_status) { return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || @@ -175,11 +181,6 @@ int led_off(int led) return ioctl(leds, LED_OFF, led); } - -PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f); -PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f); -PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3); - float battery_remaining_estimate_voltage(float voltage) { float ret = 0; diff --git a/src/modules/commander/commander.h b/src/modules/commander/commander_params.c index 6e57c0ba5..6832fc5ce 100644 --- a/src/modules/commander/commander.h +++ b/src/modules/commander/commander_params.c @@ -1,10 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Petri Tanskanen <petri.tanskanen@inf.ethz.ch> - * Lorenz Meier <lm@inf.ethz.ch> - * Thomas Gubler <thomasgubler@student.ethz.ch> - * Julian Oes <joes@student.ethz.ch> + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier <lm@inf.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,19 +33,22 @@ ****************************************************************************/ /** - * @file commander.h - * Main system state machine definition. + * @file commander_params.c + * + * Parameters defined by the sensors task. * - * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch> * @author Lorenz Meier <lm@inf.ethz.ch> * @author Thomas Gubler <thomasgubler@student.ethz.ch> * @author Julian Oes <joes@student.ethz.ch> - * */ -#ifndef COMMANDER_H_ -#define COMMANDER_H_ - - +#include <nuttx/config.h> +#include <systemlib/param/param.h> -#endif /* COMMANDER_H_ */ +PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0); /**< Go into low-level failsafe after 0 ms */ +PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); +PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); +PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); +PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f); +PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f); +PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3); diff --git a/src/modules/commander/gyro_calibration.c b/src/modules/commander/gyro_calibration.cpp index 865f74ab4..9e6909db0 100644 --- a/src/modules/commander/gyro_calibration.c +++ b/src/modules/commander/gyro_calibration.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file gyro_calibration.c + * @file gyro_calibration.cpp * Gyroscope calibration routine */ @@ -82,7 +82,9 @@ void do_gyro_calibration(int mavlink_fd) while (calibration_counter < calibration_count) { /* wait blocking for new data */ - struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; + struct pollfd fds[1]; + fds[0].fd = sub_sensor_combined; + fds[0].events = POLLIN; int poll_ret = poll(fds, 1, 1000); @@ -105,6 +107,49 @@ void do_gyro_calibration(int mavlink_fd) gyro_offset[2] = gyro_offset[2] / calibration_count; + if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) { + + if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0])) + || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1])) + || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) { + mavlink_log_critical(mavlink_fd, "Setting gyro offsets failed!"); + } + + /* set offsets to actual value */ + fd = open(GYRO_DEVICE_PATH, 0); + struct gyro_scale gscale = { + gyro_offset[0], + 1.0f, + gyro_offset[1], + 1.0f, + gyro_offset[2], + 1.0f, + }; + + if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) + warn("WARNING: failed to set scale / offsets for gyro"); + + close(fd); + + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + warnx("WARNING: auto-save of params to storage failed"); + mavlink_log_critical(mavlink_fd, "gyro store failed"); + // XXX negative tune + return; + } + + mavlink_log_info(mavlink_fd, "gyro calibration done"); + + tune_positive(); + /* third beep by cal end routine */ + + } else { + mavlink_log_info(mavlink_fd, "offset cal FAILED (NaN)"); + return; + } /*** --- SCALING --- ***/ @@ -136,7 +181,9 @@ void do_gyro_calibration(int mavlink_fd) break; /* wait blocking for new data */ - struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; + struct pollfd fds[1]; + fds[0].fd = sub_sensor_combined; + fds[0].events = POLLIN; int poll_ret = poll(fds, 1, 1000); @@ -180,27 +227,28 @@ void do_gyro_calibration(int mavlink_fd) } float gyro_scale = baseline_integral / gyro_integral; + float gyro_scales[] = { gyro_scale, gyro_scale, gyro_scale }; warnx("gyro scale: yaw (z): %6.4f", gyro_scale); mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", gyro_scale); - if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) { + if (isfinite(gyro_scales[0]) && isfinite(gyro_scales[1]) && isfinite(gyro_scales[2])) { - if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0])) - || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1])) - || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) { - mavlink_log_critical(mavlink_fd, "Setting gyro offsets failed!"); + if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scales[0])) + || param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scales[1])) + || param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scales[2]))) { + mavlink_log_critical(mavlink_fd, "Setting gyro scale failed!"); } /* set offsets to actual value */ fd = open(GYRO_DEVICE_PATH, 0); struct gyro_scale gscale = { gyro_offset[0], - 1.0f, + gyro_scales[0], gyro_offset[1], - 1.0f, + gyro_scales[1], gyro_offset[2], - 1.0f, + gyro_scales[2], }; if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) diff --git a/src/modules/commander/mag_calibration.c b/src/modules/commander/mag_calibration.cpp index dbd31a7e7..9a25103f8 100644 --- a/src/modules/commander/mag_calibration.c +++ b/src/modules/commander/mag_calibration.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file mag_calibration.c + * @file mag_calibration.cpp * Magnetometer calibration routine */ @@ -120,7 +120,9 @@ void do_mag_calibration(int mavlink_fd) calibration_counter < calibration_maxcount) { /* wait blocking for new data */ - struct pollfd fds[1] = { { .fd = sub_mag, .events = POLLIN } }; + struct pollfd fds[1]; + fds[0].fd = sub_mag; + fds[0].events = POLLIN; /* user guidance */ if (hrt_absolute_time() >= axis_deadline && diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk index fef8e366b..91d75121e 100644 --- a/src/modules/commander/module.mk +++ b/src/modules/commander/module.mk @@ -36,14 +36,15 @@ # MODULE_COMMAND = commander -SRCS = commander.c \ - state_machine_helper.c \ - commander_helper.c \ - calibration_routines.c \ - accelerometer_calibration.c \ - gyro_calibration.c \ - mag_calibration.c \ - baro_calibration.c \ - rc_calibration.c \ - airspeed_calibration.c +SRCS = commander.cpp \ + commander_params.c \ + state_machine_helper.cpp \ + commander_helper.cpp \ + calibration_routines.cpp \ + accelerometer_calibration.cpp \ + gyro_calibration.cpp \ + mag_calibration.cpp \ + baro_calibration.cpp \ + rc_calibration.cpp \ + airspeed_calibration.cpp diff --git a/src/modules/commander/rc_calibration.c b/src/modules/commander/rc_calibration.cpp index a21d3f558..0de411713 100644 --- a/src/modules/commander/rc_calibration.c +++ b/src/modules/commander/rc_calibration.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file rc_calibration.c + * @file rc_calibration.cpp * Remote Control calibration routine */ diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.cpp index 792ead8f3..3cf60a99a 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.cpp @@ -34,7 +34,7 @@ ****************************************************************************/ /** - * @file state_machine_helper.c + * @file state_machine_helper.cpp * State machine helper functions implementations */ @@ -56,6 +56,11 @@ #include "state_machine_helper.h" #include "commander_helper.h" +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int armed_pub, struct actuator_armed_s *armed, const int mavlink_fd) { |