fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-06 06:21:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander fix and enforce code style

Browse Source
This commit is contained in:
Daniel Agar
2018-11-28 20:42:03 -05:00
parent 91721f2060
commit 48d9484ceb
16 changed files with 855 additions and 481 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Tools/astyle/files_to_check_code_style.sh
-1
View File
@@ -13,7 +13,6 @@ exec find src platforms \
-path src/lib/DriverFramework -prune -o \ -path src/lib/DriverFramework -prune -o \
-path src/lib/ecl -prune -o \ -path src/lib/ecl -prune -o \
-path src/lib/matrix -prune -o \ -path src/lib/matrix -prune -o \
-path src/modules/commander -prune -o \
-path src/modules/micrortps_bridge/micro-CDR -prune -o \ -path src/modules/micrortps_bridge/micro-CDR -prune -o \
-path src/modules/systemlib/uthash -prune -o \ -path src/modules/systemlib/uthash -prune -o \
-path src/modules/uavcan/libuavcan -prune -o \ -path src/modules/uavcan/libuavcan -prune -o \
src/modules/commander/Commander.cpp
+130 -41
View File
@@ -109,8 +109,7 @@
#include <uORB/topics/vtol_vehicle_status.h> #include <uORB/topics/vtol_vehicle_status.h>
#include <uORB/uORB.h> #include <uORB/uORB.h>
typedef enum VEHICLE_MODE_FLAG typedef enum VEHICLE_MODE_FLAG {
{
VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */ VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */
VEHICLE_MODE_FLAG_TEST_ENABLED = 2, /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */ VEHICLE_MODE_FLAG_TEST_ENABLED = 2, /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */
VEHICLE_MODE_FLAG_AUTO_ENABLED = 4, /* 0b00000100 autonomous mode enabled, system finds its own goal positions. Guided flag can be set or not, depends on the actual implementation. | */ VEHICLE_MODE_FLAG_AUTO_ENABLED = 4, /* 0b00000100 autonomous mode enabled, system finds its own goal positions. Guided flag can be set or not, depends on the actual implementation. | */
@@ -181,7 +180,8 @@ static bool _last_condition_global_position_valid = false;
static struct vehicle_land_detected_s land_detector = {}; static struct vehicle_land_detected_s land_detector = {};
static float _eph_threshold_adj = INFINITY; ///< maximum allowable horizontal position uncertainty after adjustment for flight condition static float _eph_threshold_adj =
INFINITY; ///< maximum allowable horizontal position uncertainty after adjustment for flight condition
static bool _skip_pos_accuracy_check = false; static bool _skip_pos_accuracy_check = false;
/** /**
@@ -201,7 +201,8 @@ extern "C" __EXPORT int commander_main(int argc, char *argv[]);
*/ */
void usage(const char *reason); void usage(const char *reason);
void control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actuator_armed, bool changed, const uint8_t battery_warning, const cpuload_s *cpuload_local); void control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actuator_armed, bool changed,
const uint8_t battery_warning, const cpuload_s *cpuload_local);
void get_circuit_breaker_params(); void get_circuit_breaker_params();
@@ -236,15 +237,19 @@ static int power_button_state_notification_cb(board_power_button_state_notificat
case PWR_BUTTON_IDEL: case PWR_BUTTON_IDEL:
button_state.event = power_button_state_s::PWR_BUTTON_STATE_IDEL; button_state.event = power_button_state_s::PWR_BUTTON_STATE_IDEL;
break; break;
case PWR_BUTTON_DOWN: case PWR_BUTTON_DOWN:
button_state.event = power_button_state_s::PWR_BUTTON_STATE_DOWN; button_state.event = power_button_state_s::PWR_BUTTON_STATE_DOWN;
break; break;
case PWR_BUTTON_UP: case PWR_BUTTON_UP:
button_state.event = power_button_state_s::PWR_BUTTON_STATE_UP; button_state.event = power_button_state_s::PWR_BUTTON_STATE_UP;
break; break;
case PWR_BUTTON_REQUEST_SHUT_DOWN: case PWR_BUTTON_REQUEST_SHUT_DOWN:
button_state.event = power_button_state_s::PWR_BUTTON_STATE_REQUEST_SHUTDOWN; button_state.event = power_button_state_s::PWR_BUTTON_STATE_REQUEST_SHUTDOWN;
break; break;
default: default:
PX4_ERR("unhandled power button state: %i", (int)request); PX4_ERR("unhandled power button state: %i", (int)request);
return ret; return ret;
@@ -252,6 +257,7 @@ static int power_button_state_notification_cb(board_power_button_state_notificat
if (power_button_state_pub != nullptr) { if (power_button_state_pub != nullptr) {
orb_publish(ORB_ID(power_button_state), power_button_state_pub, &button_state); orb_publish(ORB_ID(power_button_state), power_button_state_pub, &button_state);
} else { } else {
PX4_ERR("power_button_state_pub not properly initialized"); PX4_ERR("power_button_state_pub not properly initialized");
} }
@@ -302,8 +308,10 @@ int commander_main(int argc, char *argv[])
unsigned constexpr max_wait_steps = 2000; unsigned constexpr max_wait_steps = 2000;
unsigned i; unsigned i;
for (i = 0; i < max_wait_steps; i++) { for (i = 0; i < max_wait_steps; i++) {
usleep(max_wait_us / max_wait_steps); usleep(max_wait_us / max_wait_steps);
if (thread_running) { if (thread_running) {
break; break;
} }
@@ -342,18 +350,25 @@ int commander_main(int argc, char *argv[])
if (!strcmp(argv[1], "calibrate")) { if (!strcmp(argv[1], "calibrate")) {
if (argc > 2) { if (argc > 2) {
int calib_ret = OK; int calib_ret = OK;
if (!strcmp(argv[2], "mag")) { if (!strcmp(argv[2], "mag")) {
calib_ret = do_mag_calibration(&mavlink_log_pub); calib_ret = do_mag_calibration(&mavlink_log_pub);
} else if (!strcmp(argv[2], "accel")) { } else if (!strcmp(argv[2], "accel")) {
calib_ret = do_accel_calibration(&mavlink_log_pub); calib_ret = do_accel_calibration(&mavlink_log_pub);
} else if (!strcmp(argv[2], "gyro")) { } else if (!strcmp(argv[2], "gyro")) {
calib_ret = do_gyro_calibration(&mavlink_log_pub); calib_ret = do_gyro_calibration(&mavlink_log_pub);
} else if (!strcmp(argv[2], "level")) { } else if (!strcmp(argv[2], "level")) {
calib_ret = do_level_calibration(&mavlink_log_pub); calib_ret = do_level_calibration(&mavlink_log_pub);
} else if (!strcmp(argv[2], "esc")) { } else if (!strcmp(argv[2], "esc")) {
calib_ret = do_esc_calibration(&mavlink_log_pub, &armed); calib_ret = do_esc_calibration(&mavlink_log_pub, &armed);
} else if (!strcmp(argv[2], "airspeed")) { } else if (!strcmp(argv[2], "airspeed")) {
calib_ret = do_airspeed_calibration(&mavlink_log_pub); calib_ret = do_airspeed_calibration(&mavlink_log_pub);
} else { } else {
PX4_ERR("argument %s unsupported.", argv[2]); PX4_ERR("argument %s unsupported.", argv[2]);
} }
@@ -361,9 +376,11 @@ int commander_main(int argc, char *argv[])
if (calib_ret) { if (calib_ret) {
PX4_ERR("calibration failed, exiting."); PX4_ERR("calibration failed, exiting.");
return 1; return 1;
} else { } else {
return 0; return 0;
} }
} else { } else {
PX4_ERR("missing argument"); PX4_ERR("missing argument");
} }
@@ -383,6 +400,7 @@ int commander_main(int argc, char *argv[])
if (TRANSITION_CHANGED != arm_disarm(true, &mavlink_log_pub, "command line")) { if (TRANSITION_CHANGED != arm_disarm(true, &mavlink_log_pub, "command line")) {
PX4_ERR("arming failed"); PX4_ERR("arming failed");
} }
return 0; return 0;
} }
@@ -390,6 +408,7 @@ int commander_main(int argc, char *argv[])
if (TRANSITION_DENIED == arm_disarm(false, &mavlink_log_pub, "command line")) { if (TRANSITION_DENIED == arm_disarm(false, &mavlink_log_pub, "command line")) {
PX4_ERR("rejected disarm"); PX4_ERR("rejected disarm");
} }
return 0; return 0;
} }
@@ -423,7 +442,8 @@ int commander_main(int argc, char *argv[])
if (!strcmp(argv[1], "transition")) { if (!strcmp(argv[1], "transition")) {
bool ret = send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION, bool ret = send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION,
(float)(status.is_rotary_wing ? vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW : vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC)); (float)(status.is_rotary_wing ? vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW :
vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC));
return (ret ? 0 : 1); return (ret ? 0 : 1);
} }
@@ -431,32 +451,46 @@ int commander_main(int argc, char *argv[])
if (!strcmp(argv[1], "mode")) { if (!strcmp(argv[1], "mode")) {
if (argc > 2) { if (argc > 2) {
uint8_t new_main_state = commander_state_s::MAIN_STATE_MAX; uint8_t new_main_state = commander_state_s::MAIN_STATE_MAX;
if (!strcmp(argv[2], "manual")) { if (!strcmp(argv[2], "manual")) {
new_main_state = commander_state_s::MAIN_STATE_MANUAL; new_main_state = commander_state_s::MAIN_STATE_MANUAL;
} else if (!strcmp(argv[2], "altctl")) { } else if (!strcmp(argv[2], "altctl")) {
new_main_state = commander_state_s::MAIN_STATE_ALTCTL; new_main_state = commander_state_s::MAIN_STATE_ALTCTL;
} else if (!strcmp(argv[2], "posctl")) { } else if (!strcmp(argv[2], "posctl")) {
new_main_state = commander_state_s::MAIN_STATE_POSCTL; new_main_state = commander_state_s::MAIN_STATE_POSCTL;
} else if (!strcmp(argv[2], "auto:mission")) { } else if (!strcmp(argv[2], "auto:mission")) {
new_main_state = commander_state_s::MAIN_STATE_AUTO_MISSION; new_main_state = commander_state_s::MAIN_STATE_AUTO_MISSION;
} else if (!strcmp(argv[2], "auto:loiter")) { } else if (!strcmp(argv[2], "auto:loiter")) {
new_main_state = commander_state_s::MAIN_STATE_AUTO_LOITER; new_main_state = commander_state_s::MAIN_STATE_AUTO_LOITER;
} else if (!strcmp(argv[2], "auto:rtl")) { } else if (!strcmp(argv[2], "auto:rtl")) {
new_main_state = commander_state_s::MAIN_STATE_AUTO_RTL; new_main_state = commander_state_s::MAIN_STATE_AUTO_RTL;
} else if (!strcmp(argv[2], "acro")) { } else if (!strcmp(argv[2], "acro")) {
new_main_state = commander_state_s::MAIN_STATE_ACRO; new_main_state = commander_state_s::MAIN_STATE_ACRO;
} else if (!strcmp(argv[2], "offboard")) { } else if (!strcmp(argv[2], "offboard")) {
new_main_state = commander_state_s::MAIN_STATE_OFFBOARD; new_main_state = commander_state_s::MAIN_STATE_OFFBOARD;
} else if (!strcmp(argv[2], "stabilized")) { } else if (!strcmp(argv[2], "stabilized")) {
new_main_state = commander_state_s::MAIN_STATE_STAB; new_main_state = commander_state_s::MAIN_STATE_STAB;
} else if (!strcmp(argv[2], "rattitude")) { } else if (!strcmp(argv[2], "rattitude")) {
new_main_state = commander_state_s::MAIN_STATE_RATTITUDE; new_main_state = commander_state_s::MAIN_STATE_RATTITUDE;
} else if (!strcmp(argv[2], "auto:takeoff")) { } else if (!strcmp(argv[2], "auto:takeoff")) {
new_main_state = commander_state_s::MAIN_STATE_AUTO_TAKEOFF; new_main_state = commander_state_s::MAIN_STATE_AUTO_TAKEOFF;
} else if (!strcmp(argv[2], "auto:land")) { } else if (!strcmp(argv[2], "auto:land")) {
new_main_state = commander_state_s::MAIN_STATE_AUTO_LAND; new_main_state = commander_state_s::MAIN_STATE_AUTO_LAND;
} else if (!strcmp(argv[2], "auto:precland")) { } else if (!strcmp(argv[2], "auto:precland")) {
new_main_state = commander_state_s::MAIN_STATE_AUTO_PRECLAND; new_main_state = commander_state_s::MAIN_STATE_AUTO_PRECLAND;
} else { } else {
PX4_ERR("argument %s unsupported.", argv[2]); PX4_ERR("argument %s unsupported.", argv[2]);
} }
@@ -464,6 +498,7 @@ int commander_main(int argc, char *argv[])
if (TRANSITION_DENIED == main_state_transition(status, new_main_state, status_flags, &internal_state)) { if (TRANSITION_DENIED == main_state_transition(status, new_main_state, status_flags, &internal_state)) {
PX4_ERR("mode change failed"); PX4_ERR("mode change failed");
} }
return 0; return 0;
} else { } else {
@@ -564,7 +599,8 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
// Check if a mode switch had been requested // Check if a mode switch had been requested
if ((((uint32_t)cmd.param2) & 1) > 0) { if ((((uint32_t)cmd.param2) & 1) > 0) {
transition_result_t main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_LOITER, status_flags, &internal_state); transition_result_t main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_LOITER,
status_flags, &internal_state);
if ((main_ret != TRANSITION_DENIED)) { if ((main_ret != TRANSITION_DENIED)) {
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
@@ -573,11 +609,13 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
mavlink_log_critical(&mavlink_log_pub, "Rejecting reposition command"); mavlink_log_critical(&mavlink_log_pub, "Rejecting reposition command");
} }
} else { } else {
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
} }
} }
break; break;
case vehicle_command_s::VEHICLE_CMD_DO_SET_MODE: { case vehicle_command_s::VEHICLE_CMD_DO_SET_MODE: {
uint8_t base_mode = (uint8_t)cmd.param1; uint8_t base_mode = (uint8_t)cmd.param1;
uint8_t custom_main_mode = (uint8_t)cmd.param2; uint8_t custom_main_mode = (uint8_t)cmd.param2;
@@ -610,14 +648,18 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
/* AUTO */ /* AUTO */
if (custom_sub_mode > 0) { if (custom_sub_mode > 0) {
reset_posvel_validity(changed); reset_posvel_validity(changed);
switch (custom_sub_mode) { switch (custom_sub_mode) {
case PX4_CUSTOM_SUB_MODE_AUTO_LOITER: case PX4_CUSTOM_SUB_MODE_AUTO_LOITER:
main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_LOITER, status_flags, &internal_state); main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_LOITER, status_flags,
&internal_state);
break; break;
case PX4_CUSTOM_SUB_MODE_AUTO_MISSION: case PX4_CUSTOM_SUB_MODE_AUTO_MISSION:
if (status_flags.condition_auto_mission_available) { if (status_flags.condition_auto_mission_available) {
main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_MISSION, status_flags, &internal_state); main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_MISSION, status_flags,
&internal_state);
} else { } else {
main_ret = TRANSITION_DENIED; main_ret = TRANSITION_DENIED;
} }
@@ -629,7 +671,8 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
break; break;
case PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF: case PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF:
main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_TAKEOFF, status_flags, &internal_state); main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_TAKEOFF, status_flags,
&internal_state);
break; break;
case PX4_CUSTOM_SUB_MODE_AUTO_LAND: case PX4_CUSTOM_SUB_MODE_AUTO_LAND:
@@ -637,11 +680,13 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
break; break;
case PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET: case PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET:
main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET, status_flags, &internal_state); main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET, status_flags,
&internal_state);
break; break;
case PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND: case PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND:
main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_PRECLAND, status_flags, &internal_state); main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_PRECLAND, status_flags,
&internal_state);
break; break;
default: default:
@@ -651,7 +696,8 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
} }
} else { } else {
main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_MISSION, status_flags, &internal_state); main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_MISSION, status_flags,
&internal_state);
} }
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) { } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) {
@@ -676,7 +722,8 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
/* use base mode */ /* use base mode */
if (base_mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) { if (base_mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) {
/* AUTO */ /* AUTO */
main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_MISSION, status_flags, &internal_state); main_ret = main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_MISSION, status_flags,
&internal_state);
} else if (base_mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) { } else if (base_mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) {
if (base_mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) { if (base_mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) {
@@ -724,7 +771,8 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
} else { } else {
// Refuse to arm if preflight checks have failed // Refuse to arm if preflight checks have failed
if ((!status_local->hil_state) != vehicle_status_s::HIL_STATE_ON && !status_flags.condition_system_sensors_initialized) { if ((!status_local->hil_state) != vehicle_status_s::HIL_STATE_ON
&& !status_flags.condition_system_sensors_initialized) {
mavlink_log_critical(&mavlink_log_pub, "Arming DENIED. Preflight checks have failed."); mavlink_log_critical(&mavlink_log_pub, "Arming DENIED. Preflight checks have failed.");
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED; cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
break; break;
@@ -881,7 +929,8 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
case vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH: { case vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH: {
/* switch to RTL which ends the mission */ /* switch to RTL which ends the mission */
if (TRANSITION_CHANGED == main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags, &internal_state)) { if (TRANSITION_CHANGED == main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags,
&internal_state)) {
mavlink_and_console_log_info(&mavlink_log_pub, "Returning to launch"); mavlink_and_console_log_info(&mavlink_log_pub, "Returning to launch");
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
@@ -894,7 +943,8 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
case vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF: { case vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF: {
/* ok, home set, use it to take off */ /* ok, home set, use it to take off */
if (TRANSITION_CHANGED == main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_TAKEOFF, status_flags, &internal_state)) { if (TRANSITION_CHANGED == main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_TAKEOFF, status_flags,
&internal_state)) {
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else if (internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF) { } else if (internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF) {
@@ -908,7 +958,8 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
break; break;
case vehicle_command_s::VEHICLE_CMD_NAV_LAND: { case vehicle_command_s::VEHICLE_CMD_NAV_LAND: {
if (TRANSITION_CHANGED == main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_LAND, status_flags, &internal_state)) { if (TRANSITION_CHANGED == main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_LAND, status_flags,
&internal_state)) {
mavlink_and_console_log_info(&mavlink_log_pub, "Landing at current position"); mavlink_and_console_log_info(&mavlink_log_pub, "Landing at current position");
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
@@ -920,7 +971,8 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
break; break;
case vehicle_command_s::VEHICLE_CMD_NAV_PRECLAND: { case vehicle_command_s::VEHICLE_CMD_NAV_PRECLAND: {
if (TRANSITION_CHANGED == main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_PRECLAND, status_flags, &internal_state)) { if (TRANSITION_CHANGED == main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_PRECLAND,
status_flags, &internal_state)) {
mavlink_and_console_log_info(&mavlink_log_pub, "Precision landing"); mavlink_and_console_log_info(&mavlink_log_pub, "Precision landing");
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
@@ -942,22 +994,27 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
if (PX4_ISFINITE(cmd.param1) && (cmd.param1 >= -1) && (cmd.param1 < _mission_result_sub.get().seq_total)) { if (PX4_ISFINITE(cmd.param1) && (cmd.param1 >= -1) && (cmd.param1 < _mission_result_sub.get().seq_total)) {
// switch to AUTO_MISSION and ARM // switch to AUTO_MISSION and ARM
if ((TRANSITION_DENIED != main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_MISSION, status_flags, &internal_state)) if ((TRANSITION_DENIED != main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_MISSION, status_flags,
&internal_state))
&& (TRANSITION_DENIED != arm_disarm(true, &mavlink_log_pub, "mission start command"))) { && (TRANSITION_DENIED != arm_disarm(true, &mavlink_log_pub, "mission start command"))) {
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else { } else {
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
mavlink_log_critical(&mavlink_log_pub, "Mission start denied"); mavlink_log_critical(&mavlink_log_pub, "Mission start denied");
} }
} }
} else { } else {
mavlink_log_critical(&mavlink_log_pub, "Mission start denied, no valid mission"); mavlink_log_critical(&mavlink_log_pub, "Mission start denied, no valid mission");
} }
} }
break; break;
case vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY: { case vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY: {
bool hl_exists = false; bool hl_exists = false;
for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
if (_telemetry[i].high_latency) { if (_telemetry[i].high_latency) {
hl_exists = true; hl_exists = true;
@@ -1670,6 +1727,7 @@ Commander::run()
if (!have_taken_off_since_arming) { if (!have_taken_off_since_arming) {
// pilot has ten seconds time to take off // pilot has ten seconds time to take off
auto_disarm_hysteresis.set_hysteresis_time_from(false, 10_s); auto_disarm_hysteresis.set_hysteresis_time_from(false, 10_s);
} else { } else {
auto_disarm_hysteresis.set_hysteresis_time_from(false, _disarm_when_landed_timeout.get() * 1_s); auto_disarm_hysteresis.set_hysteresis_time_from(false, _disarm_when_landed_timeout.get() * 1_s);
} }
@@ -1709,6 +1767,7 @@ Commander::run()
/* update subsystem info which arrives from outside of commander*/ /* update subsystem info which arrives from outside of commander*/
do { do {
orb_check(subsys_sub, &updated); orb_check(subsys_sub, &updated);
if (updated) { if (updated) {
orb_copy(ORB_ID(subsystem_info), subsys_sub, &info); orb_copy(ORB_ID(subsystem_info), subsys_sub, &info);
set_health_flags(info.subsystem_type, info.present, info.enabled, info.ok, status); set_health_flags(info.subsystem_type, info.present, info.enabled, info.ok, status);
@@ -1736,7 +1795,8 @@ Commander::run()
const mission_result_s &mission_result = _mission_result_sub.get(); const mission_result_s &mission_result = _mission_result_sub.get();
// if mission_result is valid for the current mission // if mission_result is valid for the current mission
const bool mission_result_ok = (mission_result.timestamp > commander_boot_timestamp) && (mission_result.instance_count > 0); const bool mission_result_ok = (mission_result.timestamp > commander_boot_timestamp)
&& (mission_result.instance_count > 0);
status_flags.condition_auto_mission_available = mission_result_ok && mission_result.valid; status_flags.condition_auto_mission_available = mission_result_ok && mission_result.valid;
@@ -1798,24 +1858,30 @@ Commander::run()
// do nothing // do nothing
break; break;
} }
case (geofence_result_s::GF_ACTION_WARN) : { case (geofence_result_s::GF_ACTION_WARN) : {
// do nothing, mavlink critical messages are sent by navigator // do nothing, mavlink critical messages are sent by navigator
break; break;
} }
case (geofence_result_s::GF_ACTION_LOITER) : { case (geofence_result_s::GF_ACTION_LOITER) : {
if (TRANSITION_CHANGED == main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LOITER, status_flags, &internal_state)) { if (TRANSITION_CHANGED == main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LOITER, status_flags,
&internal_state)) {
geofence_loiter_on = true; geofence_loiter_on = true;
} }
break; break;
} }
case (geofence_result_s::GF_ACTION_RTL) : { case (geofence_result_s::GF_ACTION_RTL) : {
if (TRANSITION_CHANGED == main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags, &internal_state)) { if (TRANSITION_CHANGED == main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags,
&internal_state)) {
geofence_rtl_on = true; geofence_rtl_on = true;
} }
break; break;
} }
case (geofence_result_s::GF_ACTION_TERMINATE) : { case (geofence_result_s::GF_ACTION_TERMINATE) : {
warnx("Flight termination because of geofence"); warnx("Flight termination because of geofence");
mavlink_log_critical(&mavlink_log_pub, "Geofence violation: flight termination"); mavlink_log_critical(&mavlink_log_pub, "Geofence violation: flight termination");
@@ -1911,13 +1977,16 @@ Commander::run()
/* handle the case where RC signal was regained */ /* handle the case where RC signal was regained */
if (!status_flags.rc_signal_found_once) { if (!status_flags.rc_signal_found_once) {
status_flags.rc_signal_found_once = true; status_flags.rc_signal_found_once = true;
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, true && status_flags.rc_calibration_valid, status); set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, true
&& status_flags.rc_calibration_valid, status);
status_changed = true; status_changed = true;
} else { } else {
if (status.rc_signal_lost) { if (status.rc_signal_lost) {
mavlink_log_info(&mavlink_log_pub, "MANUAL CONTROL REGAINED after %llums", hrt_elapsed_time(&rc_signal_lost_timestamp) / 1000); mavlink_log_info(&mavlink_log_pub, "MANUAL CONTROL REGAINED after %llums",
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, true && status_flags.rc_calibration_valid, status); hrt_elapsed_time(&rc_signal_lost_timestamp) / 1000);
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, true
&& status_flags.rc_calibration_valid, status);
status_changed = true; status_changed = true;
} }
} }
@@ -2080,7 +2149,8 @@ Commander::run()
} }
// data link checks which update the status // data link checks which update the status
data_link_checks(highlatencydatalink_loss_timeout, highlatencydatalink_regain_timeout, datalink_loss_timeout, datalink_regain_timeout, &status_changed); data_link_checks(highlatencydatalink_loss_timeout, highlatencydatalink_regain_timeout, datalink_loss_timeout,
datalink_regain_timeout, &status_changed);
// engine failure detection // engine failure detection
// TODO: move out of commander // TODO: move out of commander
@@ -2496,14 +2566,17 @@ get_circuit_breaker_params()
status_flags.circuit_breaker_engaged_power_check = circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY); status_flags.circuit_breaker_engaged_power_check = circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY);
status_flags.circuit_breaker_engaged_usb_check = circuit_breaker_enabled("CBRK_USB_CHK", CBRK_USB_CHK_KEY); status_flags.circuit_breaker_engaged_usb_check = circuit_breaker_enabled("CBRK_USB_CHK", CBRK_USB_CHK_KEY);
status_flags.circuit_breaker_engaged_airspd_check = circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY); status_flags.circuit_breaker_engaged_airspd_check = circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY);
status_flags.circuit_breaker_engaged_enginefailure_check = circuit_breaker_enabled("CBRK_ENGINEFAIL", CBRK_ENGINEFAIL_KEY); status_flags.circuit_breaker_engaged_enginefailure_check = circuit_breaker_enabled("CBRK_ENGINEFAIL",
CBRK_ENGINEFAIL_KEY);
status_flags.circuit_breaker_engaged_gpsfailure_check = circuit_breaker_enabled("CBRK_GPSFAIL", CBRK_GPSFAIL_KEY); status_flags.circuit_breaker_engaged_gpsfailure_check = circuit_breaker_enabled("CBRK_GPSFAIL", CBRK_GPSFAIL_KEY);
status_flags.circuit_breaker_flight_termination_disabled = circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY); status_flags.circuit_breaker_flight_termination_disabled = circuit_breaker_enabled("CBRK_FLIGHTTERM",
CBRK_FLIGHTTERM_KEY);
status_flags.circuit_breaker_engaged_posfailure_check = circuit_breaker_enabled("CBRK_VELPOSERR", CBRK_VELPOSERR_KEY); status_flags.circuit_breaker_engaged_posfailure_check = circuit_breaker_enabled("CBRK_VELPOSERR", CBRK_VELPOSERR_KEY);
} }
void void
Commander::check_valid(const hrt_abstime &timestamp, const hrt_abstime &timeout, const bool valid_in, bool *valid_out, bool *changed) Commander::check_valid(const hrt_abstime &timestamp, const hrt_abstime &timeout, const bool valid_in, bool *valid_out,
bool *changed)
{ {
hrt_abstime t = hrt_absolute_time(); hrt_abstime t = hrt_absolute_time();
bool valid_new = (t < timestamp + timeout && t > timeout && valid_in); bool valid_new = (t < timestamp + timeout && t > timeout && valid_in);
@@ -2659,7 +2732,8 @@ Commander::set_main_state(const vehicle_status_s &status_local, bool *changed)
transition_result_t transition_result_t
Commander::set_main_state_override_on(const vehicle_status_s &status_local, bool *changed) Commander::set_main_state_override_on(const vehicle_status_s &status_local, bool *changed)
{ {
transition_result_t res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, status_flags, &internal_state); transition_result_t res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, status_flags,
&internal_state);
*changed = (res == TRANSITION_CHANGED); *changed = (res == TRANSITION_CHANGED);
return res; return res;
@@ -3056,10 +3130,14 @@ Commander::reset_posvel_validity(bool *changed)
// recheck validity // recheck validity
if (!_skip_pos_accuracy_check) { if (!_skip_pos_accuracy_check) {
check_posvel_validity(true, global_position.eph, _eph_threshold_adj, global_position.timestamp, &_last_gpos_fail_time_us, &_gpos_probation_time_us, &status_flags.condition_global_position_valid, changed); check_posvel_validity(true, global_position.eph, _eph_threshold_adj, global_position.timestamp,
&_last_gpos_fail_time_us, &_gpos_probation_time_us, &status_flags.condition_global_position_valid, changed);
} }
check_posvel_validity(local_position.xy_valid, local_position.eph, _eph_threshold_adj, local_position.timestamp, &_last_lpos_fail_time_us, &_lpos_probation_time_us, &status_flags.condition_local_position_valid, changed);
check_posvel_validity(local_position.v_xy_valid, local_position.evh, _evh_threshold.get(), local_position.timestamp, &_last_lvel_fail_time_us, &_lvel_probation_time_us, &status_flags.condition_local_velocity_valid, changed); check_posvel_validity(local_position.xy_valid, local_position.eph, _eph_threshold_adj, local_position.timestamp,
&_last_lpos_fail_time_us, &_lpos_probation_time_us, &status_flags.condition_local_position_valid, changed);
check_posvel_validity(local_position.v_xy_valid, local_position.evh, _evh_threshold.get(), local_position.timestamp,
&_last_lvel_fail_time_us, &_lvel_probation_time_us, &status_flags.condition_local_velocity_valid, changed);
} }
bool bool
@@ -3075,7 +3153,8 @@ Commander::check_posvel_validity(const bool data_valid, const float data_accurac
*probation_time_us = POSVEL_PROBATION_MIN; *probation_time_us = POSVEL_PROBATION_MIN;
} }
const bool data_stale = ((hrt_elapsed_time(&data_timestamp_us) > _failsafe_pos_delay.get() * 1_s) || (data_timestamp_us == 0)); const bool data_stale = ((hrt_elapsed_time(&data_timestamp_us) > _failsafe_pos_delay.get() * 1_s)
|| (data_timestamp_us == 0));
const float req_accuracy = (was_valid ? required_accuracy * 2.5f : required_accuracy); const float req_accuracy = (was_valid ? required_accuracy * 2.5f : required_accuracy);
const bool level_check_pass = data_valid && !data_stale && (data_accuracy < req_accuracy); const bool level_check_pass = data_valid && !data_stale && (data_accuracy < req_accuracy);
@@ -3401,7 +3480,8 @@ void answer_command(const vehicle_command_s &cmd, unsigned result, orb_advert_t
orb_publish(ORB_ID(vehicle_command_ack), command_ack_pub, &command_ack); orb_publish(ORB_ID(vehicle_command_ack), command_ack_pub, &command_ack);
} else { } else {
command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack, vehicle_command_ack_s::ORB_QUEUE_LENGTH); command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
vehicle_command_ack_s::ORB_QUEUE_LENGTH);
} }
} }
@@ -3800,6 +3880,7 @@ void Commander::poll_telemetry_status()
mavlink_log_critical(&mavlink_log_pub, "Planned mission fails check. Please upload again."); mavlink_log_critical(&mavlink_log_pub, "Planned mission fails check. Please upload again.");
//set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MISSION, true, true, false, status); // TODO //set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MISSION, true, true, false, status); // TODO
} else { } else {
//set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MISSION, true, true, true, status); // TODO //set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MISSION, true, true, true, status); // TODO
} }
@@ -3970,6 +4051,7 @@ void Commander::battery_status_check()
if (updated) { if (updated) {
battery_status_s battery = {}; battery_status_s battery = {};
if (orb_copy(ORB_ID(battery_status), _battery_sub, &battery) == PX4_OK) { if (orb_copy(ORB_ID(battery_status), _battery_sub, &battery) == PX4_OK) {
if ((hrt_elapsed_time(&battery.timestamp) < 5_s) if ((hrt_elapsed_time(&battery.timestamp) < 5_s)
@@ -3985,7 +4067,8 @@ void Commander::battery_status_check()
// execute battery failsafe if the state has gotten worse // execute battery failsafe if the state has gotten worse
if (armed.armed) { if (armed.armed) {
if (battery.warning > _battery_warning) { if (battery.warning > _battery_warning) {
battery_failsafe(&mavlink_log_pub, status, status_flags, &internal_state, battery.warning, (low_battery_action_t)_low_bat_action.get()); battery_failsafe(&mavlink_log_pub, status, status_flags, &internal_state, battery.warning,
(low_battery_action_t)_low_bat_action.get());
} }
} }
@@ -4032,6 +4115,7 @@ void Commander::estimator_check(bool *status_changed)
// Check for a magnetomer fault and notify the user // Check for a magnetomer fault and notify the user
const bool mag_fault = (estimator_status.control_mode_flags & (1 << estimator_status_s::CS_MAG_FAULT)); const bool mag_fault = (estimator_status.control_mode_flags & (1 << estimator_status_s::CS_MAG_FAULT));
if (!mag_fault_prev && mag_fault) { if (!mag_fault_prev && mag_fault) {
mavlink_log_critical(&mavlink_log_pub, "Stopping compass use, check calibration on landing"); mavlink_log_critical(&mavlink_log_pub, "Stopping compass use, check calibration on landing");
} }
@@ -4110,20 +4194,25 @@ void Commander::estimator_check(bool *status_changed)
} else { } else {
if (!_skip_pos_accuracy_check) { if (!_skip_pos_accuracy_check) {
// use global position message to determine validity // use global position message to determine validity
check_posvel_validity(true, gpos.eph, _eph_threshold_adj, gpos.timestamp, &_last_gpos_fail_time_us, &_gpos_probation_time_us, &status_flags.condition_global_position_valid, status_changed); check_posvel_validity(true, gpos.eph, _eph_threshold_adj, gpos.timestamp, &_last_gpos_fail_time_us,
&_gpos_probation_time_us, &status_flags.condition_global_position_valid, status_changed);
} }
// use local position message to determine validity // use local position message to determine validity
check_posvel_validity(lpos.xy_valid, lpos.eph, _eph_threshold_adj, lpos.timestamp, &_last_lpos_fail_time_us, &_lpos_probation_time_us, &status_flags.condition_local_position_valid, status_changed); check_posvel_validity(lpos.xy_valid, lpos.eph, _eph_threshold_adj, lpos.timestamp, &_last_lpos_fail_time_us,
check_posvel_validity(lpos.v_xy_valid, lpos.evh, _evh_threshold.get(), lpos.timestamp, &_last_lvel_fail_time_us, &_lvel_probation_time_us, &status_flags.condition_local_velocity_valid, status_changed); &_lpos_probation_time_us, &status_flags.condition_local_position_valid, status_changed);
check_posvel_validity(lpos.v_xy_valid, lpos.evh, _evh_threshold.get(), lpos.timestamp, &_last_lvel_fail_time_us,
&_lvel_probation_time_us, &status_flags.condition_local_velocity_valid, status_changed);
} }
} }
if ((_last_condition_global_position_valid != status_flags.condition_global_position_valid) && status_flags.condition_global_position_valid) { if ((_last_condition_global_position_valid != status_flags.condition_global_position_valid)
&& status_flags.condition_global_position_valid) {
// If global position state changed and is now valid, set respective health flags to true. For now also assume GPS is OK if global pos is OK, but not vice versa. // If global position state changed and is now valid, set respective health flags to true. For now also assume GPS is OK if global pos is OK, but not vice versa.
set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, true, status); set_health_flags_healthy(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, true, status);
set_health_flags_present_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GPS, true, true, status); set_health_flags_present_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GPS, true, true, status);
} }
check_valid(lpos.timestamp, _failsafe_pos_delay.get() * 1_s, lpos.z_valid, &(status_flags.condition_local_altitude_valid), status_changed); check_valid(lpos.timestamp, _failsafe_pos_delay.get() * 1_s, lpos.z_valid,
&(status_flags.condition_local_altitude_valid), status_changed);
} }
src/modules/commander/PreflightCheck.cpp
+21 -3
View File
@@ -474,7 +474,8 @@ static bool powerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
success = false; success = false;
if (report_fail) { if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Avionics Power low: %6.2f Volt", (double)avionics_power_rail_voltage); mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Avionics Power low: %6.2f Volt",
(double)avionics_power_rail_voltage);
} }
} else if (avionics_power_rail_voltage < 4.9f) { } else if (avionics_power_rail_voltage < 4.9f) {
@@ -610,44 +611,59 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_s
if (report_fail) { if (report_fail) {
// Only report the first failure to avoid spamming // Only report the first failure to avoid spamming
const char *message = nullptr; const char *message = nullptr;
if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_GPS_FIX)) { if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_GPS_FIX)) {
message = "Preflight%s: GPS fix too low"; message = "Preflight%s: GPS fix too low";
} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MIN_SAT_COUNT)) { } else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MIN_SAT_COUNT)) {
message = "Preflight%s: not enough GPS Satellites"; message = "Preflight%s: not enough GPS Satellites";
} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MIN_GDOP)) { } else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MIN_GDOP)) {
message = "Preflight%s: GPS GDoP too low"; message = "Preflight%s: GPS GDoP too low";
} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_ERR)) { } else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_ERR)) {
message = "Preflight%s: GPS Horizontal Pos Error too high"; message = "Preflight%s: GPS Horizontal Pos Error too high";
} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_ERR)) { } else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_ERR)) {
message = "Preflight%s: GPS Vertical Pos Error too high"; message = "Preflight%s: GPS Vertical Pos Error too high";
} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_SPD_ERR)) { } else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_SPD_ERR)) {
message = "Preflight%s: GPS Speed Accuracy too low"; message = "Preflight%s: GPS Speed Accuracy too low";
} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_DRIFT)) { } else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_DRIFT)) {
message = "Preflight%s: GPS Horizontal Pos Drift too high"; message = "Preflight%s: GPS Horizontal Pos Drift too high";
} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_DRIFT)) { } else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_DRIFT)) {
message = "Preflight%s: GPS Vertical Pos Drift too high"; message = "Preflight%s: GPS Vertical Pos Drift too high";
} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_SPD_ERR)) { } else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_SPD_ERR)) {
message = "Preflight%s: GPS Hor Speed Drift too high"; message = "Preflight%s: GPS Hor Speed Drift too high";
} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_SPD_ERR)) { } else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_SPD_ERR)) {
message = "Preflight%s: GPS Vert Speed Drift too high"; message = "Preflight%s: GPS Vert Speed Drift too high";
} else { } else {
if (!ekf_gps_fusion) { if (!ekf_gps_fusion) {
// Likely cause unknown // Likely cause unknown
message = "Preflight%s: Estimator not using GPS"; message = "Preflight%s: Estimator not using GPS";
gps_present = false; gps_present = false;
} else { } else {
// if we land here there was a new flag added and the code not updated. Show a generic message. // if we land here there was a new flag added and the code not updated. Show a generic message.
message = "Preflight%s: Poor GPS Quality"; message = "Preflight%s: Poor GPS Quality";
} }
} }
if (message) { if (message) {
if (enforce_gps_required) { if (enforce_gps_required) {
mavlink_log_critical(mavlink_log_pub, message, " Fail"); mavlink_log_critical(mavlink_log_pub, message, " Fail");
} else { } else {
mavlink_log_warning(mavlink_log_pub, message, ""); mavlink_log_warning(mavlink_log_pub, message, "");
} }
} }
} }
gps_success = false; gps_success = false;
if (enforce_gps_required) { if (enforce_gps_required) {
@@ -690,7 +706,8 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
checkAirspeed = true; checkAirspeed = true;
} }
reportFailures = (reportFailures && status_flags.condition_system_hotplug_timeout && !status_flags.condition_calibration_enabled); reportFailures = (reportFailures && status_flags.condition_system_hotplug_timeout
&& !status_flags.condition_calibration_enabled);
bool failed = false; bool failed = false;
@@ -902,7 +919,8 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
} else { } else {
// The calibration is fine, but only set the overall health state to true if the signal is not currently lost // The calibration is fine, but only set the overall health state to true if the signal is not currently lost
status_flags.rc_calibration_valid = true; status_flags.rc_calibration_valid = true;
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, status_flags.rc_signal_found_once, true, !status.rc_signal_lost, status); set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, status_flags.rc_signal_found_once, true,
!status.rc_signal_lost, status);
} }
} }
src/modules/commander/accelerometer_calibration.cpp
+75 -13
View File
@@ -155,10 +155,14 @@ static int32_t device_id[max_accel_sens];
static int device_prio_max = 0; static int device_prio_max = 0;
static int32_t device_id_primary = 0; static int32_t device_id_primary = 0;
calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors); calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub,
calibrate_return read_accelerometer_avg(int sensor_correction_sub, int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num); float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors);
calibrate_return read_accelerometer_avg(int sensor_correction_sub, int (&subs)[max_accel_sens],
float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num);
int mat_invert3(float src[3][3], float dst[3][3]); int mat_invert3(float src[3][3], float dst[3][3]);
calibrate_return calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_sens][detect_orientation_side_count][3], float (&accel_T)[max_accel_sens][3][3], float (&accel_offs)[max_accel_sens][3], float g); calibrate_return calculate_calibration_values(unsigned sensor,
float (&accel_ref)[max_accel_sens][detect_orientation_side_count][3], float (&accel_T)[max_accel_sens][3][3],
float (&accel_offs)[max_accel_sens][3], float g);
/// Data passed to calibration worker routine /// Data passed to calibration worker routine
typedef struct { typedef struct {
@@ -208,37 +212,50 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
if (res != PX4_OK) { if (res != PX4_OK) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_RESET_CAL_MSG, s); calibration_log_critical(mavlink_log_pub, CAL_ERROR_RESET_CAL_MSG, s);
} }
#else #else
(void)sprintf(str, "CAL_ACC%u_XOFF", s); (void)sprintf(str, "CAL_ACC%u_XOFF", s);
res = param_set_no_notification(param_find(str), &accel_scale.x_offset); res = param_set_no_notification(param_find(str), &accel_scale.x_offset);
if (res != PX4_OK) { if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str); PX4_ERR("unable to reset %s", str);
} }
(void)sprintf(str, "CAL_ACC%u_YOFF", s); (void)sprintf(str, "CAL_ACC%u_YOFF", s);
res = param_set_no_notification(param_find(str), &accel_scale.y_offset); res = param_set_no_notification(param_find(str), &accel_scale.y_offset);
if (res != PX4_OK) { if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str); PX4_ERR("unable to reset %s", str);
} }
(void)sprintf(str, "CAL_ACC%u_ZOFF", s); (void)sprintf(str, "CAL_ACC%u_ZOFF", s);
res = param_set_no_notification(param_find(str), &accel_scale.z_offset); res = param_set_no_notification(param_find(str), &accel_scale.z_offset);
if (res != PX4_OK) { if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str); PX4_ERR("unable to reset %s", str);
} }
(void)sprintf(str, "CAL_ACC%u_XSCALE", s); (void)sprintf(str, "CAL_ACC%u_XSCALE", s);
res = param_set_no_notification(param_find(str), &accel_scale.x_scale); res = param_set_no_notification(param_find(str), &accel_scale.x_scale);
if (res != PX4_OK) { if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str); PX4_ERR("unable to reset %s", str);
} }
(void)sprintf(str, "CAL_ACC%u_YSCALE", s); (void)sprintf(str, "CAL_ACC%u_YSCALE", s);
res = param_set_no_notification(param_find(str), &accel_scale.y_scale); res = param_set_no_notification(param_find(str), &accel_scale.y_scale);
if (res != PX4_OK) { if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str); PX4_ERR("unable to reset %s", str);
} }
(void)sprintf(str, "CAL_ACC%u_ZSCALE", s); (void)sprintf(str, "CAL_ACC%u_ZSCALE", s);
res = param_set_no_notification(param_find(str), &accel_scale.z_scale); res = param_set_no_notification(param_find(str), &accel_scale.z_scale);
if (res != PX4_OK) { if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str); PX4_ERR("unable to reset %s", str);
} }
param_notify_changes(); param_notify_changes();
#endif #endif
} }
@@ -250,11 +267,14 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
/* measure and calculate offsets & scales */ /* measure and calculate offsets & scales */
if (res == PX4_OK) { if (res == PX4_OK) {
calibrate_return cal_return = do_accel_calibration_measurements(mavlink_log_pub, accel_offs, accel_T, &active_sensors); calibrate_return cal_return = do_accel_calibration_measurements(mavlink_log_pub, accel_offs, accel_T, &active_sensors);
if (cal_return == calibrate_return_cancelled) { if (cal_return == calibrate_return_cancelled) {
// Cancel message already displayed, nothing left to do // Cancel message already displayed, nothing left to do
return PX4_ERROR; return PX4_ERROR;
} else if (cal_return == calibrate_return_ok) { } else if (cal_return == calibrate_return_ok) {
res = PX4_OK; res = PX4_OK;
} else { } else {
res = PX4_ERROR; res = PX4_ERROR;
} }
@@ -264,6 +284,7 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
if (active_sensors == 0) { if (active_sensors == 0) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SENSOR_MSG); calibration_log_critical(mavlink_log_pub, CAL_ERROR_SENSOR_MSG);
} }
return PX4_ERROR; return PX4_ERROR;
} }
@@ -310,6 +331,7 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
/* check if thermal compensation is enabled */ /* check if thermal compensation is enabled */
int32_t tc_enabled_int; int32_t tc_enabled_int;
param_get(param_find("TC_A_ENABLE"), &(tc_enabled_int)); param_get(param_find("TC_A_ENABLE"), &(tc_enabled_int));
if (tc_enabled_int == 1) { if (tc_enabled_int == 1) {
/* Get struct containing sensor thermal compensation data */ /* Get struct containing sensor thermal compensation data */
struct sensor_correction_s sensor_correction; /**< sensor thermal corrections */ struct sensor_correction_s sensor_correction; /**< sensor thermal corrections */
@@ -325,18 +347,23 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
/* update the _X0_ terms to include the additional offset */ /* update the _X0_ terms to include the additional offset */
int32_t handle; int32_t handle;
float val; float val;
for (unsigned axis_index = 0; axis_index < 3; axis_index++) { for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
val = 0.0f; val = 0.0f;
(void)sprintf(str, "TC_A%u_X0_%u", sensor_correction.accel_mapping[uorb_index], axis_index); (void)sprintf(str, "TC_A%u_X0_%u", sensor_correction.accel_mapping[uorb_index], axis_index);
handle = param_find(str); handle = param_find(str);
param_get(handle, &val); param_get(handle, &val);
if (axis_index == 0) { if (axis_index == 0) {
val += accel_scale.x_offset; val += accel_scale.x_offset;
} else if (axis_index == 1) { } else if (axis_index == 1) {
val += accel_scale.y_offset; val += accel_scale.y_offset;
} else if (axis_index == 2) { } else if (axis_index == 2) {
val += accel_scale.z_offset; val += accel_scale.z_offset;
} }
failed |= (PX4_OK != param_set_no_notification(handle, &val)); failed |= (PX4_OK != param_set_no_notification(handle, &val));
} }
@@ -345,15 +372,20 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
val = 1.0f; val = 1.0f;
(void)sprintf(str, "TC_A%u_SCL_%u", sensor_correction.accel_mapping[uorb_index], axis_index); (void)sprintf(str, "TC_A%u_SCL_%u", sensor_correction.accel_mapping[uorb_index], axis_index);
handle = param_find(str); handle = param_find(str);
if (axis_index == 0) { if (axis_index == 0) {
val = accel_scale.x_scale; val = accel_scale.x_scale;
} else if (axis_index == 1) { } else if (axis_index == 1) {
val = accel_scale.y_scale; val = accel_scale.y_scale;
} else if (axis_index == 2) { } else if (axis_index == 2) {
val = accel_scale.z_scale; val = accel_scale.z_scale;
} }
failed |= (PX4_OK != param_set_no_notification(handle, &val)); failed |= (PX4_OK != param_set_no_notification(handle, &val));
} }
param_notify_changes(); param_notify_changes();
} }
@@ -394,6 +426,7 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
if (fd < 0) { if (fd < 0) {
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "sensor does not exist"); calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "sensor does not exist");
res = PX4_ERROR; res = PX4_ERROR;
} else { } else {
res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale); res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
px4_close(fd); px4_close(fd);
@@ -402,6 +435,7 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
if (res != PX4_OK) { if (res != PX4_OK) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG, uorb_index); calibration_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG, uorb_index);
} }
#endif #endif
} }
@@ -426,11 +460,14 @@ static calibrate_return accel_calibration_worker(detect_orientation_return orien
const unsigned samples_num = 750; const unsigned samples_num = 750;
accel_worker_data_t *worker_data = (accel_worker_data_t *)(data); accel_worker_data_t *worker_data = (accel_worker_data_t *)(data);
calibration_log_info(worker_data->mavlink_log_pub, "[cal] Hold still, measuring %s side", detect_orientation_str(orientation)); calibration_log_info(worker_data->mavlink_log_pub, "[cal] Hold still, measuring %s side",
detect_orientation_str(orientation));
read_accelerometer_avg(worker_data->sensor_correction_sub, worker_data->subs, worker_data->accel_ref, orientation, samples_num); read_accelerometer_avg(worker_data->sensor_correction_sub, worker_data->subs, worker_data->accel_ref, orientation,
samples_num);
calibration_log_info(worker_data->mavlink_log_pub, "[cal] %s side result: [%8.4f %8.4f %8.4f]", detect_orientation_str(orientation), calibration_log_info(worker_data->mavlink_log_pub, "[cal] %s side result: [%8.4f %8.4f %8.4f]",
detect_orientation_str(orientation),
(double)worker_data->accel_ref[0][orientation][0], (double)worker_data->accel_ref[0][orientation][0],
(double)worker_data->accel_ref[0][orientation][1], (double)worker_data->accel_ref[0][orientation][1],
(double)worker_data->accel_ref[0][orientation][2]); (double)worker_data->accel_ref[0][orientation][2]);
@@ -441,7 +478,8 @@ static calibrate_return accel_calibration_worker(detect_orientation_return orien
return calibrate_return_ok; return calibrate_return_ok;
} }
calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors) calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub,
float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors)
{ {
calibrate_return result = calibrate_return_ok; calibrate_return result = calibrate_return_ok;
@@ -469,13 +507,15 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub
// Warn that we will not calibrate more than max_accels accelerometers // Warn that we will not calibrate more than max_accels accelerometers
if (orb_accel_count > max_accel_sens) { if (orb_accel_count > max_accel_sens) {
calibration_log_critical(mavlink_log_pub, "Detected %u accels, but will calibrate only %u", orb_accel_count, max_accel_sens); calibration_log_critical(mavlink_log_pub, "Detected %u accels, but will calibrate only %u", orb_accel_count,
max_accel_sens);
} }
for (unsigned cur_accel = 0; cur_accel < orb_accel_count && cur_accel < max_accel_sens; cur_accel++) { for (unsigned cur_accel = 0; cur_accel < orb_accel_count && cur_accel < max_accel_sens; cur_accel++) {
// Lock in to correct ORB instance // Lock in to correct ORB instance
bool found_cur_accel = false; bool found_cur_accel = false;
for (unsigned i = 0; i < orb_accel_count && !found_cur_accel; i++) { for (unsigned i = 0; i < orb_accel_count && !found_cur_accel; i++) {
worker_data.subs[cur_accel] = orb_subscribe_multi(ORB_ID(sensor_accel), i); worker_data.subs[cur_accel] = orb_subscribe_multi(ORB_ID(sensor_accel), i);
@@ -493,6 +533,7 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub
found_cur_accel = true; found_cur_accel = true;
// store initial timestamp - used to infer which sensors are active // store initial timestamp - used to infer which sensors are active
timestamps[cur_accel] = report.timestamp; timestamps[cur_accel] = report.timestamp;
} else { } else {
orb_unsubscribe(worker_data.subs[cur_accel]); orb_unsubscribe(worker_data.subs[cur_accel]);
} }
@@ -521,6 +562,7 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub
device_prio_max = prio; device_prio_max = prio;
device_id_primary = device_id[cur_accel]; device_id_primary = device_id[cur_accel];
} }
} else { } else {
calibration_log_critical(mavlink_log_pub, "Accel #%u no device id, abort", cur_accel); calibration_log_critical(mavlink_log_pub, "Accel #%u no device id, abort", cur_accel);
result = calibrate_return_error; result = calibrate_return_error;
@@ -530,7 +572,8 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub
if (result == calibrate_return_ok) { if (result == calibrate_return_ok) {
int cancel_sub = calibrate_cancel_subscribe(); int cancel_sub = calibrate_cancel_subscribe();
result = calibrate_from_orientation(mavlink_log_pub, cancel_sub, data_collected, accel_calibration_worker, &worker_data, false /* normal still */); result = calibrate_from_orientation(mavlink_log_pub, cancel_sub, data_collected, accel_calibration_worker, &worker_data,
false /* normal still */);
calibrate_cancel_unsubscribe(cancel_sub); calibrate_cancel_unsubscribe(cancel_sub);
} }
@@ -540,12 +583,15 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub
/* figure out which sensors were active */ /* figure out which sensors were active */
sensor_accel_s arp = {}; sensor_accel_s arp = {};
(void)orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &arp); (void)orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &arp);
if (arp.timestamp != 0 && timestamps[i] != arp.timestamp) { if (arp.timestamp != 0 && timestamps[i] != arp.timestamp) {
(*active_sensors)++; (*active_sensors)++;
} }
px4_close(worker_data.subs[i]); px4_close(worker_data.subs[i]);
} }
} }
orb_unsubscribe(worker_data.sensor_correction_sub); orb_unsubscribe(worker_data.sensor_correction_sub);
if (result == calibrate_return_ok) { if (result == calibrate_return_ok) {
@@ -566,7 +612,8 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub
/* /*
* Read specified number of accelerometer samples, calculate average and dispersion. * Read specified number of accelerometer samples, calculate average and dispersion.
*/ */
calibrate_return read_accelerometer_avg(int sensor_correction_sub, int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num) calibrate_return read_accelerometer_avg(int sensor_correction_sub, int (&subs)[max_accel_sens],
float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num)
{ {
/* get total sensor board rotation matrix */ /* get total sensor board rotation matrix */
param_t board_rotation_h = param_find("SENS_BOARD_ROT"); param_t board_rotation_h = param_find("SENS_BOARD_ROT");
@@ -607,6 +654,7 @@ calibrate_return read_accelerometer_avg(int sensor_correction_sub, int (&subs)[m
if (orb_copy(ORB_ID(sensor_correction), sensor_correction_sub, &sensor_correction) != 0) { if (orb_copy(ORB_ID(sensor_correction), sensor_correction_sub, &sensor_correction) != 0) {
/* use default values */ /* use default values */
memset(&sensor_correction, 0, sizeof(sensor_correction)); memset(&sensor_correction, 0, sizeof(sensor_correction));
for (unsigned i = 0; i < 3; i++) { for (unsigned i = 0; i < 3; i++) {
sensor_correction.accel_scale_0[i] = 1.0f; sensor_correction.accel_scale_0[i] = 1.0f;
sensor_correction.accel_scale_1[i] = 1.0f; sensor_correction.accel_scale_1[i] = 1.0f;
@@ -634,14 +682,17 @@ calibrate_return read_accelerometer_avg(int sensor_correction_sub, int (&subs)[m
accel_sum[s][0] += (arp.x - sensor_correction.accel_offset_0[0]); accel_sum[s][0] += (arp.x - sensor_correction.accel_offset_0[0]);
accel_sum[s][1] += (arp.y - sensor_correction.accel_offset_0[1]); accel_sum[s][1] += (arp.y - sensor_correction.accel_offset_0[1]);
accel_sum[s][2] += (arp.z - sensor_correction.accel_offset_0[2]); accel_sum[s][2] += (arp.z - sensor_correction.accel_offset_0[2]);
} else if (s == 1) { } else if (s == 1) {
accel_sum[s][0] += (arp.x - sensor_correction.accel_offset_1[0]); accel_sum[s][0] += (arp.x - sensor_correction.accel_offset_1[0]);
accel_sum[s][1] += (arp.y - sensor_correction.accel_offset_1[1]); accel_sum[s][1] += (arp.y - sensor_correction.accel_offset_1[1]);
accel_sum[s][2] += (arp.z - sensor_correction.accel_offset_1[2]); accel_sum[s][2] += (arp.z - sensor_correction.accel_offset_1[2]);
} else if (s == 2) { } else if (s == 2) {
accel_sum[s][0] += (arp.x - sensor_correction.accel_offset_2[0]); accel_sum[s][0] += (arp.x - sensor_correction.accel_offset_2[0]);
accel_sum[s][1] += (arp.y - sensor_correction.accel_offset_2[1]); accel_sum[s][1] += (arp.y - sensor_correction.accel_offset_2[1]);
accel_sum[s][2] += (arp.z - sensor_correction.accel_offset_2[2]); accel_sum[s][2] += (arp.z - sensor_correction.accel_offset_2[2]);
} else { } else {
accel_sum[s][0] += arp.x; accel_sum[s][0] += arp.x;
accel_sum[s][1] += arp.y; accel_sum[s][1] += arp.y;
@@ -701,7 +752,9 @@ int mat_invert3(float src[3][3], float dst[3][3])
return PX4_OK; return PX4_OK;
} }
calibrate_return calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_sens][detect_orientation_side_count][3], float (&accel_T)[max_accel_sens][3][3], float (&accel_offs)[max_accel_sens][3], float g) calibrate_return calculate_calibration_values(unsigned sensor,
float (&accel_ref)[max_accel_sens][detect_orientation_side_count][3], float (&accel_T)[max_accel_sens][3][3],
float (&accel_offs)[max_accel_sens][3], float g)
{ {
/* calculate offsets */ /* calculate offsets */
for (unsigned i = 0; i < 3; i++) { for (unsigned i = 0; i < 3; i++) {
@@ -737,7 +790,8 @@ calibrate_return calculate_calibration_values(unsigned sensor, float (&accel_ref
return calibrate_return_ok; return calibrate_return_ok;
} }
int do_level_calibration(orb_advert_t *mavlink_log_pub) { int do_level_calibration(orb_advert_t *mavlink_log_pub)
{
const unsigned cal_time = 5; const unsigned cal_time = 5;
const unsigned cal_hz = 100; const unsigned cal_hz = 100;
unsigned settle_time = 30; unsigned settle_time = 30;
@@ -782,12 +836,15 @@ int do_level_calibration(orb_advert_t *mavlink_log_pub) {
// sleep for some time // sleep for some time
hrt_abstime start = hrt_absolute_time(); hrt_abstime start = hrt_absolute_time();
while (hrt_elapsed_time(&start) < settle_time * 1000000) { while (hrt_elapsed_time(&start) < settle_time * 1000000) {
calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, (int)(90*hrt_elapsed_time(&start)/1e6f/(float)settle_time)); calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG,
(int)(90 * hrt_elapsed_time(&start) / 1e6f / (float)settle_time));
sleep(settle_time / 10); sleep(settle_time / 10);
} }
start = hrt_absolute_time(); start = hrt_absolute_time();
// average attitude for 5 seconds // average attitude for 5 seconds
while (hrt_elapsed_time(&start) < cal_time * 1000000) { while (hrt_elapsed_time(&start) < cal_time * 1000000) {
int pollret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); int pollret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
@@ -811,6 +868,7 @@ int do_level_calibration(orb_advert_t *mavlink_log_pub) {
if (counter > (cal_time * cal_hz / 2)) { if (counter > (cal_time * cal_hz / 2)) {
roll_mean /= counter; roll_mean /= counter;
pitch_mean /= counter; pitch_mean /= counter;
} else { } else {
calibration_log_info(mavlink_log_pub, "not enough measurements taken"); calibration_log_info(mavlink_log_pub, "not enough measurements taken");
success = false; success = false;
@@ -819,8 +877,10 @@ int do_level_calibration(orb_advert_t *mavlink_log_pub) {
if (fabsf(roll_mean) > 0.8f) { if (fabsf(roll_mean) > 0.8f) {
calibration_log_critical(mavlink_log_pub, "excess roll angle"); calibration_log_critical(mavlink_log_pub, "excess roll angle");
} else if (fabsf(pitch_mean) > 0.8f) { } else if (fabsf(pitch_mean) > 0.8f) {
calibration_log_critical(mavlink_log_pub, "excess pitch angle"); calibration_log_critical(mavlink_log_pub, "excess pitch angle");
} else { } else {
roll_mean *= (float)M_RAD_TO_DEG; roll_mean *= (float)M_RAD_TO_DEG;
pitch_mean *= (float)M_RAD_TO_DEG; pitch_mean *= (float)M_RAD_TO_DEG;
@@ -831,9 +891,11 @@ int do_level_calibration(orb_advert_t *mavlink_log_pub) {
} }
out: out:
if (success) { if (success) {
calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "level"); calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "level");
return 0; return 0;
} else { } else {
// set old parameters // set old parameters
param_set_no_notification(roll_offset_handle, &roll_offset_current); param_set_no_notification(roll_offset_handle, &roll_offset_current);
src/modules/commander/airspeed_calibration.cpp
+13 -4
View File
@@ -107,6 +107,7 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
/* only warn if analog scaling is zero */ /* only warn if analog scaling is zero */
float analog_scaling = 0.0f; float analog_scaling = 0.0f;
param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling)); param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling));
if (fabsf(analog_scaling) < 0.1f) { if (fabsf(analog_scaling) < 0.1f) {
calibration_log_critical(mavlink_log_pub, "[cal] No airspeed sensor found"); calibration_log_critical(mavlink_log_pub, "[cal] No airspeed sensor found");
goto error_return; goto error_return;
@@ -143,7 +144,8 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
/* any differential pressure failure a reason to abort */ /* any differential pressure failure a reason to abort */
if (diff_pres.error_count != 0) { if (diff_pres.error_count != 0) {
calibration_log_critical(mavlink_log_pub, "[cal] Airspeed sensor is reporting errors (%" PRIu64 ")", diff_pres.error_count); calibration_log_critical(mavlink_log_pub, "[cal] Airspeed sensor is reporting errors (%" PRIu64 ")",
diff_pres.error_count);
calibration_log_critical(mavlink_log_pub, "[cal] Check your wiring before trying again"); calibration_log_critical(mavlink_log_pub, "[cal] Check your wiring before trying again");
feedback_calibration_failed(mavlink_log_pub); feedback_calibration_failed(mavlink_log_pub);
goto error_return; goto error_return;
@@ -166,6 +168,7 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
int fd_scale = px4_open(AIRSPEED0_DEVICE_PATH, 0); int fd_scale = px4_open(AIRSPEED0_DEVICE_PATH, 0);
airscale.offset_pa = diff_pres_offset; airscale.offset_pa = diff_pres_offset;
if (fd_scale > 0) { if (fd_scale > 0) {
if (PX4_OK != px4_ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { if (PX4_OK != px4_ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
calibration_log_critical(mavlink_log_pub, "[cal] airspeed offset update failed"); calibration_log_critical(mavlink_log_pub, "[cal] airspeed offset update failed");
@@ -220,15 +223,19 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
if (fabsf(diff_pres.differential_pressure_filtered_pa) > 50.0f) { if (fabsf(diff_pres.differential_pressure_filtered_pa) > 50.0f) {
if (diff_pres.differential_pressure_filtered_pa > 0) { if (diff_pres.differential_pressure_filtered_pa > 0) {
calibration_log_info(mavlink_log_pub, "[cal] Positive pressure: OK (%d Pa)", (int)diff_pres.differential_pressure_filtered_pa); calibration_log_info(mavlink_log_pub, "[cal] Positive pressure: OK (%d Pa)",
(int)diff_pres.differential_pressure_filtered_pa);
break; break;
} else { } else {
/* do not allow negative values */ /* do not allow negative values */
calibration_log_critical(mavlink_log_pub, "[cal] Negative pressure difference detected (%d Pa)", (int)diff_pres.differential_pressure_filtered_pa); calibration_log_critical(mavlink_log_pub, "[cal] Negative pressure difference detected (%d Pa)",
(int)diff_pres.differential_pressure_filtered_pa);
calibration_log_critical(mavlink_log_pub, "[cal] Swap static and dynamic ports!"); calibration_log_critical(mavlink_log_pub, "[cal] Swap static and dynamic ports!");
/* the user setup is wrong, wipe the calibration to force a proper re-calibration */ /* the user setup is wrong, wipe the calibration to force a proper re-calibration */
diff_pres_offset = 0.0f; diff_pres_offset = 0.0f;
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, 1); calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, 1);
goto error_return; goto error_return;
@@ -244,9 +251,11 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
} }
if (calibration_counter % 500 == 0) { if (calibration_counter % 500 == 0) {
calibration_log_info(mavlink_log_pub, "[cal] Create air pressure! (got %d, wanted: 50 Pa)", (int)diff_pres.differential_pressure_filtered_pa); calibration_log_info(mavlink_log_pub, "[cal] Create air pressure! (got %d, wanted: 50 Pa)",
(int)diff_pres.differential_pressure_filtered_pa);
tune_neutral(true); tune_neutral(true);
} }
calibration_counter++; calibration_counter++;
} else if (poll_ret == 0) { } else if (poll_ret == 0) {
src/modules/commander/arm_auth.cpp
+20
View File
@@ -85,6 +85,7 @@ static void arm_auth_request_msg_send()
if (handle_vehicle_command_pub == nullptr) { if (handle_vehicle_command_pub == nullptr) {
handle_vehicle_command_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); handle_vehicle_command_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
} else { } else {
orb_publish(ORB_ID(vehicle_command), handle_vehicle_command_pub, &vcmd); orb_publish(ORB_ID(vehicle_command), handle_vehicle_command_pub, &vcmd);
} }
@@ -96,8 +97,10 @@ static uint8_t _auth_method_arm_req_check()
case ARM_AUTH_IDLE: case ARM_AUTH_IDLE:
/* no authentication in process? handle bellow */ /* no authentication in process? handle bellow */
break; break;
case ARM_AUTH_MISSION_APPROVED: case ARM_AUTH_MISSION_APPROVED:
return vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED; return vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
default: default:
return vehicle_command_ack_s::VEHICLE_RESULT_DENIED; return vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
} }
@@ -127,6 +130,7 @@ static uint8_t _auth_method_arm_req_check()
state = ARM_AUTH_IDLE; state = ARM_AUTH_IDLE;
mavlink_log_critical(mavlink_log_pub, "Arm auth: No response"); mavlink_log_critical(mavlink_log_pub, "Arm auth: No response");
break; break;
default: default:
break; break;
} }
@@ -141,11 +145,14 @@ static uint8_t _auth_method_two_arm_check()
case ARM_AUTH_IDLE: case ARM_AUTH_IDLE:
/* no authentication in process? handle bellow */ /* no authentication in process? handle bellow */
break; break;
case ARM_AUTH_MISSION_APPROVED: case ARM_AUTH_MISSION_APPROVED:
return vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED; return vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
case ARM_AUTH_WAITING_AUTH: case ARM_AUTH_WAITING_AUTH:
case ARM_AUTH_WAITING_AUTH_WITH_ACK: case ARM_AUTH_WAITING_AUTH_WITH_ACK:
return vehicle_command_ack_s::VEHICLE_RESULT_TEMPORARILY_REJECTED; return vehicle_command_ack_s::VEHICLE_RESULT_TEMPORARILY_REJECTED;
default: default:
return vehicle_command_ack_s::VEHICLE_RESULT_DENIED; return vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
} }
@@ -182,11 +189,14 @@ void arm_auth_update(hrt_abstime now, bool param_update)
case ARM_AUTH_WAITING_AUTH_WITH_ACK: case ARM_AUTH_WAITING_AUTH_WITH_ACK:
/* handle bellow */ /* handle bellow */
break; break;
case ARM_AUTH_MISSION_APPROVED: case ARM_AUTH_MISSION_APPROVED:
if (now > auth_timeout) { if (now > auth_timeout) {
state = ARM_AUTH_IDLE; state = ARM_AUTH_IDLE;
} }
return; return;
case ARM_AUTH_IDLE: case ARM_AUTH_IDLE:
default: default:
return; return;
@@ -199,6 +209,7 @@ void arm_auth_update(hrt_abstime now, bool param_update)
bool updated = false; bool updated = false;
orb_check(command_ack_sub, &updated); orb_check(command_ack_sub, &updated);
if (updated) { if (updated) {
orb_copy(ORB_ID(vehicle_command_ack), command_ack_sub, &command_ack); orb_copy(ORB_ID(vehicle_command_ack), command_ack_sub, &command_ack);
} }
@@ -210,6 +221,7 @@ void arm_auth_update(hrt_abstime now, bool param_update)
case vehicle_command_ack_s::VEHICLE_RESULT_IN_PROGRESS: case vehicle_command_ack_s::VEHICLE_RESULT_IN_PROGRESS:
state = ARM_AUTH_WAITING_AUTH_WITH_ACK; state = ARM_AUTH_WAITING_AUTH_WITH_ACK;
break; break;
case vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED: case vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED:
mavlink_log_critical(mavlink_log_pub, mavlink_log_critical(mavlink_log_pub,
"Arm auth: Authorized for the next %u seconds", "Arm auth: Authorized for the next %u seconds",
@@ -217,32 +229,40 @@ void arm_auth_update(hrt_abstime now, bool param_update)
state = ARM_AUTH_MISSION_APPROVED; state = ARM_AUTH_MISSION_APPROVED;
auth_timeout = now + (command_ack.result_param2 * 1000000); auth_timeout = now + (command_ack.result_param2 * 1000000);
return; return;
case vehicle_command_ack_s::VEHICLE_RESULT_TEMPORARILY_REJECTED: case vehicle_command_ack_s::VEHICLE_RESULT_TEMPORARILY_REJECTED:
mavlink_log_critical(mavlink_log_pub, "Arm auth: Temporarily rejected"); mavlink_log_critical(mavlink_log_pub, "Arm auth: Temporarily rejected");
state = ARM_AUTH_IDLE; state = ARM_AUTH_IDLE;
return; return;
case vehicle_command_ack_s::VEHICLE_RESULT_DENIED: case vehicle_command_ack_s::VEHICLE_RESULT_DENIED:
default: default:
switch (command_ack.result_param1) { switch (command_ack.result_param1) {
case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_NONE: case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_NONE:
/* Authorizer will send reason to ground station */ /* Authorizer will send reason to ground station */
break; break;
case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_INVALID_WAYPOINT: case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_INVALID_WAYPOINT:
mavlink_log_critical(mavlink_log_pub, "Arm auth: Denied, waypoint %i have a invalid value", command_ack.result_param2); mavlink_log_critical(mavlink_log_pub, "Arm auth: Denied, waypoint %i have a invalid value", command_ack.result_param2);
break; break;
case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_TIMEOUT: case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_TIMEOUT:
mavlink_log_critical(mavlink_log_pub, "Arm auth: Denied by timeout in authorizer"); mavlink_log_critical(mavlink_log_pub, "Arm auth: Denied by timeout in authorizer");
break; break;
case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_AIRSPACE_IN_USE: case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_AIRSPACE_IN_USE:
mavlink_log_critical(mavlink_log_pub, "Arm auth: Denied because airspace is in use"); mavlink_log_critical(mavlink_log_pub, "Arm auth: Denied because airspace is in use");
break; break;
case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_BAD_WEATHER: case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_BAD_WEATHER:
mavlink_log_critical(mavlink_log_pub, "Arm auth: Denied because of bad weather"); mavlink_log_critical(mavlink_log_pub, "Arm auth: Denied because of bad weather");
break; break;
case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_GENERIC: case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_GENERIC:
default: default:
mavlink_log_critical(mavlink_log_pub, "Arm auth: Denied"); mavlink_log_critical(mavlink_log_pub, "Arm auth: Denied");
} }
state = ARM_AUTH_IDLE; state = ARM_AUTH_IDLE;
return; return;
} }
src/modules/commander/calibration_routines.cpp
+3
View File
@@ -800,14 +800,17 @@ calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub,
int calibrate_cancel_subscribe() int calibrate_cancel_subscribe()
{ {
int vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command)); int vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));
if (vehicle_command_sub >= 0) { if (vehicle_command_sub >= 0) {
// make sure we won't read any old messages // make sure we won't read any old messages
struct vehicle_command_s cmd; struct vehicle_command_s cmd;
bool update; bool update;
while (orb_check(vehicle_command_sub, &update) == 0 && update) { while (orb_check(vehicle_command_sub, &update) == 0 && update) {
orb_copy(ORB_ID(vehicle_command), vehicle_command_sub, &cmd); orb_copy(ORB_ID(vehicle_command), vehicle_command_sub, &cmd);
} }
} }
return vehicle_command_sub; return vehicle_command_sub;
} }
src/modules/commander/commander_helper.cpp
+2 -1
View File
@@ -97,7 +97,8 @@ bool is_rotary_wing(const struct vehicle_status_s *current_status)
|| (current_status->system_type == VEHICLE_TYPE_COAXIAL); || (current_status->system_type == VEHICLE_TYPE_COAXIAL);
} }
bool is_vtol(const struct vehicle_status_s * current_status) { bool is_vtol(const struct vehicle_status_s *current_status)
{
return (current_status->system_type == VEHICLE_TYPE_VTOL_DUOROTOR || return (current_status->system_type == VEHICLE_TYPE_VTOL_DUOROTOR ||
current_status->system_type == VEHICLE_TYPE_VTOL_QUADROTOR || current_status->system_type == VEHICLE_TYPE_VTOL_QUADROTOR ||
current_status->system_type == VEHICLE_TYPE_VTOL_TILTROTOR || current_status->system_type == VEHICLE_TYPE_VTOL_TILTROTOR ||
src/modules/commander/commander_tests/state_machine_helper_test.cpp
+196 -96
View File
@@ -75,7 +75,8 @@ bool StateMachineHelperTest::armingStateTransitionTest()
const char *assertMsg; // Text to show when test case fails const char *assertMsg; // Text to show when test case fails
ArmingTransitionVolatileState_t current_state; // Machine state prior to transition ArmingTransitionVolatileState_t current_state; // Machine state prior to transition
hil_state_t hil_state; // Current vehicle_status_s.hil_state hil_state_t hil_state; // Current vehicle_status_s.hil_state
bool condition_system_sensors_initialized; // Current vehicle_status_s.condition_system_sensors_initialized bool
condition_system_sensors_initialized; // Current vehicle_status_s.condition_system_sensors_initialized
bool safety_switch_available; // Current safety_s.safety_switch_available bool safety_switch_available; // Current safety_s.safety_switch_available
bool safety_off; // Current safety_s.safety_off bool safety_off; // Current safety_s.safety_off
arming_state_t requested_state; // Requested arming state to transition to arming_state_t requested_state; // Requested arming state to transition to
@@ -99,122 +100,164 @@ bool StateMachineHelperTest::armingStateTransitionTest()
static const ArmingTransitionTest_t rgArmingTransitionTests[] = { static const ArmingTransitionTest_t rgArmingTransitionTests[] = {
// TRANSITION_NOT_CHANGED tests // TRANSITION_NOT_CHANGED tests
{ "no transition: identical states", {
"no transition: identical states",
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_INIT, vehicle_status_s::ARMING_STATE_INIT,
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_NOT_CHANGED }, { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_NOT_CHANGED
},
// TRANSITION_CHANGED tests // TRANSITION_CHANGED tests
// Check all basic valid transitions, these don't require special state in vehicle_status_t or safety_s // Check all basic valid transitions, these don't require special state in vehicle_status_t or safety_s
{ "transition: init to standby", {
"transition: init to standby",
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_STANDBY, vehicle_status_s::ARMING_STATE_STANDBY,
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED
},
{ "transition: init to standby error", {
"transition: init to standby error",
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_STANDBY_ERROR, vehicle_status_s::ARMING_STATE_STANDBY_ERROR,
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED
},
{ "transition: init to reboot", {
"transition: init to reboot",
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_REBOOT, vehicle_status_s::ARMING_STATE_REBOOT,
{ vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED
},
{ "transition: standby to init", {
"transition: standby to init",
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_INIT, vehicle_status_s::ARMING_STATE_INIT,
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED
},
{ "transition: standby to standby error", {
"transition: standby to standby error",
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_STANDBY_ERROR, vehicle_status_s::ARMING_STATE_STANDBY_ERROR,
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED
},
{ "transition: standby to reboot", {
"transition: standby to reboot",
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_REBOOT, vehicle_status_s::ARMING_STATE_REBOOT,
{ vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED
},
{ "transition: armed to standby", {
"transition: armed to standby",
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_STANDBY, vehicle_status_s::ARMING_STATE_STANDBY,
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED
},
{ "transition: standby error to reboot", {
"transition: standby error to reboot",
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_REBOOT, vehicle_status_s::ARMING_STATE_REBOOT,
{ vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED
},
{ "transition: in air restore to armed", {
"transition: in air restore to armed",
{ vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_ARMED, vehicle_status_s::ARMING_STATE_ARMED,
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED
},
{ "transition: in air restore to reboot", {
"transition: in air restore to reboot",
{ vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_REBOOT, vehicle_status_s::ARMING_STATE_REBOOT,
{ vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED
},
// hil on tests, standby error to standby not normally allowed // hil on tests, standby error to standby not normally allowed
{ "transition: standby error to standby, hil on", {
"transition: standby error to standby, hil on",
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_STANDBY, vehicle_status_s::ARMING_STATE_STANDBY,
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED
},
// Safety switch arming tests // Safety switch arming tests
{ "transition: standby to armed, no safety switch", {
"transition: standby to armed, no safety switch",
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF, { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF,
vehicle_status_s::ARMING_STATE_ARMED, vehicle_status_s::ARMING_STATE_ARMED,
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED
},
{ "transition: standby to armed, safety switch off", {
"transition: standby to armed, safety switch off",
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF, { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF,
vehicle_status_s::ARMING_STATE_ARMED, vehicle_status_s::ARMING_STATE_ARMED,
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED
},
// TRANSITION_DENIED tests // TRANSITION_DENIED tests
// Check some important basic invalid transitions, these don't require special state in vehicle_status_t or safety_s // Check some important basic invalid transitions, these don't require special state in vehicle_status_t or safety_s
{ "no transition: init to armed", {
"no transition: init to armed",
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_ARMED, vehicle_status_s::ARMING_STATE_ARMED,
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED
},
{ "no transition: armed to init", {
"no transition: armed to init",
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_INIT, vehicle_status_s::ARMING_STATE_INIT,
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED }, { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED
},
{ "no transition: armed to reboot", {
"no transition: armed to reboot",
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_REBOOT, vehicle_status_s::ARMING_STATE_REBOOT,
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED }, { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED
},
{ "no transition: standby error to armed", {
"no transition: standby error to armed",
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_ARMED, vehicle_status_s::ARMING_STATE_ARMED,
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED
},
{ "no transition: standby error to standby", {
"no transition: standby error to standby",
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_STANDBY, vehicle_status_s::ARMING_STATE_STANDBY,
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED
},
{ "no transition: reboot to armed", {
"no transition: reboot to armed",
{ vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_ARMED, vehicle_status_s::ARMING_STATE_ARMED,
{ vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, { vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED
},
{ "no transition: in air restore to standby", {
"no transition: in air restore to standby",
{ vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_STANDBY, vehicle_status_s::ARMING_STATE_STANDBY,
{ vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED
},
// Sensor tests // Sensor tests
@@ -225,10 +268,12 @@ bool StateMachineHelperTest::armingStateTransitionTest()
// Safety switch arming tests // Safety switch arming tests
{ "no transition: init to standby, safety switch on", {
"no transition: init to standby, safety switch on",
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_ARMED, vehicle_status_s::ARMING_STATE_ARMED,
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED }, { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED
},
}; };
struct vehicle_status_s status = {}; struct vehicle_status_s status = {};
@@ -237,6 +282,7 @@ bool StateMachineHelperTest::armingStateTransitionTest()
struct actuator_armed_s armed = {}; struct actuator_armed_s armed = {};
size_t cArmingTransitionTests = sizeof(rgArmingTransitionTests) / sizeof(rgArmingTransitionTests[0]); size_t cArmingTransitionTests = sizeof(rgArmingTransitionTests) / sizeof(rgArmingTransitionTests[0]);
for (size_t i = 0; i < cArmingTransitionTests; i++) { for (size_t i = 0; i < cArmingTransitionTests; i++) {
const ArmingTransitionTest_t *test = &rgArmingTransitionTests[i]; const ArmingTransitionTest_t *test = &rgArmingTransitionTests[i];
@@ -295,112 +341,163 @@ bool StateMachineHelperTest::mainStateTransitionTest()
// TRANSITION_NOT_CHANGED tests // TRANSITION_NOT_CHANGED tests
{ "no transition: identical states", {
"no transition: identical states",
MTT_ALL_NOT_VALID, MTT_ALL_NOT_VALID,
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_NOT_CHANGED }, commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_NOT_CHANGED
},
// TRANSITION_CHANGED tests // TRANSITION_CHANGED tests
{ "transition: MANUAL to ACRO - rotary", {
"transition: MANUAL to ACRO - rotary",
MTT_ROTARY_WING, MTT_ROTARY_WING,
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ACRO, TRANSITION_CHANGED }, commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ACRO, TRANSITION_CHANGED
},
{ "transition: MANUAL to ACRO - not rotary", {
"transition: MANUAL to ACRO - not rotary",
MTT_ALL_NOT_VALID, MTT_ALL_NOT_VALID,
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ACRO, TRANSITION_CHANGED }, commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ACRO, TRANSITION_CHANGED
},
{ "transition: ACRO to MANUAL", {
"transition: ACRO to MANUAL",
MTT_ALL_NOT_VALID, MTT_ALL_NOT_VALID,
commander_state_s::MAIN_STATE_ACRO, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED }, commander_state_s::MAIN_STATE_ACRO, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED
},
{ "transition: MANUAL to AUTO_MISSION - global position valid, home position valid", {
"transition: MANUAL to AUTO_MISSION - global position valid, home position valid",
MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID, MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED }, commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED
},
{ "transition: AUTO_MISSION to MANUAL - global position valid, home position valid", {
"transition: AUTO_MISSION to MANUAL - global position valid, home position valid",
MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID, MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
commander_state_s::MAIN_STATE_AUTO_MISSION, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED }, commander_state_s::MAIN_STATE_AUTO_MISSION, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED
},
{ "transition: MANUAL to AUTO_LOITER - global position valid", {
"transition: MANUAL to AUTO_LOITER - global position valid",
MTT_GLOBAL_POS_VALID, MTT_GLOBAL_POS_VALID,
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_LOITER, TRANSITION_CHANGED }, commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_LOITER, TRANSITION_CHANGED
},
{ "transition: AUTO_LOITER to MANUAL - global position valid", {
"transition: AUTO_LOITER to MANUAL - global position valid",
MTT_GLOBAL_POS_VALID, MTT_GLOBAL_POS_VALID,
commander_state_s::MAIN_STATE_AUTO_LOITER, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED }, commander_state_s::MAIN_STATE_AUTO_LOITER, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED
},
{ "transition: MANUAL to AUTO_RTL - global position valid, home position valid", {
"transition: MANUAL to AUTO_RTL - global position valid, home position valid",
MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID, MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_RTL, TRANSITION_CHANGED }, commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_RTL, TRANSITION_CHANGED
},
{ "transition: AUTO_RTL to MANUAL - global position valid, home position valid", {
"transition: AUTO_RTL to MANUAL - global position valid, home position valid",
MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID, MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
commander_state_s::MAIN_STATE_AUTO_RTL, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED }, commander_state_s::MAIN_STATE_AUTO_RTL, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED
},
{ "transition: MANUAL to ALTCTL - not rotary", {
"transition: MANUAL to ALTCTL - not rotary",
MTT_LOC_ALT_VALID, MTT_LOC_ALT_VALID,
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED }, commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED
},
{ "transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude valid", {
"transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude valid",
MTT_ROTARY_WING | MTT_LOC_ALT_VALID, MTT_ROTARY_WING | MTT_LOC_ALT_VALID,
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED }, commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED
},
{ "transition: MANUAL to ALTCTL - rotary, global position valid, local altitude not valid", {
"transition: MANUAL to ALTCTL - rotary, global position valid, local altitude not valid",
MTT_ROTARY_WING | MTT_GLOBAL_POS_VALID, MTT_ROTARY_WING | MTT_GLOBAL_POS_VALID,
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED }, commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED
},
{ "transition: ALTCTL to MANUAL - local altitude valid", {
"transition: ALTCTL to MANUAL - local altitude valid",
MTT_LOC_ALT_VALID, MTT_LOC_ALT_VALID,
commander_state_s::MAIN_STATE_ALTCTL, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED }, commander_state_s::MAIN_STATE_ALTCTL, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED
},
{ "transition: MANUAL to POSCTL - local position not valid, global position valid", {
"transition: MANUAL to POSCTL - local position not valid, global position valid",
MTT_GLOBAL_POS_VALID, MTT_GLOBAL_POS_VALID,
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_POSCTL, TRANSITION_CHANGED }, commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_POSCTL, TRANSITION_CHANGED
},
{ "transition: MANUAL to POSCTL - local position valid, global position not valid", {
"transition: MANUAL to POSCTL - local position valid, global position not valid",
MTT_LOC_POS_VALID, MTT_LOC_POS_VALID,
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_POSCTL, TRANSITION_CHANGED }, commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_POSCTL, TRANSITION_CHANGED
},
{ "transition: POSCTL to MANUAL - local position valid, global position valid", {
"transition: POSCTL to MANUAL - local position valid, global position valid",
MTT_LOC_POS_VALID, MTT_LOC_POS_VALID,
commander_state_s::MAIN_STATE_POSCTL, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED }, commander_state_s::MAIN_STATE_POSCTL, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED
},
// TRANSITION_DENIED tests // TRANSITION_DENIED tests
{ "no transition: MANUAL to AUTO_MISSION - global position not valid", {
"no transition: MANUAL to AUTO_MISSION - global position not valid",
MTT_ALL_NOT_VALID, MTT_ALL_NOT_VALID,
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_MISSION, TRANSITION_DENIED }, commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_MISSION, TRANSITION_DENIED
},
{ "no transition: MANUAL to AUTO_LOITER - global position not valid", {
"no transition: MANUAL to AUTO_LOITER - global position not valid",
MTT_ALL_NOT_VALID, MTT_ALL_NOT_VALID,
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_LOITER, TRANSITION_DENIED }, commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_LOITER, TRANSITION_DENIED
},
{ "no transition: MANUAL to AUTO_RTL - global position not valid, home position not valid", {
"no transition: MANUAL to AUTO_RTL - global position not valid, home position not valid",
MTT_ALL_NOT_VALID, MTT_ALL_NOT_VALID,
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED }, commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED
},
{ "no transition: MANUAL to AUTO_RTL - global position not valid, home position valid", {
"no transition: MANUAL to AUTO_RTL - global position not valid, home position valid",
MTT_HOME_POS_VALID, MTT_HOME_POS_VALID,
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED }, commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED
},
{ "no transition: MANUAL to AUTO_RTL - global position valid, home position not valid", {
"no transition: MANUAL to AUTO_RTL - global position valid, home position not valid",
MTT_GLOBAL_POS_VALID, MTT_GLOBAL_POS_VALID,
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED }, commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED
},
{ "transition: MANUAL to ALTCTL - not rotary", {
"transition: MANUAL to ALTCTL - not rotary",
MTT_ALL_NOT_VALID, MTT_ALL_NOT_VALID,
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ALTCTL, TRANSITION_DENIED }, commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ALTCTL, TRANSITION_DENIED
},
{ "no transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude not valid", {
"no transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude not valid",
MTT_ROTARY_WING, MTT_ROTARY_WING,
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ALTCTL, TRANSITION_DENIED }, commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ALTCTL, TRANSITION_DENIED
},
{ "no transition: MANUAL to POSCTL - local position not valid, global position not valid", {
"no transition: MANUAL to POSCTL - local position not valid, global position not valid",
MTT_ALL_NOT_VALID, MTT_ALL_NOT_VALID,
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_POSCTL, TRANSITION_DENIED }, commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_POSCTL, TRANSITION_DENIED
},
}; };
size_t cMainTransitionTests = sizeof(rgMainTransitionTests) / sizeof(rgMainTransitionTests[0]); size_t cMainTransitionTests = sizeof(rgMainTransitionTests) / sizeof(rgMainTransitionTests[0]);
for (size_t i = 0; i < cMainTransitionTests; i++) { for (size_t i = 0; i < cMainTransitionTests; i++) {
const MainTransitionTest_t *test = &rgMainTransitionTests[i]; const MainTransitionTest_t *test = &rgMainTransitionTests[i];
@@ -418,13 +515,16 @@ bool StateMachineHelperTest::mainStateTransitionTest()
current_status_flags.condition_auto_mission_available = true; current_status_flags.condition_auto_mission_available = true;
// Attempt transition // Attempt transition
transition_result_t result = main_state_transition(current_vehicle_status, test->to_state, current_status_flags, &current_commander_state); transition_result_t result = main_state_transition(current_vehicle_status, test->to_state, current_status_flags,
&current_commander_state);
// Validate result of transition // Validate result of transition
ut_compare(test->assertMsg, test->expected_transition_result, result); ut_compare(test->assertMsg, test->expected_transition_result, result);
if (test->expected_transition_result == result) { if (test->expected_transition_result == result) {
if (test->expected_transition_result == TRANSITION_CHANGED) { if (test->expected_transition_result == TRANSITION_CHANGED) {
ut_compare(test->assertMsg, test->to_state, current_commander_state.main_state); ut_compare(test->assertMsg, test->to_state, current_commander_state.main_state);
} else { } else {
ut_compare(test->assertMsg, test->from_state, current_commander_state.main_state); ut_compare(test->assertMsg, test->from_state, current_commander_state.main_state);
} }
src/modules/commander/esc_calibration.cpp
+11
View File
@@ -77,6 +77,7 @@ int do_esc_calibration(orb_advert_t *mavlink_log_pub, struct actuator_armed_s* a
while (true) { while (true) {
if (hrt_absolute_time() - timeout_start > timeout_wait) { if (hrt_absolute_time() - timeout_start > timeout_wait) {
break; break;
} else { } else {
usleep(50000); usleep(50000);
} }
@@ -106,6 +107,7 @@ int do_esc_calibration(orb_advert_t *mavlink_log_pub, struct actuator_armed_s* a
calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "esc"); calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "esc");
batt_sub = orb_subscribe(ORB_ID(battery_status)); batt_sub = orb_subscribe(ORB_ID(battery_status));
if (batt_sub < 0) { if (batt_sub < 0) {
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Subscribe to battery"); calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Subscribe to battery");
goto Error; goto Error;
@@ -173,8 +175,10 @@ int do_esc_calibration(orb_advert_t *mavlink_log_pub, struct actuator_armed_s* a
if (!batt_connected) { if (!batt_connected) {
orb_check(batt_sub, &batt_updated); orb_check(batt_sub, &batt_updated);
if (batt_updated) { if (batt_updated) {
orb_copy(ORB_ID(battery_status), batt_sub, &battery); orb_copy(ORB_ID(battery_status), batt_sub, &battery);
if (battery.connected) { if (battery.connected) {
// Battery is connected, signal to user and start waiting again // Battery is connected, signal to user and start waiting again
batt_connected = true; batt_connected = true;
@@ -183,25 +187,32 @@ int do_esc_calibration(orb_advert_t *mavlink_log_pub, struct actuator_armed_s* a
} }
} }
} }
usleep(50_ms); usleep(50_ms);
} }
Out: Out:
if (batt_sub != -1) { if (batt_sub != -1) {
orb_unsubscribe(batt_sub); orb_unsubscribe(batt_sub);
} }
if (fd != -1) { if (fd != -1) {
if (px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_ON, 0) != PX4_OK) { if (px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_ON, 0) != PX4_OK) {
calibration_log_info(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Safety switch still off"); calibration_log_info(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Safety switch still off");
} }
if (px4_ioctl(fd, PWM_SERVO_DISARM, 0) != PX4_OK) { if (px4_ioctl(fd, PWM_SERVO_DISARM, 0) != PX4_OK) {
calibration_log_info(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Servos still armed"); calibration_log_info(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Servos still armed");
} }
if (px4_ioctl(fd, PWM_SERVO_CLEAR_ARM_OK, 0) != PX4_OK) { if (px4_ioctl(fd, PWM_SERVO_CLEAR_ARM_OK, 0) != PX4_OK) {
calibration_log_info(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Safety switch still deactivated"); calibration_log_info(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Safety switch still deactivated");
} }
px4_close(fd); px4_close(fd);
} }
armed->in_esc_calibration_mode = false; armed->in_esc_calibration_mode = false;
if (return_code == PX4_OK) { if (return_code == PX4_OK) {
src/modules/commander/failure_detector/FailureDetector.cpp
+1
View File
@@ -83,6 +83,7 @@ FailureDetector::update_attitude_status()
if (roll_status) { if (roll_status) {
_status |= FAILURE_ROLL; _status |= FAILURE_ROLL;
} }
if (pitch_status) { if (pitch_status) {
_status |= FAILURE_PITCH; _status |= FAILURE_PITCH;
} }
src/modules/commander/gyro_calibration.cpp
+50 -10
View File
@@ -83,9 +83,11 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data)
unsigned poll_errcount = 0; unsigned poll_errcount = 0;
struct sensor_correction_s sensor_correction; /**< sensor thermal corrections */ struct sensor_correction_s sensor_correction; /**< sensor thermal corrections */
if (orb_copy(ORB_ID(sensor_correction), worker_data->sensor_correction_sub, &sensor_correction) != 0) { if (orb_copy(ORB_ID(sensor_correction), worker_data->sensor_correction_sub, &sensor_correction) != 0) {
/* use default values */ /* use default values */
memset(&sensor_correction, 0, sizeof(sensor_correction)); memset(&sensor_correction, 0, sizeof(sensor_correction));
for (unsigned i = 0; i < 3; i++) { for (unsigned i = 0; i < 3; i++) {
sensor_correction.gyro_scale_0[i] = 1.0f; sensor_correction.gyro_scale_0[i] = 1.0f;
sensor_correction.gyro_scale_1[i] = 1.0f; sensor_correction.gyro_scale_1[i] = 1.0f;
@@ -94,6 +96,7 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data)
} }
px4_pollfd_struct_t fds[max_gyros]; px4_pollfd_struct_t fds[max_gyros];
for (unsigned s = 0; s < max_gyros; s++) { for (unsigned s = 0; s < max_gyros; s++) {
fds[s].fd = worker_data->gyro_sensor_sub[s]; fds[s].fd = worker_data->gyro_sensor_sub[s];
fds[s].events = POLLIN; fds[s].events = POLLIN;
@@ -120,6 +123,7 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data)
if (poll_ret > 0) { if (poll_ret > 0) {
unsigned update_count = calibration_count; unsigned update_count = calibration_count;
for (unsigned s = 0; s < max_gyros; s++) { for (unsigned s = 0; s < max_gyros; s++) {
if (calibration_counter[s] >= calibration_count) { if (calibration_counter[s] >= calibration_count) {
// Skip if instance has enough samples // Skip if instance has enough samples
@@ -134,9 +138,12 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data)
if (s == 0) { if (s == 0) {
// take a working copy // take a working copy
worker_data->gyro_scale[s].x_offset += (gyro_report.x - sensor_correction.gyro_offset_0[0]) * sensor_correction.gyro_scale_0[0]; worker_data->gyro_scale[s].x_offset += (gyro_report.x - sensor_correction.gyro_offset_0[0]) *
worker_data->gyro_scale[s].y_offset += (gyro_report.y - sensor_correction.gyro_offset_0[1]) * sensor_correction.gyro_scale_0[1]; sensor_correction.gyro_scale_0[0];
worker_data->gyro_scale[s].z_offset += (gyro_report.z - sensor_correction.gyro_offset_0[2]) * sensor_correction.gyro_scale_0[2]; worker_data->gyro_scale[s].y_offset += (gyro_report.y - sensor_correction.gyro_offset_0[1]) *
sensor_correction.gyro_scale_0[1];
worker_data->gyro_scale[s].z_offset += (gyro_report.z - sensor_correction.gyro_offset_0[2]) *
sensor_correction.gyro_scale_0[2];
// take a reference copy of the primary sensor including correction for thermal drift // take a reference copy of the primary sensor including correction for thermal drift
orb_copy(ORB_ID(sensor_gyro), worker_data->gyro_sensor_sub[s], &worker_data->gyro_report_0); orb_copy(ORB_ID(sensor_gyro), worker_data->gyro_sensor_sub[s], &worker_data->gyro_report_0);
@@ -145,14 +152,20 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data)
worker_data->gyro_report_0.z = (gyro_report.z - sensor_correction.gyro_offset_0[2]) * sensor_correction.gyro_scale_0[2]; worker_data->gyro_report_0.z = (gyro_report.z - sensor_correction.gyro_offset_0[2]) * sensor_correction.gyro_scale_0[2];
} else if (s == 1) { } else if (s == 1) {
worker_data->gyro_scale[s].x_offset += (gyro_report.x - sensor_correction.gyro_offset_1[0]) * sensor_correction.gyro_scale_1[0]; worker_data->gyro_scale[s].x_offset += (gyro_report.x - sensor_correction.gyro_offset_1[0]) *
worker_data->gyro_scale[s].y_offset += (gyro_report.y - sensor_correction.gyro_offset_1[1]) * sensor_correction.gyro_scale_1[1]; sensor_correction.gyro_scale_1[0];
worker_data->gyro_scale[s].z_offset += (gyro_report.z - sensor_correction.gyro_offset_1[2]) * sensor_correction.gyro_scale_1[2]; worker_data->gyro_scale[s].y_offset += (gyro_report.y - sensor_correction.gyro_offset_1[1]) *
sensor_correction.gyro_scale_1[1];
worker_data->gyro_scale[s].z_offset += (gyro_report.z - sensor_correction.gyro_offset_1[2]) *
sensor_correction.gyro_scale_1[2];
} else if (s == 2) { } else if (s == 2) {
worker_data->gyro_scale[s].x_offset += (gyro_report.x - sensor_correction.gyro_offset_2[0]) * sensor_correction.gyro_scale_2[0]; worker_data->gyro_scale[s].x_offset += (gyro_report.x - sensor_correction.gyro_offset_2[0]) *
worker_data->gyro_scale[s].y_offset += (gyro_report.y - sensor_correction.gyro_offset_2[1]) * sensor_correction.gyro_scale_2[1]; sensor_correction.gyro_scale_2[0];
worker_data->gyro_scale[s].z_offset += (gyro_report.z - sensor_correction.gyro_offset_2[2]) * sensor_correction.gyro_scale_2[2]; worker_data->gyro_scale[s].y_offset += (gyro_report.y - sensor_correction.gyro_offset_2[1]) *
sensor_correction.gyro_scale_2[1];
worker_data->gyro_scale[s].z_offset += (gyro_report.z - sensor_correction.gyro_offset_2[2]) *
sensor_correction.gyro_scale_2[2];
} else { } else {
worker_data->gyro_scale[s].x_offset += gyro_report.x; worker_data->gyro_scale[s].x_offset += gyro_report.x;
@@ -236,6 +249,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
worker_data.gyro_sensor_sub[s] = -1; worker_data.gyro_sensor_sub[s] = -1;
(void)sprintf(str, "CAL_GYRO%u_ID", s); (void)sprintf(str, "CAL_GYRO%u_ID", s);
res = param_set_no_notification(param_find(str), &(worker_data.device_id[s])); res = param_set_no_notification(param_find(str), &(worker_data.device_id[s]));
if (res != PX4_OK) { if (res != PX4_OK) {
calibration_log_critical(mavlink_log_pub, "Unable to reset CAL_GYRO%u_ID", s); calibration_log_critical(mavlink_log_pub, "Unable to reset CAL_GYRO%u_ID", s);
return PX4_ERROR; return PX4_ERROR;
@@ -246,6 +260,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
#ifdef __PX4_NUTTX #ifdef __PX4_NUTTX
sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
int fd = px4_open(str, 0); int fd = px4_open(str, 0);
if (fd >= 0) { if (fd >= 0) {
worker_data.device_id[s] = px4_ioctl(fd, DEVIOCGDEVICEID, 0); worker_data.device_id[s] = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale_zero); res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale_zero);
@@ -256,37 +271,50 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
return PX4_ERROR; return PX4_ERROR;
} }
} }
#else #else
(void)sprintf(str, "CAL_GYRO%u_XOFF", s); (void)sprintf(str, "CAL_GYRO%u_XOFF", s);
res = param_set_no_notification(param_find(str), &gyro_scale_zero.x_offset); res = param_set_no_notification(param_find(str), &gyro_scale_zero.x_offset);
if (res != PX4_OK) { if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str); PX4_ERR("unable to reset %s", str);
} }
(void)sprintf(str, "CAL_GYRO%u_YOFF", s); (void)sprintf(str, "CAL_GYRO%u_YOFF", s);
res = param_set_no_notification(param_find(str), &gyro_scale_zero.y_offset); res = param_set_no_notification(param_find(str), &gyro_scale_zero.y_offset);
if (res != PX4_OK) { if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str); PX4_ERR("unable to reset %s", str);
} }
(void)sprintf(str, "CAL_GYRO%u_ZOFF", s); (void)sprintf(str, "CAL_GYRO%u_ZOFF", s);
res = param_set_no_notification(param_find(str), &gyro_scale_zero.z_offset); res = param_set_no_notification(param_find(str), &gyro_scale_zero.z_offset);
if (res != PX4_OK) { if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str); PX4_ERR("unable to reset %s", str);
} }
(void)sprintf(str, "CAL_GYRO%u_XSCALE", s); (void)sprintf(str, "CAL_GYRO%u_XSCALE", s);
res = param_set_no_notification(param_find(str), &gyro_scale_zero.x_scale); res = param_set_no_notification(param_find(str), &gyro_scale_zero.x_scale);
if (res != PX4_OK) { if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str); PX4_ERR("unable to reset %s", str);
} }
(void)sprintf(str, "CAL_GYRO%u_YSCALE", s); (void)sprintf(str, "CAL_GYRO%u_YSCALE", s);
res = param_set_no_notification(param_find(str), &gyro_scale_zero.y_scale); res = param_set_no_notification(param_find(str), &gyro_scale_zero.y_scale);
if (res != PX4_OK) { if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str); PX4_ERR("unable to reset %s", str);
} }
(void)sprintf(str, "CAL_GYRO%u_ZSCALE", s); (void)sprintf(str, "CAL_GYRO%u_ZSCALE", s);
res = param_set_no_notification(param_find(str), &gyro_scale_zero.z_scale); res = param_set_no_notification(param_find(str), &gyro_scale_zero.z_scale);
if (res != PX4_OK) { if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str); PX4_ERR("unable to reset %s", str);
} }
param_notify_changes(); param_notify_changes();
#endif #endif
@@ -304,6 +332,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
// Lock in to correct ORB instance // Lock in to correct ORB instance
bool found_cur_gyro = false; bool found_cur_gyro = false;
for (unsigned i = 0; i < orb_gyro_count && !found_cur_gyro; i++) { for (unsigned i = 0; i < orb_gyro_count && !found_cur_gyro; i++) {
worker_data.gyro_sensor_sub[cur_gyro] = orb_subscribe_multi(ORB_ID(sensor_gyro), i); worker_data.gyro_sensor_sub[cur_gyro] = orb_subscribe_multi(ORB_ID(sensor_gyro), i);
@@ -319,6 +348,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
if (report.device_id == (uint32_t)worker_data.device_id[cur_gyro]) { if (report.device_id == (uint32_t)worker_data.device_id[cur_gyro]) {
// Device IDs match, correct ORB instance for this gyro // Device IDs match, correct ORB instance for this gyro
found_cur_gyro = true; found_cur_gyro = true;
} else { } else {
orb_unsubscribe(worker_data.gyro_sensor_sub[cur_gyro]); orb_unsubscribe(worker_data.gyro_sensor_sub[cur_gyro]);
} }
@@ -333,7 +363,8 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
} }
if (!found_cur_gyro) { if (!found_cur_gyro) {
calibration_log_critical(mavlink_log_pub, "Gyro #%u (ID %u) no matching uORB devid", cur_gyro, worker_data.device_id[cur_gyro]); calibration_log_critical(mavlink_log_pub, "Gyro #%u (ID %u) no matching uORB devid", cur_gyro,
worker_data.device_id[cur_gyro]);
res = calibrate_return_error; res = calibrate_return_error;
break; break;
} }
@@ -347,6 +378,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
device_prio_max = prio; device_prio_max = prio;
device_id_primary = worker_data.device_id[cur_gyro]; device_id_primary = worker_data.device_id[cur_gyro];
} }
} else { } else {
calibration_log_critical(mavlink_log_pub, "Gyro #%u no device id, abort", cur_gyro); calibration_log_critical(mavlink_log_pub, "Gyro #%u no device id, abort", cur_gyro);
} }
@@ -393,6 +425,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
res = PX4_OK; res = PX4_OK;
} }
} }
try_count++; try_count++;
} while (res == PX4_ERROR && try_count <= max_tries); } while (res == PX4_ERROR && try_count <= max_tries);
@@ -424,6 +457,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
/* check if thermal compensation is enabled */ /* check if thermal compensation is enabled */
int32_t tc_enabled_int; int32_t tc_enabled_int;
param_get(param_find("TC_G_ENABLE"), &(tc_enabled_int)); param_get(param_find("TC_G_ENABLE"), &(tc_enabled_int));
if (tc_enabled_int == 1) { if (tc_enabled_int == 1) {
/* Get struct containing sensor thermal compensation data */ /* Get struct containing sensor thermal compensation data */
struct sensor_correction_s sensor_correction; /**< sensor thermal corrections */ struct sensor_correction_s sensor_correction; /**< sensor thermal corrections */
@@ -437,11 +471,13 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
/* update the _X0_ terms to include the additional offset */ /* update the _X0_ terms to include the additional offset */
int32_t handle; int32_t handle;
float val; float val;
for (unsigned axis_index = 0; axis_index < 3; axis_index++) { for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
val = 0.0f; val = 0.0f;
(void)sprintf(str, "TC_G%u_X0_%u", sensor_correction.gyro_mapping[uorb_index], axis_index); (void)sprintf(str, "TC_G%u_X0_%u", sensor_correction.gyro_mapping[uorb_index], axis_index);
handle = param_find(str); handle = param_find(str);
param_get(handle, &val); param_get(handle, &val);
if (axis_index == 0) { if (axis_index == 0) {
val += worker_data.gyro_scale[uorb_index].x_offset; val += worker_data.gyro_scale[uorb_index].x_offset;
@@ -452,8 +488,10 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
val += worker_data.gyro_scale[uorb_index].z_offset; val += worker_data.gyro_scale[uorb_index].z_offset;
} }
failed |= (PX4_OK != param_set_no_notification(handle, &val)); failed |= (PX4_OK != param_set_no_notification(handle, &val));
} }
param_notify_changes(); param_notify_changes();
} }
@@ -489,6 +527,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
if (res != PX4_OK) { if (res != PX4_OK) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG, 1); calibration_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG, 1);
} }
#endif #endif
} }
} }
@@ -504,6 +543,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
if (res == PX4_OK) { if (res == PX4_OK) {
calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name); calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);
} else { } else {
calibration_log_info(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name); calibration_log_info(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name);
} }
src/modules/commander/mag_calibration.cpp
+12 -1
View File
@@ -118,16 +118,21 @@ int do_mag_calibration(orb_advert_t *mavlink_log_pub)
// sensor calibration and will be invalidated by a new sensor calibration // sensor calibration and will be invalidated by a new sensor calibration
(void)sprintf(str, "EKF2_MAGBIAS_X"); (void)sprintf(str, "EKF2_MAGBIAS_X");
result = param_set_no_notification(param_find(str), &mscale_null.x_offset); result = param_set_no_notification(param_find(str), &mscale_null.x_offset);
if (result != PX4_OK) { if (result != PX4_OK) {
PX4_ERR("unable to reset %s", str); PX4_ERR("unable to reset %s", str);
} }
(void)sprintf(str, "EKF2_MAGBIAS_Y"); (void)sprintf(str, "EKF2_MAGBIAS_Y");
result = param_set_no_notification(param_find(str), &mscale_null.y_offset); result = param_set_no_notification(param_find(str), &mscale_null.y_offset);
if (result != PX4_OK) { if (result != PX4_OK) {
PX4_ERR("unable to reset %s", str); PX4_ERR("unable to reset %s", str);
} }
(void)sprintf(str, "EKF2_MAGBIAS_Z"); (void)sprintf(str, "EKF2_MAGBIAS_Z");
result = param_set_no_notification(param_find(str), &mscale_null.z_offset); result = param_set_no_notification(param_find(str), &mscale_null.z_offset);
if (result != PX4_OK) { if (result != PX4_OK) {
PX4_ERR("unable to reset %s", str); PX4_ERR("unable to reset %s", str);
} }
@@ -297,13 +302,15 @@ static calibrate_return check_calibration_result(float offset_x, float offset_y,
float must_be_finite[] = {offset_x, offset_y, offset_z, float must_be_finite[] = {offset_x, offset_y, offset_z,
sphere_radius, sphere_radius,
diag_x, diag_y, diag_z, diag_x, diag_y, diag_z,
offdiag_x, offdiag_y, offdiag_z}; offdiag_x, offdiag_y, offdiag_z
};
float should_be_not_huge[] = {offset_x, offset_y, offset_z}; float should_be_not_huge[] = {offset_x, offset_y, offset_z};
float should_be_positive[] = {sphere_radius, diag_x, diag_y, diag_z}; float should_be_positive[] = {sphere_radius, diag_x, diag_y, diag_z};
// Make sure every parameter is finite // Make sure every parameter is finite
const int num_finite = sizeof(must_be_finite) / sizeof(*must_be_finite); const int num_finite = sizeof(must_be_finite) / sizeof(*must_be_finite);
for (unsigned i = 0; i < num_finite; ++i) { for (unsigned i = 0; i < num_finite; ++i) {
if (!PX4_ISFINITE(must_be_finite[i])) { if (!PX4_ISFINITE(must_be_finite[i])) {
calibration_log_emergency(mavlink_log_pub, calibration_log_emergency(mavlink_log_pub,
@@ -314,6 +321,7 @@ static calibrate_return check_calibration_result(float offset_x, float offset_y,
// Notify if offsets are too large // Notify if offsets are too large
const int num_not_huge = sizeof(should_be_not_huge) / sizeof(*should_be_not_huge); const int num_not_huge = sizeof(should_be_not_huge) / sizeof(*should_be_not_huge);
for (unsigned i = 0; i < num_not_huge; ++i) { for (unsigned i = 0; i < num_not_huge; ++i) {
if (fabsf(should_be_not_huge[i]) > MAG_MAX_OFFSET_LEN) { if (fabsf(should_be_not_huge[i]) > MAG_MAX_OFFSET_LEN) {
calibration_log_critical(mavlink_log_pub, "Warning: %s mag with large offsets", calibration_log_critical(mavlink_log_pub, "Warning: %s mag with large offsets",
@@ -324,6 +332,7 @@ static calibrate_return check_calibration_result(float offset_x, float offset_y,
// Notify if a parameter which should be positive is non-positive // Notify if a parameter which should be positive is non-positive
const int num_positive = sizeof(should_be_positive) / sizeof(*should_be_positive); const int num_positive = sizeof(should_be_positive) / sizeof(*should_be_positive);
for (unsigned i = 0; i < num_positive; ++i) { for (unsigned i = 0; i < num_positive; ++i) {
if (should_be_positive[i] <= 0.0f) { if (should_be_positive[i] <= 0.0f) {
calibration_log_critical(mavlink_log_pub, "Warning: %s mag with non-positive scale", calibration_log_critical(mavlink_log_pub, "Warning: %s mag with non-positive scale",
@@ -597,6 +606,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
// Lock in to correct ORB instance // Lock in to correct ORB instance
bool found_cur_mag = false; bool found_cur_mag = false;
for (unsigned i = 0; i < orb_mag_count && !found_cur_mag; i++) { for (unsigned i = 0; i < orb_mag_count && !found_cur_mag; i++) {
worker_data.sub_mag[cur_mag] = orb_subscribe_multi(ORB_ID(sensor_mag), i); worker_data.sub_mag[cur_mag] = orb_subscribe_multi(ORB_ID(sensor_mag), i);
@@ -612,6 +622,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
if (report.device_id == (uint32_t)device_ids[cur_mag]) { if (report.device_id == (uint32_t)device_ids[cur_mag]) {
// Device IDs match, correct ORB instance for this mag // Device IDs match, correct ORB instance for this mag
found_cur_mag = true; found_cur_mag = true;
} else { } else {
orb_unsubscribe(worker_data.sub_mag[cur_mag]); orb_unsubscribe(worker_data.sub_mag[cur_mag]);
} }
src/modules/commander/state_machine_helper.cpp
+18 -8
View File
@@ -126,7 +126,8 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const safe
if (fRunPreArmChecks && (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) if (fRunPreArmChecks && (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)
&& !hil_enabled) { && !hil_enabled) {
preflight_check_ret = Preflight::preflightCheck(mavlink_log_pub, *status, *status_flags, checkGNSS, true, true, time_since_boot); preflight_check_ret = Preflight::preflightCheck(mavlink_log_pub, *status, *status_flags, checkGNSS, true, true,
time_since_boot);
if (preflight_check_ret) { if (preflight_check_ret) {
status_flags->condition_system_sensors_initialized = true; status_flags->condition_system_sensors_initialized = true;
@@ -138,12 +139,14 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const safe
/* re-run the pre-flight check as long as sensors are failing */ /* re-run the pre-flight check as long as sensors are failing */
if (!status_flags->condition_system_sensors_initialized if (!status_flags->condition_system_sensors_initialized
&& fRunPreArmChecks && fRunPreArmChecks
&& ((new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) || (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY)) && ((new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)
|| (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY))
&& !hil_enabled) { && !hil_enabled) {
if ((last_preflight_check == 0) || (hrt_elapsed_time(&last_preflight_check) > 1000 * 1000)) { if ((last_preflight_check == 0) || (hrt_elapsed_time(&last_preflight_check) > 1000 * 1000)) {
status_flags->condition_system_sensors_initialized = Preflight::preflightCheck(mavlink_log_pub, *status, *status_flags, checkGNSS, false, false, time_since_boot); status_flags->condition_system_sensors_initialized = Preflight::preflightCheck(mavlink_log_pub, *status, *status_flags,
checkGNSS, false, false, time_since_boot);
last_preflight_check = hrt_absolute_time(); last_preflight_check = hrt_absolute_time();
} }
@@ -205,7 +208,8 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const safe
// Finish up the state transition // Finish up the state transition
if (valid_transition) { if (valid_transition) {
armed->armed = (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED); armed->armed = (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED);
armed->ready_to_arm = (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) || (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY); armed->ready_to_arm = (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)
|| (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY);
ret = TRANSITION_CHANGED; ret = TRANSITION_CHANGED;
status->arming_state = new_arming_state; status->arming_state = new_arming_state;
@@ -972,11 +976,13 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta
break; break;
case LOW_BAT_ACTION::RETURN: case LOW_BAT_ACTION::RETURN:
// FALLTHROUGH // FALLTHROUGH
case LOW_BAT_ACTION::RETURN_OR_LAND: case LOW_BAT_ACTION::RETURN_OR_LAND:
// let us send the critical message even if already in RTL // let us send the critical message even if already in RTL
if (TRANSITION_DENIED != main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags, internal_state)) { if (TRANSITION_DENIED != main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags,
internal_state)) {
mavlink_log_critical(mavlink_log_pub, "%s, RETURNING", battery_critical); mavlink_log_critical(mavlink_log_pub, "%s, RETURNING", battery_critical);
} else { } else {
@@ -986,7 +992,8 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta
break; break;
case LOW_BAT_ACTION::LAND: case LOW_BAT_ACTION::LAND:
if (TRANSITION_DENIED != main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LAND, status_flags, internal_state)) { if (TRANSITION_DENIED != main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LAND, status_flags,
internal_state)) {
mavlink_log_critical(mavlink_log_pub, "%s, LANDING AT CURRENT POSITION", battery_critical); mavlink_log_critical(mavlink_log_pub, "%s, LANDING AT CURRENT POSITION", battery_critical);
} else { } else {
@@ -1008,7 +1015,8 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta
break; break;
case LOW_BAT_ACTION::RETURN: case LOW_BAT_ACTION::RETURN:
if (TRANSITION_DENIED != main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags, internal_state)) { if (TRANSITION_DENIED != main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags,
internal_state)) {
mavlink_log_emergency(mavlink_log_pub, "%s, RETURNING", battery_dangerous); mavlink_log_emergency(mavlink_log_pub, "%s, RETURNING", battery_dangerous);
} else { } else {
@@ -1018,9 +1026,11 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta
break; break;
case LOW_BAT_ACTION::RETURN_OR_LAND: case LOW_BAT_ACTION::RETURN_OR_LAND:
// FALLTHROUGH // FALLTHROUGH
case LOW_BAT_ACTION::LAND: case LOW_BAT_ACTION::LAND:
if (TRANSITION_DENIED != main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LAND, status_flags, internal_state)) { if (TRANSITION_DENIED != main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LAND, status_flags,
internal_state)) {
mavlink_log_emergency(mavlink_log_pub, "%s, LANDING IMMEDIATELY", battery_dangerous); mavlink_log_emergency(mavlink_log_pub, "%s, LANDING IMMEDIATELY", battery_dangerous);
} else { } else {