fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-24 05:43:34 +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
+203 -114
View File
@@ -109,17 +109,16 @@
#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. | */ VEHICLE_MODE_FLAG_GUIDED_ENABLED = 8, /* 0b00001000 guided mode enabled, system flies MISSIONs / mission items. | */
VEHICLE_MODE_FLAG_GUIDED_ENABLED=8, /* 0b00001000 guided mode enabled, system flies MISSIONs / mission items. | */ VEHICLE_MODE_FLAG_STABILIZE_ENABLED = 16, /* 0b00010000 system stabilizes electronically its attitude (and optionally position). It needs however further control inputs to move around. | */
VEHICLE_MODE_FLAG_STABILIZE_ENABLED=16, /* 0b00010000 system stabilizes electronically its attitude (and optionally position). It needs however further control inputs to move around. | */ VEHICLE_MODE_FLAG_HIL_ENABLED = 32, /* 0b00100000 hardware in the loop simulation. All motors / actuators are blocked, but internal software is full operational. | */
VEHICLE_MODE_FLAG_HIL_ENABLED=32, /* 0b00100000 hardware in the loop simulation. All motors / actuators are blocked, but internal software is full operational. | */ VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, /* 0b01000000 remote control input is enabled. | */
VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED=64, /* 0b01000000 remote control input is enabled. | */ VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, /* 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. | */
VEHICLE_MODE_FLAG_SAFETY_ARMED=128, /* 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. | */ VEHICLE_MODE_FLAG_ENUM_END = 129, /* | */
VEHICLE_MODE_FLAG_ENUM_END=129, /* | */
} VEHICLE_MODE_FLAG; } VEHICLE_MODE_FLAG;
/* Decouple update interval and hysteresis counters, all depends on intervals */ /* Decouple update interval and hysteresis counters, all depends on intervals */
@@ -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();
@@ -232,26 +233,31 @@ static int power_button_state_notification_cb(board_power_button_state_notificat
button_state.timestamp = hrt_absolute_time(); button_state.timestamp = hrt_absolute_time();
int ret = PWR_BUTTON_RESPONSE_SHUT_DOWN_PENDING; int ret = PWR_BUTTON_RESPONSE_SHUT_DOWN_PENDING;
switch(request) { switch (request) {
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:
button_state.event = power_button_state_s::PWR_BUTTON_STATE_DOWN; case PWR_BUTTON_DOWN:
break; button_state.event = power_button_state_s::PWR_BUTTON_STATE_DOWN;
case PWR_BUTTON_UP: break;
button_state.event = power_button_state_s::PWR_BUTTON_STATE_UP;
break; case PWR_BUTTON_UP:
case PWR_BUTTON_REQUEST_SHUT_DOWN: button_state.event = power_button_state_s::PWR_BUTTON_STATE_UP;
button_state.event = power_button_state_s::PWR_BUTTON_STATE_REQUEST_SHUTDOWN; break;
break;
default: case PWR_BUTTON_REQUEST_SHUT_DOWN:
PX4_ERR("unhandled power button state: %i", (int)request); button_state.event = power_button_state_s::PWR_BUTTON_STATE_REQUEST_SHUTDOWN;
return ret; break;
default:
PX4_ERR("unhandled power button state: %i", (int)request);
return ret;
} }
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 {
@@ -479,7 +514,7 @@ int commander_main(int argc, char *argv[])
} }
bool ret = send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION, bool ret = send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION,
strcmp(argv[2], "off") ? 2.0f : 0.0f /* lockdown */, 0.0f); strcmp(argv[2], "off") ? 2.0f : 0.0f /* lockdown */, 0.0f);
return (ret ? 0 : 1); return (ret ? 0 : 1);
} }
@@ -541,8 +576,8 @@ Commander::~Commander()
} }
bool bool
Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_s& cmd, actuator_armed_s *armed_local, Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_s &cmd, actuator_armed_s *armed_local,
orb_advert_t *command_ack_pub, bool *changed) orb_advert_t *command_ack_pub, bool *changed)
{ {
/* only handle commands that are meant to be handled by this system and component */ /* only handle commands that are meant to be handled by this system and component */
if (cmd.target_system != status_local->system_id || ((cmd.target_component != status_local->component_id) if (cmd.target_system != status_local->system_id || ((cmd.target_component != status_local->component_id)
@@ -557,27 +592,30 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
switch (cmd.command) { switch (cmd.command) {
case vehicle_command_s::VEHICLE_CMD_DO_REPOSITION: { case vehicle_command_s::VEHICLE_CMD_DO_REPOSITION: {
// Just switch the flight mode here, the navigator takes care of // Just switch the flight mode here, the navigator takes care of
// doing something sensible with the coordinates. Its designed // doing something sensible with the coordinates. Its designed
// to not require navigator and command to receive / process // to not require navigator and command to receive / process
// the data at the exact same time. // the data at the exact same time.
// 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;
} else {
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
mavlink_log_critical(&mavlink_log_pub, "Rejecting reposition command");
}
} else { } else {
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
mavlink_log_critical(&mavlink_log_pub, "Rejecting reposition command");
} }
} else {
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;
@@ -933,31 +985,36 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
case vehicle_command_s::VEHICLE_CMD_MISSION_START: { case vehicle_command_s::VEHICLE_CMD_MISSION_START: {
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED; cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
// check if current mission and first item are valid // check if current mission and first item are valid
if (status_flags.condition_auto_mission_available) { if (status_flags.condition_auto_mission_available) {
// requested first mission item valid // requested first mission item valid
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,
&& (TRANSITION_DENIED != arm_disarm(true, &mavlink_log_pub, "mission start command"))) { &internal_state))
&& (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 {
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } else {
mavlink_log_critical(&mavlink_log_pub, "Mission start denied"); cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
mavlink_log_critical(&mavlink_log_pub, "Mission start denied");
}
} }
} else {
mavlink_log_critical(&mavlink_log_pub, "Mission start denied, no valid mission");
} }
} else {
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,12 +1767,13 @@ 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);
status_changed = true; status_changed = true;
} }
} while(updated); } while (updated);
/* If in INIT state, try to proceed to STANDBY state */ /* If in INIT state, try to proceed to STANDBY state */
if (!status_flags.condition_calibration_enabled && status.arming_state == vehicle_status_s::ARMING_STATE_INIT) { if (!status_flags.condition_calibration_enabled && status.arming_state == vehicle_status_s::ARMING_STATE_INIT) {
@@ -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;
@@ -1794,29 +1854,35 @@ Commander::run()
last_geofence_violation = hrt_absolute_time(); last_geofence_violation = hrt_absolute_time();
switch (geofence_result.geofence_action) { switch (geofence_result.geofence_action) {
case (geofence_result_s::GF_ACTION_NONE) : { case (geofence_result_s::GF_ACTION_NONE) : {
// 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) : {
if (TRANSITION_CHANGED == main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LOITER, status_flags, &internal_state)) { 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)) {
geofence_loiter_on = true; geofence_loiter_on = true;
} }
break; break;
} }
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)) { 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)) {
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");
armed.force_failsafe = true; armed.force_failsafe = true;
@@ -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;
} }
} }
@@ -1935,7 +2004,7 @@ Commander::run()
* do it only for rotary wings in manual mode or fixed wing if landed. * do it only for rotary wings in manual mode or fixed wing if landed.
* Disable stick-disarming if arming switch or button is mapped */ * Disable stick-disarming if arming switch or button is mapped */
const bool stick_in_lower_left = sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f const bool stick_in_lower_left = sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f
&& !arm_switch_or_button_mapped; && !arm_switch_or_button_mapped;
const bool arm_switch_to_disarm_transition = arm_switch_is_button == 0 && const bool arm_switch_to_disarm_transition = arm_switch_is_button == 0 &&
_last_sp_man_arm_switch == manual_control_setpoint_s::SWITCH_POS_ON && _last_sp_man_arm_switch == manual_control_setpoint_s::SWITCH_POS_ON &&
sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_OFF; sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_OFF;
@@ -1970,7 +2039,7 @@ Commander::run()
* and we're in MANUAL mode. * and we're in MANUAL mode.
* Disable stick-arming if arming switch or button is mapped */ * Disable stick-arming if arming switch or button is mapped */
const bool stick_in_lower_right = sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f const bool stick_in_lower_right = sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f
&& !arm_switch_or_button_mapped; && !arm_switch_or_button_mapped;
/* allow a grace period for re-arming: preflight checks don't need to pass during that time, /* allow a grace period for re-arming: preflight checks don't need to pass during that time,
* for example for accidential in-air disarming */ * for example for accidential in-air disarming */
const bool in_arming_grace_period = last_disarmed_timestamp != 0 && hrt_elapsed_time(&last_disarmed_timestamp) < 5_s; const bool in_arming_grace_period = last_disarmed_timestamp != 0 && hrt_elapsed_time(&last_disarmed_timestamp) < 5_s;
@@ -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);
@@ -2614,7 +2687,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
} }
} else if (actuator_armed->ready_to_arm) { } else if (actuator_armed->ready_to_arm) {
BOARD_ARMED_LED_OFF(); BOARD_ARMED_LED_OFF();
/* ready to arm, blink at 1Hz */ /* ready to arm, blink at 1Hz */
if (leds_counter % 20 == 0) { if (leds_counter % 20 == 0) {
@@ -2622,7 +2695,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
} }
} else { } else {
BOARD_ARMED_LED_OFF(); BOARD_ARMED_LED_OFF();
/* not ready to arm, blink at 10Hz */ /* not ready to arm, blink at 10Hz */
if (leds_counter % 2 == 0) { if (leds_counter % 2 == 0) {
@@ -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,11 +4051,12 @@ 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)
&& battery.connected && battery.connected
&& (_battery_warning == battery_status_s::BATTERY_WARNING_NONE)) { && (_battery_warning == battery_status_s::BATTERY_WARNING_NONE)) {
status_flags.condition_battery_healthy = true; status_flags.condition_battery_healthy = true;
@@ -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());
} }
} }
@@ -4028,10 +4111,11 @@ void Commander::estimator_check(bool *status_changed)
const bool mag_fault_prev = (_estimator_status_sub.get().control_mode_flags & (1 << estimator_status_s::CS_MAG_FAULT)); const bool mag_fault_prev = (_estimator_status_sub.get().control_mode_flags & (1 << estimator_status_s::CS_MAG_FAULT));
if (_estimator_status_sub.update()) { if (_estimator_status_sub.update()) {
const estimator_status_s& estimator_status = _estimator_status_sub.get(); const estimator_status_s &estimator_status = _estimator_status_sub.get();
// 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");
} }
@@ -4039,8 +4123,8 @@ void Commander::estimator_check(bool *status_changed)
// Set the allowable position uncertainty based on combination of flight and estimator state // Set the allowable position uncertainty based on combination of flight and estimator state
// When we are in a operator demanded position control mode and are solely reliant on optical flow, do not check position error because it will gradually increase throughout flight and the operator will compensate for the drift // When we are in a operator demanded position control mode and are solely reliant on optical flow, do not check position error because it will gradually increase throughout flight and the operator will compensate for the drift
const bool reliant_on_opt_flow = ((estimator_status.control_mode_flags & (1 << estimator_status_s::CS_OPT_FLOW)) const bool reliant_on_opt_flow = ((estimator_status.control_mode_flags & (1 << estimator_status_s::CS_OPT_FLOW))
&& !(estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GPS)) && !(estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GPS))
&& !(estimator_status.control_mode_flags & (1 << estimator_status_s::CS_EV_POS))); && !(estimator_status.control_mode_flags & (1 << estimator_status_s::CS_EV_POS)));
const bool operator_controlled_position = (internal_state.main_state == commander_state_s::MAIN_STATE_POSCTL); const bool operator_controlled_position = (internal_state.main_state == commander_state_s::MAIN_STATE_POSCTL);
@@ -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
+97 -35
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;
} }
@@ -282,9 +303,9 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
/* handle individual sensors, one by one */ /* handle individual sensors, one by one */
matrix::Vector3f accel_offs_vec(accel_offs[uorb_index]); matrix::Vector3f accel_offs_vec(accel_offs[uorb_index]);
matrix::Vector3f accel_offs_rotated = board_rotation_t * accel_offs_vec; matrix::Vector3f accel_offs_rotated = board_rotation_t *accel_offs_vec;
matrix::Matrix3f accel_T_mat(accel_T[uorb_index]); matrix::Matrix3f accel_T_mat(accel_T[uorb_index]);
matrix::Matrix3f accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation; matrix::Matrix3f accel_T_rotated = board_rotation_t *accel_T_mat * board_rotation;
accel_scale.x_offset = accel_offs_rotated(0); accel_scale.x_offset = accel_offs_rotated(0);
accel_scale.x_scale = accel_T_rotated(0, 0); accel_scale.x_scale = accel_T_rotated(0, 0);
@@ -299,17 +320,18 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
PX4_INFO("found offset %d: x: %.6f, y: %.6f, z: %.6f", uorb_index, PX4_INFO("found offset %d: x: %.6f, y: %.6f, z: %.6f", uorb_index,
(double)accel_scale.x_offset, (double)accel_scale.x_offset,
(double)accel_scale.y_offset, (double)accel_scale.y_offset,
(double)accel_scale.z_offset); (double)accel_scale.z_offset);
PX4_INFO("found scale %d: x: %.6f, y: %.6f, z: %.6f", uorb_index, PX4_INFO("found scale %d: x: %.6f, y: %.6f, z: %.6f", uorb_index,
(double)accel_scale.x_scale, (double)accel_scale.x_scale,
(double)accel_scale.y_scale, (double)accel_scale.y_scale,
(double)accel_scale.z_scale); (double)accel_scale.z_scale);
/* 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
} }
@@ -421,19 +455,22 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
return res; return res;
} }
static calibrate_return accel_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data) static calibrate_return accel_calibration_worker(detect_orientation_return orientation, int cancel_sub, void *data)
{ {
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]",
(double)worker_data->accel_ref[0][orientation][0], detect_orientation_str(orientation),
(double)worker_data->accel_ref[0][orientation][1], (double)worker_data->accel_ref[0][orientation][0],
(double)worker_data->accel_ref[0][orientation][2]); (double)worker_data->accel_ref[0][orientation][1],
(double)worker_data->accel_ref[0][orientation][2]);
worker_data->done_count++; worker_data->done_count++;
calibration_log_info(worker_data->mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 17 * worker_data->done_count); calibration_log_info(worker_data->mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 17 * worker_data->done_count);
@@ -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;
@@ -458,7 +496,7 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub
worker_data.sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction)); worker_data.sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction));
// Initialize subs to error condition so we know which ones are open and which are not // Initialize subs to error condition so we know which ones are open and which are not
for (size_t i=0; i<max_accel_sens; i++) { for (size_t i = 0; i < max_accel_sens; i++) {
worker_data.subs[i] = -1; worker_data.subs[i] = -1;
} }
@@ -469,14 +507,16 @@ 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);
sensor_accel_s report = {}; sensor_accel_s report = {};
@@ -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]);
} }
@@ -506,7 +547,7 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub
#endif #endif
} }
if(!found_cur_accel) { if (!found_cur_accel) {
calibration_log_critical(mavlink_log_pub, "Accel #%u (ID %u) no matching uORB devid", cur_accel, device_id[cur_accel]); calibration_log_critical(mavlink_log_pub, "Accel #%u (ID %u) no matching uORB devid", cur_accel, device_id[cur_accel]);
result = calibrate_return_error; result = calibrate_return_error;
break; break;
@@ -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;
@@ -762,7 +816,7 @@ int do_level_calibration(orb_advert_t *mavlink_log_pub) {
param_get(board_rot_handle, &board_rot_current); param_get(board_rot_handle, &board_rot_current);
// give attitude some time to settle if there have been changes to the board rotation parameters // give attitude some time to settle if there have been changes to the board rotation parameters
if (board_rot_current == 0 && fabsf(roll_offset_current) < FLT_EPSILON && fabsf(pitch_offset_current) < FLT_EPSILON ) { if (board_rot_current == 0 && fabsf(roll_offset_current) < FLT_EPSILON && fabsf(pitch_offset_current) < FLT_EPSILON) {
settle_time = 0; settle_time = 0;
} }
@@ -782,14 +836,17 @@ 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) {
calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, (int)(90*hrt_elapsed_time(&start)/1e6f/(float)settle_time)); 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));
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);
if (pollret <= 0) { if (pollret <= 0) {
@@ -808,19 +865,22 @@ int do_level_calibration(orb_advert_t *mavlink_log_pub) {
calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 100); calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 100);
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;
goto out; goto out;
} }
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
+26 -6
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,12 +130,13 @@ 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;
} }
return state == ARM_AUTH_MISSION_APPROVED ? return state == ARM_AUTH_MISSION_APPROVED ?
vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED : vehicle_command_ack_s::VEHICLE_RESULT_DENIED; vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED : vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
} }
static uint8_t _auth_method_two_arm_check() static uint8_t _auth_method_two_arm_check()
@@ -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;
} }
@@ -174,7 +181,7 @@ uint8_t arm_auth_check()
void arm_auth_update(hrt_abstime now, bool param_update) void arm_auth_update(hrt_abstime now, bool param_update)
{ {
if (param_update) { if (param_update) {
param_get(param_arm_parameters, (int32_t*)&arm_parameters); param_get(param_arm_parameters, (int32_t *)&arm_parameters);
} }
switch (state) { switch (state) {
@@ -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,50 +209,60 @@ 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);
} }
if (updated if (updated
&& command_ack.command == vehicle_command_s::VEHICLE_CMD_ARM_AUTHORIZATION_REQUEST && command_ack.command == vehicle_command_s::VEHICLE_CMD_ARM_AUTHORIZATION_REQUEST
&& command_ack.target_system == *system_id) { && command_ack.target_system == *system_id) {
switch (command_ack.result) { switch (command_ack.result) {
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",
command_ack.result_param2); command_ack.result_param2);
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
+9 -6
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;
} }
@@ -847,12 +850,12 @@ bool calibrate_cancel_check(orb_advert_t *mavlink_log_pub, int cancel_sub)
// ignore internal commands, such as VEHICLE_CMD_DO_MOUNT_CONTROL from vmount // ignore internal commands, such as VEHICLE_CMD_DO_MOUNT_CONTROL from vmount
if (cmd.from_external) { if (cmd.from_external) {
if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION && if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION &&
(int)cmd.param1 == 0 && (int)cmd.param1 == 0 &&
(int)cmd.param2 == 0 && (int)cmd.param2 == 0 &&
(int)cmd.param3 == 0 && (int)cmd.param3 == 0 &&
(int)cmd.param4 == 0 && (int)cmd.param4 == 0 &&
(int)cmd.param5 == 0 && (int)cmd.param5 == 0 &&
(int)cmd.param6 == 0) { (int)cmd.param6 == 0) {
calibrate_answer_command(mavlink_log_pub, cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); calibrate_answer_command(mavlink_log_pub, cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
mavlink_log_critical(mavlink_log_pub, CAL_QGC_CANCELLED_MSG); mavlink_log_critical(mavlink_log_pub, CAL_QGC_CANCELLED_MSG);
return true; return true;
src/modules/commander/calibration_routines.h
+4 -4
View File
@@ -66,11 +66,11 @@ int run_lm_sphere_fit(const float x[], const float y[], const float z[], float &
float *sphere_radius, float *diag_x, float *diag_y, float *diag_z, float *offdiag_x, float *offdiag_y, float *sphere_radius, float *diag_x, float *diag_y, float *diag_z, float *offdiag_x, float *offdiag_y,
float *offdiag_z); float *offdiag_z);
int run_lm_ellipsoid_fit(const float x[], const float y[], const float z[], float &_fitness, float &_sphere_lambda, int run_lm_ellipsoid_fit(const float x[], const float y[], const float z[], float &_fitness, float &_sphere_lambda,
unsigned int size, float *offset_x, float *offset_y, float *offset_z, unsigned int size, float *offset_x, float *offset_y, float *offset_z,
float *sphere_radius, float *diag_x, float *diag_y, float *diag_z, float *offdiag_x, float *offdiag_y, float *sphere_radius, float *diag_x, float *diag_y, float *diag_z, float *offdiag_x, float *offdiag_y,
float *offdiag_z); float *offdiag_z);
bool inverse4x4(float m[], float invOut[]); bool inverse4x4(float m[], float invOut[]);
bool mat_inverse(float* A, float* inv, uint8_t n); bool mat_inverse(float *A, float *inv, uint8_t n);
// FIXME: Change the name // FIXME: Change the name
static const unsigned max_accel_sens = 3; static const unsigned max_accel_sens = 3;
src/modules/commander/commander_helper.cpp
+3 -2
View File
@@ -94,10 +94,11 @@ bool is_multirotor(const struct vehicle_status_s *current_status)
bool is_rotary_wing(const struct vehicle_status_s *current_status) bool is_rotary_wing(const struct vehicle_status_s *current_status)
{ {
return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER) return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER)
|| (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
+325 -225
View File
@@ -59,215 +59,261 @@ private:
bool StateMachineHelperTest::armingStateTransitionTest() bool StateMachineHelperTest::armingStateTransitionTest()
{ {
// These are the critical values from vehicle_status_s and actuator_armed_s which must be primed // These are the critical values from vehicle_status_s and actuator_armed_s which must be primed
// to simulate machine state prior to testing an arming state transition. This structure is also // to simulate machine state prior to testing an arming state transition. This structure is also
// use to represent the expected machine state after the transition has been requested. // use to represent the expected machine state after the transition has been requested.
typedef struct { typedef struct {
arming_state_t arming_state; // vehicle_status_s.arming_state arming_state_t arming_state; // vehicle_status_s.arming_state
bool armed; // actuator_armed_s.armed bool armed; // actuator_armed_s.armed
bool ready_to_arm; // actuator_armed_s.ready_to_arm bool ready_to_arm; // actuator_armed_s.ready_to_arm
} ArmingTransitionVolatileState_t; } ArmingTransitionVolatileState_t;
// This structure represents a test case for arming_state_transition. It contains the machine // This structure represents a test case for arming_state_transition. It contains the machine
// state prior to transition, the requested state to transition to and finally the expected // state prior to transition, the requested state to transition to and finally the expected
// machine state after transition. // machine state after transition.
typedef struct { typedef struct {
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
bool safety_switch_available; // Current safety_s.safety_switch_available condition_system_sensors_initialized; // Current vehicle_status_s.condition_system_sensors_initialized
bool safety_off; // Current safety_s.safety_off bool safety_switch_available; // Current safety_s.safety_switch_available
arming_state_t requested_state; // Requested arming state to transition to bool safety_off; // Current safety_s.safety_off
ArmingTransitionVolatileState_t expected_state; // Expected machine state after transition arming_state_t requested_state; // Requested arming state to transition to
transition_result_t expected_transition_result; // Expected result from arming_state_transition ArmingTransitionVolatileState_t expected_state; // Expected machine state after transition
} ArmingTransitionTest_t; transition_result_t expected_transition_result; // Expected result from arming_state_transition
} ArmingTransitionTest_t;
// We use these defines so that our test cases are more readable // We use these defines so that our test cases are more readable
#define ATT_ARMED true #define ATT_ARMED true
#define ATT_DISARMED false #define ATT_DISARMED false
#define ATT_READY_TO_ARM true #define ATT_READY_TO_ARM true
#define ATT_NOT_READY_TO_ARM false #define ATT_NOT_READY_TO_ARM false
#define ATT_SENSORS_INITIALIZED true #define ATT_SENSORS_INITIALIZED true
#define ATT_SENSORS_NOT_INITIALIZED false #define ATT_SENSORS_NOT_INITIALIZED false
#define ATT_SAFETY_AVAILABLE true #define ATT_SAFETY_AVAILABLE true
#define ATT_SAFETY_NOT_AVAILABLE true #define ATT_SAFETY_NOT_AVAILABLE true
#define ATT_SAFETY_OFF true #define ATT_SAFETY_OFF true
#define ATT_SAFETY_ON false #define ATT_SAFETY_ON false
// These are test cases for arming_state_transition // These are test cases for arming_state_transition
static const ArmingTransitionTest_t rgArmingTransitionTests[] = { static const ArmingTransitionTest_t rgArmingTransitionTests[] = {
// TRANSITION_NOT_CHANGED tests // TRANSITION_NOT_CHANGED tests
{ "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, "no transition: identical states",
vehicle_status_s::ARMING_STATE_INIT, { 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 }, TRANSITION_NOT_CHANGED }, vehicle_status_s::ARMING_STATE_INIT,
{ 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", {
{ 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, "transition: init to standby",
vehicle_status_s::ARMING_STATE_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_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, vehicle_status_s::ARMING_STATE_STANDBY,
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED
},
{ "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, "transition: init to standby error",
vehicle_status_s::ARMING_STATE_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_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, vehicle_status_s::ARMING_STATE_STANDBY_ERROR,
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED
},
{ "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, "transition: init to reboot",
vehicle_status_s::ARMING_STATE_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_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, vehicle_status_s::ARMING_STATE_REBOOT,
{ vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED
},
{ "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, "transition: standby to init",
vehicle_status_s::ARMING_STATE_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_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, vehicle_status_s::ARMING_STATE_INIT,
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED
},
{ "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, "transition: standby to standby error",
vehicle_status_s::ARMING_STATE_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_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, vehicle_status_s::ARMING_STATE_STANDBY_ERROR,
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED
},
{ "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, "transition: standby to reboot",
vehicle_status_s::ARMING_STATE_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_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, vehicle_status_s::ARMING_STATE_REBOOT,
{ vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED
},
{ "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, "transition: armed to standby",
vehicle_status_s::ARMING_STATE_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_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, vehicle_status_s::ARMING_STATE_STANDBY,
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED
},
{ "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, "transition: standby error to reboot",
vehicle_status_s::ARMING_STATE_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_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, vehicle_status_s::ARMING_STATE_REBOOT,
{ vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED
},
{ "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, "transition: in air restore to armed",
vehicle_status_s::ARMING_STATE_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_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, vehicle_status_s::ARMING_STATE_ARMED,
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED
},
{ "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, "transition: in air restore to reboot",
vehicle_status_s::ARMING_STATE_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_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, vehicle_status_s::ARMING_STATE_REBOOT,
{ 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", {
{ 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, "transition: standby error to standby, hil on",
vehicle_status_s::ARMING_STATE_STANDBY, { 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, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, vehicle_status_s::ARMING_STATE_STANDBY,
{ 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", {
{ 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, "transition: standby to armed, no safety switch",
vehicle_status_s::ARMING_STATE_ARMED, { 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, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, vehicle_status_s::ARMING_STATE_ARMED,
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED
},
{ "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, "transition: standby to armed, safety switch off",
vehicle_status_s::ARMING_STATE_ARMED, { 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, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, vehicle_status_s::ARMING_STATE_ARMED,
{ 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", {
{ 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, "no transition: init to armed",
vehicle_status_s::ARMING_STATE_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 }, TRANSITION_DENIED }, vehicle_status_s::ARMING_STATE_ARMED,
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED
},
{ "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, "no transition: armed to init",
vehicle_status_s::ARMING_STATE_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 }, TRANSITION_DENIED }, vehicle_status_s::ARMING_STATE_INIT,
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED
},
{ "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, "no transition: armed to reboot",
vehicle_status_s::ARMING_STATE_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 }, TRANSITION_DENIED }, vehicle_status_s::ARMING_STATE_REBOOT,
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED
},
{ "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, "no transition: standby error to armed",
vehicle_status_s::ARMING_STATE_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 }, TRANSITION_DENIED }, vehicle_status_s::ARMING_STATE_ARMED,
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED
},
{ "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, "no transition: standby error to standby",
vehicle_status_s::ARMING_STATE_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 }, TRANSITION_DENIED }, vehicle_status_s::ARMING_STATE_STANDBY,
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED
},
{ "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, "no transition: reboot to armed",
vehicle_status_s::ARMING_STATE_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 }, TRANSITION_DENIED }, vehicle_status_s::ARMING_STATE_ARMED,
{ vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED
},
{ "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, "no transition: in air restore to standby",
vehicle_status_s::ARMING_STATE_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 }, TRANSITION_DENIED }, vehicle_status_s::ARMING_STATE_STANDBY,
{ vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED
},
// Sensor tests // Sensor tests
//{ "transition to standby error: init to standby - sensors not initialized", //{ "transition to standby error: init to standby - sensors not initialized",
// { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_NOT_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_NOT_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 },
// Safety switch arming tests // Safety switch arming tests
{ "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, "no transition: init to standby, safety switch on",
vehicle_status_s::ARMING_STATE_ARMED, { 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 }, TRANSITION_DENIED }, vehicle_status_s::ARMING_STATE_ARMED,
}; { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED
},
};
struct vehicle_status_s status = {}; struct vehicle_status_s status = {};
struct vehicle_status_flags_s status_flags = {}; struct vehicle_status_flags_s status_flags = {};
struct safety_s safety = {}; struct safety_s safety = {};
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++) {
const ArmingTransitionTest_t* test = &rgArmingTransitionTests[i];
const bool check_gps = false; for (size_t i = 0; i < cArmingTransitionTests; i++) {
const ArmingTransitionTest_t *test = &rgArmingTransitionTests[i];
// Setup initial machine state const bool check_gps = false;
status.arming_state = test->current_state.arming_state;
status_flags.condition_system_sensors_initialized = test->condition_system_sensors_initialized;
status.hil_state = test->hil_state;
// The power status of the test unit is not relevant for the unit test
status_flags.circuit_breaker_engaged_power_check = true;
safety.safety_switch_available = test->safety_switch_available;
safety.safety_off = test->safety_off;
armed.armed = test->current_state.armed;
armed.ready_to_arm = test->current_state.ready_to_arm;
// Attempt transition // Setup initial machine state
transition_result_t result = arming_state_transition(&status, safety, test->requested_state, &armed, status.arming_state = test->current_state.arming_state;
false /* no pre-arm checks */, status_flags.condition_system_sensors_initialized = test->condition_system_sensors_initialized;
nullptr /* no mavlink_log_pub */, status.hil_state = test->hil_state;
&status_flags, // The power status of the test unit is not relevant for the unit test
(check_gps ? ARM_REQ_GPS_BIT : 0), status_flags.circuit_breaker_engaged_power_check = true;
2e6 /* 2 seconds after boot, everything should be checked */ safety.safety_switch_available = test->safety_switch_available;
); safety.safety_off = test->safety_off;
armed.armed = test->current_state.armed;
armed.ready_to_arm = test->current_state.ready_to_arm;
// Validate result of transition // Attempt transition
ut_compare(test->assertMsg, test->expected_transition_result, result); transition_result_t result = arming_state_transition(&status, safety, test->requested_state, &armed,
ut_compare(test->assertMsg, status.arming_state, test->expected_state.arming_state); false /* no pre-arm checks */,
ut_compare(test->assertMsg, armed.armed, test->expected_state.armed); nullptr /* no mavlink_log_pub */,
ut_compare(test->assertMsg, armed.ready_to_arm, test->expected_state.ready_to_arm); &status_flags,
} (check_gps ? ARM_REQ_GPS_BIT : 0),
2e6 /* 2 seconds after boot, everything should be checked */
);
// Validate result of transition
ut_compare(test->assertMsg, test->expected_transition_result, result);
ut_compare(test->assertMsg, status.arming_state, test->expected_state.arming_state);
ut_compare(test->assertMsg, armed.armed, test->expected_state.armed);
ut_compare(test->assertMsg, armed.ready_to_arm, test->expected_state.ready_to_arm);
}
return true; return true;
} }
@@ -276,7 +322,7 @@ bool StateMachineHelperTest::mainStateTransitionTest()
{ {
// This structure represent a single test case for testing Main State transitions. // This structure represent a single test case for testing Main State transitions.
typedef struct { typedef struct {
const char* assertMsg; // Text to show when test case fails const char *assertMsg; // Text to show when test case fails
uint8_t condition_bits; // Bits for various condition_* values uint8_t condition_bits; // Bits for various condition_* values
uint8_t from_state; // State prior to transition request uint8_t from_state; // State prior to transition request
main_state_t to_state; // State to transition to main_state_t to_state; // State to transition to
@@ -284,125 +330,176 @@ bool StateMachineHelperTest::mainStateTransitionTest()
} MainTransitionTest_t; } MainTransitionTest_t;
// Bits for condition_bits // Bits for condition_bits
#define MTT_ALL_NOT_VALID 0 #define MTT_ALL_NOT_VALID 0
#define MTT_ROTARY_WING 1 << 0 #define MTT_ROTARY_WING 1 << 0
#define MTT_LOC_ALT_VALID 1 << 1 #define MTT_LOC_ALT_VALID 1 << 1
#define MTT_LOC_POS_VALID 1 << 2 #define MTT_LOC_POS_VALID 1 << 2
#define MTT_HOME_POS_VALID 1 << 3 #define MTT_HOME_POS_VALID 1 << 3
#define MTT_GLOBAL_POS_VALID 1 << 4 #define MTT_GLOBAL_POS_VALID 1 << 4
static const MainTransitionTest_t rgMainTransitionTests[] = { static const MainTransitionTest_t rgMainTransitionTests[] = {
// 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++) {
const MainTransitionTest_t* test = &rgMainTransitionTests[i]; for (size_t i = 0; i < cMainTransitionTests; i++) {
const MainTransitionTest_t *test = &rgMainTransitionTests[i];
// Setup initial machine state // Setup initial machine state
struct vehicle_status_s current_vehicle_status = {}; struct vehicle_status_s current_vehicle_status = {};
@@ -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
+16 -5
View File
@@ -63,21 +63,22 @@
using namespace time_literals; using namespace time_literals;
int do_esc_calibration(orb_advert_t *mavlink_log_pub, struct actuator_armed_s* armed) int do_esc_calibration(orb_advert_t *mavlink_log_pub, struct actuator_armed_s *armed)
{ {
int return_code = PX4_OK; int return_code = PX4_OK;
#if defined(__PX4_POSIX_OCPOC) || defined(__PX4_POSIX_BBBLUE) #if defined(__PX4_POSIX_OCPOC) || defined(__PX4_POSIX_BBBLUE)
hrt_abstime timeout_start = 0; hrt_abstime timeout_start = 0;
hrt_abstime timeout_wait = 60_s; hrt_abstime timeout_wait = 60_s;
armed->in_esc_calibration_mode = true; armed->in_esc_calibration_mode = true;
calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "begin esc"); calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "begin esc");
timeout_start = hrt_absolute_time(); timeout_start = hrt_absolute_time();
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);
} }
} }
@@ -88,7 +89,7 @@ int do_esc_calibration(orb_advert_t *mavlink_log_pub, struct actuator_armed_s* a
if (return_code == OK) { if (return_code == OK) {
calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "esc"); calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "esc");
} }
return return_code; return return_code;
#else #else
@@ -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/esc_calibration.h
+1 -1
View File
@@ -44,6 +44,6 @@
#include <uORB/topics/actuator_armed.h> #include <uORB/topics/actuator_armed.h>
int do_esc_calibration(orb_advert_t *mavlink_log_pub, struct actuator_armed_s* armed); int do_esc_calibration(orb_advert_t *mavlink_log_pub, struct actuator_armed_s *armed);
#endif #endif
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
+87 -47
View File
@@ -1,35 +1,35 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2013-2017 PX4 Development Team. All rights reserved. * Copyright (c) 2013-2017 PX4 Development Team. All rights reserved.
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
* are met: * are met:
* *
* 1. Redistributions of source code must retain the above copyright * 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer. * notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright * 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in * notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the * the documentation and/or other materials provided with the
* distribution. * distribution.
* 3. Neither the name PX4 nor the names of its contributors may be * 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software * used to endorse or promote products derived from this software
* without specific prior written permission. * without specific prior written permission.
* *
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE. * POSSIBILITY OF SUCH DAMAGE.
* *
****************************************************************************/ ****************************************************************************/
/** /**
* @file gyro_calibration.cpp * @file gyro_calibration.cpp
@@ -74,18 +74,20 @@ typedef struct {
sensor_gyro_s gyro_report_0; sensor_gyro_s gyro_report_0;
} gyro_worker_data_t; } gyro_worker_data_t;
static calibrate_return gyro_calibration_worker(int cancel_sub, void* data) static calibrate_return gyro_calibration_worker(int cancel_sub, void *data)
{ {
gyro_worker_data_t* worker_data = (gyro_worker_data_t*)(data); gyro_worker_data_t *worker_data = (gyro_worker_data_t *)(data);
unsigned calibration_counter[max_gyros] = { 0 }, slow_count = 0; unsigned calibration_counter[max_gyros] = { 0 }, slow_count = 0;
const unsigned calibration_count = 5000; const unsigned calibration_count = 5000;
sensor_gyro_s gyro_report; sensor_gyro_s gyro_report;
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;
@@ -165,7 +178,7 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data)
} }
// Maintain the sample count of the slowest sensor // Maintain the sample count of the slowest sensor
if (calibration_counter[s] && calibration_counter[s] < update_count) { if (calibration_counter[s] && calibration_counter[s] < update_count) {
update_count = calibration_counter[s]; update_count = calibration_counter[s];
} }
@@ -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,7 +332,8 @@ 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);
sensor_gyro_s report{}; sensor_gyro_s report{};
@@ -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]);
} }
@@ -332,8 +362,9 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
#endif #endif
} }
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
+27 -16
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);
} }
@@ -289,45 +294,49 @@ static unsigned progress_percentage(mag_worker_data_t *worker_data)
// Returns calibrate_return_error if any parameter is not finite // Returns calibrate_return_error if any parameter is not finite
// Logs if parameters are out of range // Logs if parameters are out of range
static calibrate_return check_calibration_result(float offset_x, float offset_y, float offset_z, static calibrate_return check_calibration_result(float offset_x, float offset_y, float offset_z,
float sphere_radius, float sphere_radius,
float diag_x, float diag_y, float diag_z, float diag_x, float diag_y, float diag_z,
float offdiag_x, float offdiag_y, float offdiag_z, float offdiag_x, float offdiag_y, float offdiag_z,
orb_advert_t *mavlink_log_pub, size_t cur_mag) orb_advert_t *mavlink_log_pub, size_t cur_mag)
{ {
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,
"ERROR: Retry calibration (sphere NaN, #%zu)", cur_mag); "ERROR: Retry calibration (sphere NaN, #%zu)", cur_mag);
return calibrate_return_error; return calibrate_return_error;
} }
} }
// 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",
(internal[cur_mag]) ? "autopilot, internal" : "GPS unit, external"); (internal[cur_mag]) ? "autopilot, internal" : "GPS unit, external");
break; break;
} }
} }
// 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",
(internal[cur_mag]) ? "autopilot, internal" : "GPS unit, external"); (internal[cur_mag]) ? "autopilot, internal" : "GPS unit, external");
break; break;
} }
} }
@@ -597,7 +606,8 @@ 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);
struct mag_report report; struct mag_report report;
@@ -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]);
} }
@@ -625,7 +636,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
#endif #endif
} }
if(!found_cur_mag) { if (!found_cur_mag) {
calibration_log_critical(mavlink_log_pub, "Mag #%u (ID %u) no matching uORB devid", cur_mag, device_ids[cur_mag]); calibration_log_critical(mavlink_log_pub, "Mag #%u (ID %u) no matching uORB devid", cur_mag, device_ids[cur_mag]);
result = calibrate_return_error; result = calibrate_return_error;
break; break;
@@ -724,10 +735,10 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
&offdiag_x[cur_mag], &offdiag_y[cur_mag], &offdiag_z[cur_mag]); &offdiag_x[cur_mag], &offdiag_y[cur_mag], &offdiag_z[cur_mag]);
result = check_calibration_result(sphere_x[cur_mag], sphere_y[cur_mag], sphere_z[cur_mag], result = check_calibration_result(sphere_x[cur_mag], sphere_y[cur_mag], sphere_z[cur_mag],
sphere_radius[cur_mag], sphere_radius[cur_mag],
diag_x[cur_mag], diag_y[cur_mag], diag_z[cur_mag], diag_x[cur_mag], diag_y[cur_mag], diag_z[cur_mag],
offdiag_x[cur_mag], offdiag_y[cur_mag], offdiag_z[cur_mag], offdiag_x[cur_mag], offdiag_y[cur_mag], offdiag_z[cur_mag],
mavlink_log_pub, cur_mag); mavlink_log_pub, cur_mag);
if (result == calibrate_return_error) { if (result == calibrate_return_error) {
break; break;
src/modules/commander/state_machine_helper.cpp
+22 -12
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;
@@ -137,13 +138,15 @@ 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)
&& !hil_enabled) { || (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY))
&& !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 {