diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index e001681af4..29394b97e9 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -1062,21 +1062,27 @@ then # Check if we should start a thermal calibration # TODO move further up and don't start unnecessary services if we are calibrating # + set TEMP_CALIB_ARGS "" if param compare SYS_CAL_GYRO 1 then - send_event start_temp_gyro_cal + set TEMP_CALIB_ARGS "${TEMP_CALIB_ARGS} -g" param set SYS_CAL_GYRO 0 fi if param compare SYS_CAL_ACCEL 1 then - send_event start_temp_accel_cal + set TEMP_CALIB_ARGS "${TEMP_CALIB_ARGS} -a" param set SYS_CAL_ACCEL 0 fi if param compare SYS_CAL_BARO 1 then - send_event start_temp_baro_cal + set TEMP_CALIB_ARGS "${TEMP_CALIB_ARGS} -b" param set SYS_CAL_BARO 0 fi + if [ "x$TEMP_CALIB_ARGS" != "x" ] + then + send_event temperature_calibration ${TEMP_CALIB_ARGS} + fi + unset TEMP_CALIB_ARGS # End of autostart fi diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 62480ccefd..b26cc22217 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -3978,7 +3978,7 @@ void *commander_low_prio_loop(void *arg) answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack); calib_ret = do_gyro_calibration(&mavlink_log_pub); - } else if ((int)(cmd.param1) == 2) { + } else if ((int)(cmd.param1) == 2 || (int)(cmd.param5) == 2 || (int)(cmd.param7) == 2) { /* temperature calibration: handled in events module */ break; diff --git a/src/modules/events/send_event.cpp b/src/modules/events/send_event.cpp index c5df13c3b3..397ddb2c2c 100644 --- a/src/modules/events/send_event.cpp +++ b/src/modules/events/send_event.cpp @@ -34,6 +34,7 @@ #include "send_event.h" #include "temperature_calibration/temperature_calibration.h" +#include #include #include @@ -119,36 +120,27 @@ void SendEvent::process_commands() orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd); + bool got_temperature_calibration_command = false; + switch (cmd.command) { case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION: - if ((int)(cmd.param1) == 2) { //TODO: this needs to be specified in mavlink (and adjust commander accordingly)... + if ((int)(cmd.param1) == 2) { //TODO: this (and the others) needs to be specified in mavlink... + run_temperature_gyro_calibration(); + got_temperature_calibration_command = true; + } - if (run_temperature_gyro_calibration() == 0) { - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); + if ((int)(cmd.param5) == 2) { + run_temperature_accel_calibration(); + got_temperature_calibration_command = true; + } - } else { - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED); - } + if ((int)(cmd.param7) == 2) { + run_temperature_baro_calibration(); + got_temperature_calibration_command = true; + } - } else if ((int)(cmd.param1) == - 3) { //TODO: this needs to be specified in mavlink (and adjust commander accordingly)... - - if (run_temperature_accel_calibration() == 0) { - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); - - } else { - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED); - } - - } else if ((int)(cmd.param1) == - 4) { //TODO: this needs to be specified in mavlink (and adjust commander accordingly)... - - if (run_temperature_baro_calibration() == 0) { - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); - - } else { - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED); - } + if (got_temperature_calibration_command) { + answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); } break; @@ -190,9 +182,8 @@ static void print_usage(const char *reason = nullptr) PX4_INFO("usage: send_event {start_listening|stop_listening|status|temperature_calibration}\n" "\tstart_listening: start background task to listen to events\n" - "\tstart_temp_gyro_cal: start gyro temperature calibration task\n" - "\tstart_temp_accel_cal: start accelerometer temperature calibration task\n" - "\tstart_temp_baro_cal: start barometer temperature calibration task\n" + "\ttemperature_calibration [-g] [-a] [-b]: start temperature calibration task\n" + "\t all sensors if no option given, 1 or several of gyro, accel, baro otherwise\n" ); } @@ -238,71 +229,54 @@ int send_event_main(int argc, char *argv[]) PX4_INFO("not running"); } - } else if (!strcmp(argv[1], "start_temp_gyro_cal")) { + } else if (!strcmp(argv[1], "temperature_calibration")) { if (!send_event_obj) { PX4_ERR("background task not running"); return -1; } + bool gyro_calib = false, accel_calib = false, baro_calib = false; + bool calib_all = true; + int myoptind = 1; + int ch; + const char *myoptarg = NULL; + + while ((ch = px4_getopt(argc, argv, "abg", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'a': + accel_calib = true; + calib_all = false; + break; + + case 'b': + baro_calib = true; + calib_all = false; + break; + + case 'g': + gyro_calib = true; + calib_all = false; + break; + + default: + print_usage("unrecognized flag"); + return 1; + } + } + vehicle_command_s cmd = {}; cmd.target_system = -1; cmd.target_component = -1; cmd.command = vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION; - cmd.param1 = 2; + cmd.param1 = (gyro_calib || calib_all) ? 2 : NAN; cmd.param2 = NAN; cmd.param3 = NAN; cmd.param4 = NAN; - cmd.param5 = NAN; + cmd.param5 = (accel_calib || calib_all) ? 2 : NAN; cmd.param6 = NAN; - cmd.param7 = NAN; - - orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH); - (void)orb_unadvertise(h); - - } else if (!strcmp(argv[1], "start_temp_accel_cal")) { - - if (!send_event_obj) { - PX4_ERR("background task not running"); - return -1; - } - - vehicle_command_s cmd = {}; - cmd.target_system = -1; - cmd.target_component = -1; - - cmd.command = vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION; - cmd.param1 = 3; - cmd.param2 = NAN; - cmd.param3 = NAN; - cmd.param4 = NAN; - cmd.param5 = NAN; - cmd.param6 = NAN; - cmd.param7 = NAN; - - orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH); - (void)orb_unadvertise(h); - - } else if (!strcmp(argv[1], "start_temp_baro_cal")) { - - if (!send_event_obj) { - PX4_ERR("background task not running"); - return -1; - } - - vehicle_command_s cmd = {}; - cmd.target_system = -1; - cmd.target_component = -1; - - cmd.command = vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION; - cmd.param1 = 4; - cmd.param2 = NAN; - cmd.param3 = NAN; - cmd.param4 = NAN; - cmd.param5 = NAN; - cmd.param6 = NAN; - cmd.param7 = NAN; + cmd.param7 = (baro_calib || calib_all) ? 2 : NAN; orb_advert_t h = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH); (void)orb_unadvertise(h);