From 68ab69de010adbe37b37558824ca013ecd0d569a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 20 Jul 2013 13:47:51 +0200 Subject: [PATCH] moved commander to C++, preparation for better gyro scale calibration respecting the current attitude for more accurate results --- ...ration.c => accelerometer_calibration.cpp} | 59 +++++++++------- ...calibration.c => airspeed_calibration.cpp} | 6 +- ...aro_calibration.c => baro_calibration.cpp} | 2 +- ...on_routines.c => calibration_routines.cpp} | 2 +- .../commander/{commander.c => commander.cpp} | 19 +++-- ...ommander_helper.c => commander_helper.cpp} | 13 ++-- .../{commander.h => commander_params.c} | 28 ++++---- ...yro_calibration.c => gyro_calibration.cpp} | 70 ++++++++++++++++--- ...{mag_calibration.c => mag_calibration.cpp} | 6 +- src/modules/commander/module.mk | 21 +++--- .../{rc_calibration.c => rc_calibration.cpp} | 2 +- ...hine_helper.c => state_machine_helper.cpp} | 7 +- src/modules/systemlib/systemlib.h | 7 +- 13 files changed, 156 insertions(+), 86 deletions(-) rename src/modules/commander/{accelerometer_calibration.c => accelerometer_calibration.cpp} (91%) rename src/modules/commander/{airspeed_calibration.c => airspeed_calibration.cpp} (97%) rename src/modules/commander/{baro_calibration.c => baro_calibration.cpp} (98%) rename src/modules/commander/{calibration_routines.c => calibration_routines.cpp} (99%) rename src/modules/commander/{commander.c => commander.cpp} (99%) rename src/modules/commander/{commander_helper.c => commander_helper.cpp} (97%) rename src/modules/commander/{commander.h => commander_params.c} (76%) rename src/modules/commander/{gyro_calibration.c => gyro_calibration.cpp} (83%) rename src/modules/commander/{mag_calibration.c => mag_calibration.cpp} (98%) rename src/modules/commander/{rc_calibration.c => rc_calibration.cpp} (99%) rename src/modules/commander/{state_machine_helper.c => state_machine_helper.cpp} (99%) diff --git a/src/modules/commander/accelerometer_calibration.c b/src/modules/commander/accelerometer_calibration.cpp similarity index 91% rename from src/modules/commander/accelerometer_calibration.c rename to src/modules/commander/accelerometer_calibration.cpp index bc9d24956f..df92d51d20 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 #include #include - - #include #include #include @@ -123,6 +121,12 @@ #include #include +/* 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 similarity index 97% rename from src/modules/commander/airspeed_calibration.c rename to src/modules/commander/airspeed_calibration.cpp index feaf11aee7..df08292e32 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 similarity index 98% rename from src/modules/commander/baro_calibration.c rename to src/modules/commander/baro_calibration.cpp index a705947948..d7515b3d9b 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 similarity index 99% rename from src/modules/commander/calibration_routines.c rename to src/modules/commander/calibration_routines.cpp index 799cd42697..be38ea1046 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 diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.cpp similarity index 99% rename from src/modules/commander/commander.c rename to src/modules/commander/commander.cpp index c4dc495f6a..253b6295f1 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 #include #include @@ -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 similarity index 97% rename from src/modules/commander/commander_helper.c rename to src/modules/commander/commander_helper.cpp index 199f73e6c8..9427bd8925 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 similarity index 76% rename from src/modules/commander/commander.h rename to src/modules/commander/commander_params.c index 6e57c0ba5f..6832fc5ce3 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 - * Lorenz Meier - * Thomas Gubler - * Julian Oes + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier * * 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 * @author Lorenz Meier * @author Thomas Gubler * @author Julian Oes - * */ -#ifndef COMMANDER_H_ -#define COMMANDER_H_ +#include +#include - - -#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 similarity index 83% rename from src/modules/commander/gyro_calibration.c rename to src/modules/commander/gyro_calibration.cpp index 865f74ab44..9e6909db07 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 similarity index 98% rename from src/modules/commander/mag_calibration.c rename to src/modules/commander/mag_calibration.cpp index dbd31a7e75..9a25103f8c 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 fef8e366b2..91d75121eb 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 similarity index 99% rename from src/modules/commander/rc_calibration.c rename to src/modules/commander/rc_calibration.cpp index a21d3f558b..0de411713b 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 similarity index 99% rename from src/modules/commander/state_machine_helper.c rename to src/modules/commander/state_machine_helper.cpp index 792ead8f3c..3cf60a99ac 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) { diff --git a/src/modules/systemlib/systemlib.h b/src/modules/systemlib/systemlib.h index 0194b5e529..356215b0e2 100644 --- a/src/modules/systemlib/systemlib.h +++ b/src/modules/systemlib/systemlib.h @@ -42,11 +42,12 @@ #include #include -/** Reboots the board */ -extern void up_systemreset(void) noreturn_function; - __BEGIN_DECLS +/** Reboots the board */ +//extern void up_systemreset(void) noreturn_function; +#include <../arch/common/up_internal.h> + /** Sends SIGUSR1 to all processes */ __EXPORT void killall(void);