Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-07 17:35:22 +08:00
Code Issues Actions 32 Packages Projects Releases Wiki Activity

events: refactor temperature_calibration command to take options and use a single vehicle_command

Browse Source 
This makes it easier to start calibration for all sensors at once.
This commit is contained in:
Beat Küng
2017-02-02 15:42:09 +01:00
committed by Lorenz Meier
parent 603cd1e6dc
commit b6f3cf9425
3 changed files with 62 additions and 82 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d/rcS
+9 -3
View File
@@ -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
src/modules/commander/commander.cpp
+1 -1
View File
@@ -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;
src/modules/events/send_event.cpp
+52 -78
View File
@@ -34,6 +34,7 @@
#include "send_event.h"
#include "temperature_calibration/temperature_calibration.h"
#include <px4_getopt.h>
#include <px4_log.h>
#include <drivers/drv_hrt.h>
@@ -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);