forked from Archive/PX4-Autopilot
commander fix and enforce code style
This commit is contained in:
Daniel Agar
parent
91721f2060
commit
48d9484ceb
|
|
@ -13,7 +13,6 @@ exec find src platforms \
|
|||
-path src/lib/DriverFramework -prune -o \
|
||||
-path src/lib/ecl -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/systemlib/uthash -prune -o \
|
||||
-path src/modules/uavcan/libuavcan -prune -o \
|
||||
|
|
|
|||
|
|
@ -109,17 +109,16 @@
|
|||
#include <uORB/topics/vtol_vehicle_status.h>
|
||||
#include <uORB/uORB.h>
|
||||
|
||||
typedef enum VEHICLE_MODE_FLAG
|
||||
{
|
||||
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_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_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_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_ENUM_END=129, /* | */
|
||||
typedef enum VEHICLE_MODE_FLAG {
|
||||
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_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_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_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_ENUM_END = 129, /* | */
|
||||
} VEHICLE_MODE_FLAG;
|
||||
|
||||
/* 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 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;
|
||||
|
||||
/**
|
||||
|
|
@ -201,7 +201,8 @@ extern "C" __EXPORT int commander_main(int argc, char *argv[]);
|
|||
*/
|
||||
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();
|
||||
|
||||
|
|
@ -232,26 +233,31 @@ static int power_button_state_notification_cb(board_power_button_state_notificat
|
|||
button_state.timestamp = hrt_absolute_time();
|
||||
int ret = PWR_BUTTON_RESPONSE_SHUT_DOWN_PENDING;
|
||||
|
||||
switch(request) {
|
||||
case PWR_BUTTON_IDEL:
|
||||
button_state.event = power_button_state_s::PWR_BUTTON_STATE_IDEL;
|
||||
break;
|
||||
case PWR_BUTTON_DOWN:
|
||||
button_state.event = power_button_state_s::PWR_BUTTON_STATE_DOWN;
|
||||
break;
|
||||
case PWR_BUTTON_UP:
|
||||
button_state.event = power_button_state_s::PWR_BUTTON_STATE_UP;
|
||||
break;
|
||||
case PWR_BUTTON_REQUEST_SHUT_DOWN:
|
||||
button_state.event = power_button_state_s::PWR_BUTTON_STATE_REQUEST_SHUTDOWN;
|
||||
break;
|
||||
default:
|
||||
PX4_ERR("unhandled power button state: %i", (int)request);
|
||||
return ret;
|
||||
switch (request) {
|
||||
case PWR_BUTTON_IDEL:
|
||||
button_state.event = power_button_state_s::PWR_BUTTON_STATE_IDEL;
|
||||
break;
|
||||
|
||||
case PWR_BUTTON_DOWN:
|
||||
button_state.event = power_button_state_s::PWR_BUTTON_STATE_DOWN;
|
||||
break;
|
||||
|
||||
case PWR_BUTTON_UP:
|
||||
button_state.event = power_button_state_s::PWR_BUTTON_STATE_UP;
|
||||
break;
|
||||
|
||||
case PWR_BUTTON_REQUEST_SHUT_DOWN:
|
||||
button_state.event = power_button_state_s::PWR_BUTTON_STATE_REQUEST_SHUTDOWN;
|
||||
break;
|
||||
|
||||
default:
|
||||
PX4_ERR("unhandled power button state: %i", (int)request);
|
||||
return ret;
|
||||
}
|
||||
|
||||
if (power_button_state_pub != nullptr) {
|
||||
orb_publish(ORB_ID(power_button_state), power_button_state_pub, &button_state);
|
||||
|
||||
} else {
|
||||
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 i;
|
||||
|
||||
for (i = 0; i < max_wait_steps; i++) {
|
||||
usleep(max_wait_us / max_wait_steps);
|
||||
|
||||
if (thread_running) {
|
||||
break;
|
||||
}
|
||||
|
|
@ -342,18 +350,25 @@ int commander_main(int argc, char *argv[])
|
|||
if (!strcmp(argv[1], "calibrate")) {
|
||||
if (argc > 2) {
|
||||
int calib_ret = OK;
|
||||
|
||||
if (!strcmp(argv[2], "mag")) {
|
||||
calib_ret = do_mag_calibration(&mavlink_log_pub);
|
||||
|
||||
} else if (!strcmp(argv[2], "accel")) {
|
||||
calib_ret = do_accel_calibration(&mavlink_log_pub);
|
||||
|
||||
} else if (!strcmp(argv[2], "gyro")) {
|
||||
calib_ret = do_gyro_calibration(&mavlink_log_pub);
|
||||
|
||||
} else if (!strcmp(argv[2], "level")) {
|
||||
calib_ret = do_level_calibration(&mavlink_log_pub);
|
||||
|
||||
} else if (!strcmp(argv[2], "esc")) {
|
||||
calib_ret = do_esc_calibration(&mavlink_log_pub, &armed);
|
||||
|
||||
} else if (!strcmp(argv[2], "airspeed")) {
|
||||
calib_ret = do_airspeed_calibration(&mavlink_log_pub);
|
||||
|
||||
} else {
|
||||
PX4_ERR("argument %s unsupported.", argv[2]);
|
||||
}
|
||||
|
|
@ -361,9 +376,11 @@ int commander_main(int argc, char *argv[])
|
|||
if (calib_ret) {
|
||||
PX4_ERR("calibration failed, exiting.");
|
||||
return 1;
|
||||
|
||||
} else {
|
||||
return 0;
|
||||
}
|
||||
|
||||
} else {
|
||||
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")) {
|
||||
PX4_ERR("arming failed");
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
|
@ -390,6 +408,7 @@ int commander_main(int argc, char *argv[])
|
|||
if (TRANSITION_DENIED == arm_disarm(false, &mavlink_log_pub, "command line")) {
|
||||
PX4_ERR("rejected disarm");
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
|
@ -423,7 +442,8 @@ int commander_main(int argc, char *argv[])
|
|||
if (!strcmp(argv[1], "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);
|
||||
}
|
||||
|
|
@ -431,32 +451,46 @@ int commander_main(int argc, char *argv[])
|
|||
if (!strcmp(argv[1], "mode")) {
|
||||
if (argc > 2) {
|
||||
uint8_t new_main_state = commander_state_s::MAIN_STATE_MAX;
|
||||
|
||||
if (!strcmp(argv[2], "manual")) {
|
||||
new_main_state = commander_state_s::MAIN_STATE_MANUAL;
|
||||
|
||||
} else if (!strcmp(argv[2], "altctl")) {
|
||||
new_main_state = commander_state_s::MAIN_STATE_ALTCTL;
|
||||
|
||||
} else if (!strcmp(argv[2], "posctl")) {
|
||||
new_main_state = commander_state_s::MAIN_STATE_POSCTL;
|
||||
|
||||
} else if (!strcmp(argv[2], "auto:mission")) {
|
||||
new_main_state = commander_state_s::MAIN_STATE_AUTO_MISSION;
|
||||
|
||||
} else if (!strcmp(argv[2], "auto:loiter")) {
|
||||
new_main_state = commander_state_s::MAIN_STATE_AUTO_LOITER;
|
||||
|
||||
} else if (!strcmp(argv[2], "auto:rtl")) {
|
||||
new_main_state = commander_state_s::MAIN_STATE_AUTO_RTL;
|
||||
|
||||
} else if (!strcmp(argv[2], "acro")) {
|
||||
new_main_state = commander_state_s::MAIN_STATE_ACRO;
|
||||
|
||||
} else if (!strcmp(argv[2], "offboard")) {
|
||||
new_main_state = commander_state_s::MAIN_STATE_OFFBOARD;
|
||||
|
||||
} else if (!strcmp(argv[2], "stabilized")) {
|
||||
new_main_state = commander_state_s::MAIN_STATE_STAB;
|
||||
|
||||
} else if (!strcmp(argv[2], "rattitude")) {
|
||||
new_main_state = commander_state_s::MAIN_STATE_RATTITUDE;
|
||||
|
||||
} else if (!strcmp(argv[2], "auto:takeoff")) {
|
||||
new_main_state = commander_state_s::MAIN_STATE_AUTO_TAKEOFF;
|
||||
|
||||
} else if (!strcmp(argv[2], "auto:land")) {
|
||||
new_main_state = commander_state_s::MAIN_STATE_AUTO_LAND;
|
||||
|
||||
} else if (!strcmp(argv[2], "auto:precland")) {
|
||||
new_main_state = commander_state_s::MAIN_STATE_AUTO_PRECLAND;
|
||||
|
||||
} else {
|
||||
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)) {
|
||||
PX4_ERR("mode change failed");
|
||||
}
|
||||
|
||||
return 0;
|
||||
|
||||
} else {
|
||||
|
|
@ -479,7 +514,7 @@ int commander_main(int argc, char *argv[])
|
|||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
|
@ -541,8 +576,8 @@ Commander::~Commander()
|
|||
}
|
||||
|
||||
bool
|
||||
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)
|
||||
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)
|
||||
{
|
||||
/* 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)
|
||||
|
|
@ -557,27 +592,30 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
|
|||
switch (cmd.command) {
|
||||
case vehicle_command_s::VEHICLE_CMD_DO_REPOSITION: {
|
||||
|
||||
// Just switch the flight mode here, the navigator takes care of
|
||||
// doing something sensible with the coordinates. Its designed
|
||||
// to not require navigator and command to receive / process
|
||||
// the data at the exact same time.
|
||||
// Just switch the flight mode here, the navigator takes care of
|
||||
// doing something sensible with the coordinates. Its designed
|
||||
// to not require navigator and command to receive / process
|
||||
// the data at the exact same time.
|
||||
|
||||
// Check if a mode switch had been requested
|
||||
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);
|
||||
// Check if a mode switch had been requested
|
||||
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);
|
||||
|
||||
if ((main_ret != TRANSITION_DENIED)) {
|
||||
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
|
||||
if ((main_ret != TRANSITION_DENIED)) {
|
||||
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 {
|
||||
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
|
||||
mavlink_log_critical(&mavlink_log_pub, "Rejecting reposition command");
|
||||
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
|
||||
}
|
||||
} else {
|
||||
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
|
||||
}
|
||||
}
|
||||
break;
|
||||
break;
|
||||
|
||||
case vehicle_command_s::VEHICLE_CMD_DO_SET_MODE: {
|
||||
uint8_t base_mode = (uint8_t)cmd.param1;
|
||||
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 */
|
||||
if (custom_sub_mode > 0) {
|
||||
reset_posvel_validity(changed);
|
||||
switch(custom_sub_mode) {
|
||||
|
||||
switch (custom_sub_mode) {
|
||||
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;
|
||||
|
||||
case PX4_CUSTOM_SUB_MODE_AUTO_MISSION:
|
||||
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 {
|
||||
main_ret = TRANSITION_DENIED;
|
||||
}
|
||||
|
|
@ -629,7 +671,8 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
|
|||
break;
|
||||
|
||||
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;
|
||||
|
||||
case PX4_CUSTOM_SUB_MODE_AUTO_LAND:
|
||||
|
|
@ -637,11 +680,13 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
|
|||
break;
|
||||
|
||||
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;
|
||||
|
||||
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;
|
||||
|
||||
default:
|
||||
|
|
@ -651,7 +696,8 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
|
|||
}
|
||||
|
||||
} 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) {
|
||||
|
|
@ -676,7 +722,8 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
|
|||
/* use base mode */
|
||||
if (base_mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) {
|
||||
/* 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) {
|
||||
if (base_mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) {
|
||||
|
|
@ -724,7 +771,8 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
|
|||
|
||||
} else {
|
||||
// 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.");
|
||||
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
|
||||
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: {
|
||||
/* 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");
|
||||
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: {
|
||||
/* 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;
|
||||
|
||||
} 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;
|
||||
|
||||
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");
|
||||
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;
|
||||
|
||||
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");
|
||||
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: {
|
||||
|
||||
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
|
||||
if (status_flags.condition_auto_mission_available) {
|
||||
// check if current mission and first item are valid
|
||||
if (status_flags.condition_auto_mission_available) {
|
||||
|
||||
// requested first mission item valid
|
||||
if (PX4_ISFINITE(cmd.param1) && (cmd.param1 >= -1) && (cmd.param1 < _mission_result_sub.get().seq_total)) {
|
||||
// requested first mission item valid
|
||||
if (PX4_ISFINITE(cmd.param1) && (cmd.param1 >= -1) && (cmd.param1 < _mission_result_sub.get().seq_total)) {
|
||||
|
||||
// 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))
|
||||
&& (TRANSITION_DENIED != arm_disarm(true, &mavlink_log_pub, "mission start command"))) {
|
||||
// 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))
|
||||
&& (TRANSITION_DENIED != arm_disarm(true, &mavlink_log_pub, "mission start command"))) {
|
||||
|
||||
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, "Mission start denied");
|
||||
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, "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: {
|
||||
bool hl_exists = false;
|
||||
|
||||
for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
|
||||
if (_telemetry[i].high_latency) {
|
||||
hl_exists = true;
|
||||
|
|
@ -1670,6 +1727,7 @@ Commander::run()
|
|||
if (!have_taken_off_since_arming) {
|
||||
// pilot has ten seconds time to take off
|
||||
auto_disarm_hysteresis.set_hysteresis_time_from(false, 10_s);
|
||||
|
||||
} else {
|
||||
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*/
|
||||
do {
|
||||
orb_check(subsys_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(subsystem_info), subsys_sub, &info);
|
||||
set_health_flags(info.subsystem_type, info.present, info.enabled, info.ok, status);
|
||||
status_changed = true;
|
||||
}
|
||||
} while(updated);
|
||||
} while (updated);
|
||||
|
||||
/* 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) {
|
||||
|
|
@ -1736,7 +1795,8 @@ Commander::run()
|
|||
const mission_result_s &mission_result = _mission_result_sub.get();
|
||||
|
||||
// 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;
|
||||
|
||||
|
|
@ -1794,29 +1854,35 @@ Commander::run()
|
|||
last_geofence_violation = hrt_absolute_time();
|
||||
|
||||
switch (geofence_result.geofence_action) {
|
||||
case (geofence_result_s::GF_ACTION_NONE) : {
|
||||
case (geofence_result_s::GF_ACTION_NONE) : {
|
||||
// do nothing
|
||||
break;
|
||||
}
|
||||
case (geofence_result_s::GF_ACTION_WARN) : {
|
||||
|
||||
case (geofence_result_s::GF_ACTION_WARN) : {
|
||||
// do nothing, mavlink critical messages are sent by navigator
|
||||
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;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
case (geofence_result_s::GF_ACTION_TERMINATE) : {
|
||||
|
||||
case (geofence_result_s::GF_ACTION_TERMINATE) : {
|
||||
warnx("Flight termination because of geofence");
|
||||
mavlink_log_critical(&mavlink_log_pub, "Geofence violation: flight termination");
|
||||
armed.force_failsafe = true;
|
||||
|
|
@ -1911,13 +1977,16 @@ Commander::run()
|
|||
/* handle the case where RC signal was regained */
|
||||
if (!status_flags.rc_signal_found_once) {
|
||||
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;
|
||||
|
||||
} else {
|
||||
if (status.rc_signal_lost) {
|
||||
mavlink_log_info(&mavlink_log_pub, "MANUAL CONTROL REGAINED after %llums", 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);
|
||||
mavlink_log_info(&mavlink_log_pub, "MANUAL CONTROL REGAINED after %llums",
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
|
@ -1935,7 +2004,7 @@ Commander::run()
|
|||
* do it only for rotary wings in manual mode or fixed wing if landed.
|
||||
* 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
|
||||
&& !arm_switch_or_button_mapped;
|
||||
&& !arm_switch_or_button_mapped;
|
||||
const bool arm_switch_to_disarm_transition = arm_switch_is_button == 0 &&
|
||||
_last_sp_man_arm_switch == manual_control_setpoint_s::SWITCH_POS_ON &&
|
||||
sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_OFF;
|
||||
|
|
@ -1970,7 +2039,7 @@ Commander::run()
|
|||
* and we're in MANUAL mode.
|
||||
* 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
|
||||
&& !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,
|
||||
* 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;
|
||||
|
|
@ -2080,7 +2149,8 @@ Commander::run()
|
|||
}
|
||||
|
||||
// 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
|
||||
// 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_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_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_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);
|
||||
}
|
||||
|
||||
void
|
||||
Commander::check_valid(const hrt_abstime ×tamp, const hrt_abstime &timeout, const bool valid_in, bool *valid_out, bool *changed)
|
||||
Commander::check_valid(const hrt_abstime ×tamp, const hrt_abstime &timeout, const bool valid_in, bool *valid_out,
|
||||
bool *changed)
|
||||
{
|
||||
hrt_abstime t = hrt_absolute_time();
|
||||
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) {
|
||||
BOARD_ARMED_LED_OFF();
|
||||
BOARD_ARMED_LED_OFF();
|
||||
|
||||
/* ready to arm, blink at 1Hz */
|
||||
if (leds_counter % 20 == 0) {
|
||||
|
|
@ -2622,7 +2695,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
|
|||
}
|
||||
|
||||
} else {
|
||||
BOARD_ARMED_LED_OFF();
|
||||
BOARD_ARMED_LED_OFF();
|
||||
|
||||
/* not ready to arm, blink at 10Hz */
|
||||
if (leds_counter % 2 == 0) {
|
||||
|
|
@ -2659,7 +2732,8 @@ Commander::set_main_state(const vehicle_status_s &status_local, bool *changed)
|
|||
transition_result_t
|
||||
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);
|
||||
|
||||
return res;
|
||||
|
|
@ -3056,10 +3130,14 @@ Commander::reset_posvel_validity(bool *changed)
|
|||
|
||||
// recheck validity
|
||||
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
|
||||
|
|
@ -3075,7 +3153,8 @@ Commander::check_posvel_validity(const bool data_valid, const float data_accurac
|
|||
*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 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);
|
||||
|
||||
} 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.");
|
||||
//set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_MISSION, true, true, false, status); // TODO
|
||||
|
||||
} else {
|
||||
//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) {
|
||||
|
||||
battery_status_s battery = {};
|
||||
|
||||
if (orb_copy(ORB_ID(battery_status), _battery_sub, &battery) == PX4_OK) {
|
||||
|
||||
if ((hrt_elapsed_time(&battery.timestamp) < 5_s)
|
||||
&& battery.connected
|
||||
&& (_battery_warning == battery_status_s::BATTERY_WARNING_NONE)) {
|
||||
&& battery.connected
|
||||
&& (_battery_warning == battery_status_s::BATTERY_WARNING_NONE)) {
|
||||
|
||||
status_flags.condition_battery_healthy = true;
|
||||
|
||||
|
|
@ -3985,7 +4067,8 @@ void Commander::battery_status_check()
|
|||
// execute battery failsafe if the state has gotten worse
|
||||
if (armed.armed) {
|
||||
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));
|
||||
|
||||
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
|
||||
const bool mag_fault = (estimator_status.control_mode_flags & (1 << estimator_status_s::CS_MAG_FAULT));
|
||||
|
||||
if (!mag_fault_prev && mag_fault) {
|
||||
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
|
||||
// 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))
|
||||
&& !(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_GPS))
|
||||
&& !(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);
|
||||
|
||||
|
|
@ -4110,20 +4194,25 @@ void Commander::estimator_check(bool *status_changed)
|
|||
} else {
|
||||
if (!_skip_pos_accuracy_check) {
|
||||
// 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
|
||||
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.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);
|
||||
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.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.
|
||||
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);
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -474,7 +474,8 @@ static bool powerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
|
|||
success = false;
|
||||
|
||||
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) {
|
||||
|
|
@ -610,44 +611,59 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_s
|
|||
if (report_fail) {
|
||||
// Only report the first failure to avoid spamming
|
||||
const char *message = nullptr;
|
||||
|
||||
if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_GPS_FIX)) {
|
||||
message = "Preflight%s: GPS fix too low";
|
||||
|
||||
} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MIN_SAT_COUNT)) {
|
||||
message = "Preflight%s: not enough GPS Satellites";
|
||||
|
||||
} else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MIN_GDOP)) {
|
||||
message = "Preflight%s: GPS GDoP too low";
|
||||
|
||||
} 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";
|
||||
|
||||
} 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";
|
||||
|
||||
} 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";
|
||||
|
||||
} 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";
|
||||
|
||||
} 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";
|
||||
|
||||
} 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";
|
||||
|
||||
} 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";
|
||||
|
||||
} else {
|
||||
if (!ekf_gps_fusion) {
|
||||
// Likely cause unknown
|
||||
message = "Preflight%s: Estimator not using GPS";
|
||||
gps_present = false;
|
||||
|
||||
} else {
|
||||
// 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";
|
||||
}
|
||||
}
|
||||
|
||||
if (message) {
|
||||
if (enforce_gps_required) {
|
||||
mavlink_log_critical(mavlink_log_pub, message, " Fail");
|
||||
|
||||
} else {
|
||||
mavlink_log_warning(mavlink_log_pub, message, "");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
gps_success = false;
|
||||
|
||||
if (enforce_gps_required) {
|
||||
|
|
@ -690,7 +706,8 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
|
|||
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;
|
||||
|
||||
|
|
@ -902,7 +919,8 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status,
|
|||
} else {
|
||||
// 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;
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -155,10 +155,14 @@ static int32_t device_id[max_accel_sens];
|
|||
static int device_prio_max = 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 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 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 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]);
|
||||
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
|
||||
typedef struct {
|
||||
|
|
@ -208,37 +212,50 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
|
|||
if (res != PX4_OK) {
|
||||
calibration_log_critical(mavlink_log_pub, CAL_ERROR_RESET_CAL_MSG, s);
|
||||
}
|
||||
|
||||
#else
|
||||
(void)sprintf(str, "CAL_ACC%u_XOFF", s);
|
||||
res = param_set_no_notification(param_find(str), &accel_scale.x_offset);
|
||||
|
||||
if (res != PX4_OK) {
|
||||
PX4_ERR("unable to reset %s", str);
|
||||
}
|
||||
|
||||
(void)sprintf(str, "CAL_ACC%u_YOFF", s);
|
||||
res = param_set_no_notification(param_find(str), &accel_scale.y_offset);
|
||||
|
||||
if (res != PX4_OK) {
|
||||
PX4_ERR("unable to reset %s", str);
|
||||
}
|
||||
|
||||
(void)sprintf(str, "CAL_ACC%u_ZOFF", s);
|
||||
res = param_set_no_notification(param_find(str), &accel_scale.z_offset);
|
||||
|
||||
if (res != PX4_OK) {
|
||||
PX4_ERR("unable to reset %s", str);
|
||||
}
|
||||
|
||||
(void)sprintf(str, "CAL_ACC%u_XSCALE", s);
|
||||
res = param_set_no_notification(param_find(str), &accel_scale.x_scale);
|
||||
|
||||
if (res != PX4_OK) {
|
||||
PX4_ERR("unable to reset %s", str);
|
||||
}
|
||||
|
||||
(void)sprintf(str, "CAL_ACC%u_YSCALE", s);
|
||||
res = param_set_no_notification(param_find(str), &accel_scale.y_scale);
|
||||
|
||||
if (res != PX4_OK) {
|
||||
PX4_ERR("unable to reset %s", str);
|
||||
}
|
||||
|
||||
(void)sprintf(str, "CAL_ACC%u_ZSCALE", s);
|
||||
res = param_set_no_notification(param_find(str), &accel_scale.z_scale);
|
||||
|
||||
if (res != PX4_OK) {
|
||||
PX4_ERR("unable to reset %s", str);
|
||||
}
|
||||
|
||||
param_notify_changes();
|
||||
#endif
|
||||
}
|
||||
|
|
@ -250,11 +267,14 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
|
|||
/* measure and calculate offsets & scales */
|
||||
if (res == PX4_OK) {
|
||||
calibrate_return cal_return = do_accel_calibration_measurements(mavlink_log_pub, accel_offs, accel_T, &active_sensors);
|
||||
|
||||
if (cal_return == calibrate_return_cancelled) {
|
||||
// Cancel message already displayed, nothing left to do
|
||||
return PX4_ERROR;
|
||||
|
||||
} else if (cal_return == calibrate_return_ok) {
|
||||
res = PX4_OK;
|
||||
|
||||
} else {
|
||||
res = PX4_ERROR;
|
||||
}
|
||||
|
|
@ -264,6 +284,7 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
|
|||
if (active_sensors == 0) {
|
||||
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SENSOR_MSG);
|
||||
}
|
||||
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
|
|
@ -282,9 +303,9 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
|
|||
|
||||
/* handle individual sensors, one by one */
|
||||
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_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_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,
|
||||
(double)accel_scale.x_offset,
|
||||
(double)accel_scale.y_offset,
|
||||
(double)accel_scale.z_offset);
|
||||
(double)accel_scale.x_offset,
|
||||
(double)accel_scale.y_offset,
|
||||
(double)accel_scale.z_offset);
|
||||
PX4_INFO("found scale %d: x: %.6f, y: %.6f, z: %.6f", uorb_index,
|
||||
(double)accel_scale.x_scale,
|
||||
(double)accel_scale.y_scale,
|
||||
(double)accel_scale.z_scale);
|
||||
(double)accel_scale.x_scale,
|
||||
(double)accel_scale.y_scale,
|
||||
(double)accel_scale.z_scale);
|
||||
|
||||
/* check if thermal compensation is enabled */
|
||||
int32_t tc_enabled_int;
|
||||
param_get(param_find("TC_A_ENABLE"), &(tc_enabled_int));
|
||||
|
||||
if (tc_enabled_int == 1) {
|
||||
/* Get struct containing sensor thermal compensation data */
|
||||
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 */
|
||||
int32_t handle;
|
||||
float val;
|
||||
|
||||
for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
|
||||
val = 0.0f;
|
||||
(void)sprintf(str, "TC_A%u_X0_%u", sensor_correction.accel_mapping[uorb_index], axis_index);
|
||||
handle = param_find(str);
|
||||
param_get(handle, &val);
|
||||
|
||||
if (axis_index == 0) {
|
||||
val += accel_scale.x_offset;
|
||||
|
||||
} else if (axis_index == 1) {
|
||||
val += accel_scale.y_offset;
|
||||
|
||||
} else if (axis_index == 2) {
|
||||
val += accel_scale.z_offset;
|
||||
}
|
||||
|
||||
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;
|
||||
(void)sprintf(str, "TC_A%u_SCL_%u", sensor_correction.accel_mapping[uorb_index], axis_index);
|
||||
handle = param_find(str);
|
||||
|
||||
if (axis_index == 0) {
|
||||
val = accel_scale.x_scale;
|
||||
|
||||
} else if (axis_index == 1) {
|
||||
val = accel_scale.y_scale;
|
||||
|
||||
} else if (axis_index == 2) {
|
||||
val = accel_scale.z_scale;
|
||||
}
|
||||
|
||||
failed |= (PX4_OK != param_set_no_notification(handle, &val));
|
||||
}
|
||||
|
||||
param_notify_changes();
|
||||
}
|
||||
|
||||
|
|
@ -394,6 +426,7 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
|
|||
if (fd < 0) {
|
||||
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "sensor does not exist");
|
||||
res = PX4_ERROR;
|
||||
|
||||
} else {
|
||||
res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
|
||||
px4_close(fd);
|
||||
|
|
@ -402,6 +435,7 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
|
|||
if (res != PX4_OK) {
|
||||
calibration_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG, uorb_index);
|
||||
}
|
||||
|
||||
#endif
|
||||
}
|
||||
|
||||
|
|
@ -421,19 +455,22 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
|
|||
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;
|
||||
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),
|
||||
(double)worker_data->accel_ref[0][orientation][0],
|
||||
(double)worker_data->accel_ref[0][orientation][1],
|
||||
(double)worker_data->accel_ref[0][orientation][2]);
|
||||
calibration_log_info(worker_data->mavlink_log_pub, "[cal] %s side result: [%8.4f %8.4f %8.4f]",
|
||||
detect_orientation_str(orientation),
|
||||
(double)worker_data->accel_ref[0][orientation][0],
|
||||
(double)worker_data->accel_ref[0][orientation][1],
|
||||
(double)worker_data->accel_ref[0][orientation][2]);
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
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;
|
||||
|
||||
|
|
@ -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));
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
|
|
@ -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
|
||||
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++) {
|
||||
|
||||
// Lock in to correct ORB instance
|
||||
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);
|
||||
|
||||
sensor_accel_s report = {};
|
||||
|
|
@ -493,6 +533,7 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub
|
|||
found_cur_accel = true;
|
||||
// store initial timestamp - used to infer which sensors are active
|
||||
timestamps[cur_accel] = report.timestamp;
|
||||
|
||||
} else {
|
||||
orb_unsubscribe(worker_data.subs[cur_accel]);
|
||||
}
|
||||
|
|
@ -506,7 +547,7 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub
|
|||
#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]);
|
||||
result = calibrate_return_error;
|
||||
break;
|
||||
|
|
@ -521,6 +562,7 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub
|
|||
device_prio_max = prio;
|
||||
device_id_primary = device_id[cur_accel];
|
||||
}
|
||||
|
||||
} else {
|
||||
calibration_log_critical(mavlink_log_pub, "Accel #%u no device id, abort", cur_accel);
|
||||
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) {
|
||||
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);
|
||||
}
|
||||
|
||||
|
|
@ -540,12 +583,15 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub
|
|||
/* figure out which sensors were active */
|
||||
sensor_accel_s arp = {};
|
||||
(void)orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &arp);
|
||||
|
||||
if (arp.timestamp != 0 && timestamps[i] != arp.timestamp) {
|
||||
(*active_sensors)++;
|
||||
}
|
||||
|
||||
px4_close(worker_data.subs[i]);
|
||||
}
|
||||
}
|
||||
|
||||
orb_unsubscribe(worker_data.sensor_correction_sub);
|
||||
|
||||
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.
|
||||
*/
|
||||
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 */
|
||||
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) {
|
||||
/* use default values */
|
||||
memset(&sensor_correction, 0, sizeof(sensor_correction));
|
||||
|
||||
for (unsigned i = 0; i < 3; i++) {
|
||||
sensor_correction.accel_scale_0[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][1] += (arp.y - sensor_correction.accel_offset_0[1]);
|
||||
accel_sum[s][2] += (arp.z - sensor_correction.accel_offset_0[2]);
|
||||
|
||||
} else if (s == 1) {
|
||||
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][2] += (arp.z - sensor_correction.accel_offset_1[2]);
|
||||
|
||||
} else if (s == 2) {
|
||||
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][2] += (arp.z - sensor_correction.accel_offset_2[2]);
|
||||
|
||||
} else {
|
||||
accel_sum[s][0] += arp.x;
|
||||
accel_sum[s][1] += arp.y;
|
||||
|
|
@ -701,7 +752,9 @@ int mat_invert3(float src[3][3], float dst[3][3])
|
|||
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 */
|
||||
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;
|
||||
}
|
||||
|
||||
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_hz = 100;
|
||||
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);
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
|
|
@ -782,14 +836,17 @@ int do_level_calibration(orb_advert_t *mavlink_log_pub) {
|
|||
|
||||
// sleep for some 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);
|
||||
}
|
||||
|
||||
start = hrt_absolute_time();
|
||||
|
||||
// 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);
|
||||
|
||||
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);
|
||||
|
||||
if (counter > (cal_time * cal_hz / 2 )) {
|
||||
if (counter > (cal_time * cal_hz / 2)) {
|
||||
roll_mean /= counter;
|
||||
pitch_mean /= counter;
|
||||
|
||||
} else {
|
||||
calibration_log_info(mavlink_log_pub, "not enough measurements taken");
|
||||
success = false;
|
||||
goto out;
|
||||
}
|
||||
|
||||
if (fabsf(roll_mean) > 0.8f ) {
|
||||
if (fabsf(roll_mean) > 0.8f) {
|
||||
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");
|
||||
|
||||
} else {
|
||||
roll_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:
|
||||
|
||||
if (success) {
|
||||
calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "level");
|
||||
return 0;
|
||||
|
||||
} else {
|
||||
// set old parameters
|
||||
param_set_no_notification(roll_offset_handle, &roll_offset_current);
|
||||
|
|
|
|||
|
|
@ -107,6 +107,7 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
|
|||
/* only warn if analog scaling is zero */
|
||||
float analog_scaling = 0.0f;
|
||||
param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling));
|
||||
|
||||
if (fabsf(analog_scaling) < 0.1f) {
|
||||
calibration_log_critical(mavlink_log_pub, "[cal] No airspeed sensor found");
|
||||
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 */
|
||||
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");
|
||||
feedback_calibration_failed(mavlink_log_pub);
|
||||
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);
|
||||
airscale.offset_pa = diff_pres_offset;
|
||||
|
||||
if (fd_scale > 0) {
|
||||
if (PX4_OK != px4_ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
|
||||
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 (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;
|
||||
|
||||
} else {
|
||||
/* 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!");
|
||||
|
||||
/* the user setup is wrong, wipe the calibration to force a proper re-calibration */
|
||||
diff_pres_offset = 0.0f;
|
||||
|
||||
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
|
||||
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, 1);
|
||||
goto error_return;
|
||||
|
|
@ -244,9 +251,11 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
|
|||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
calibration_counter++;
|
||||
|
||||
} else if (poll_ret == 0) {
|
||||
|
|
|
|||
|
|
@ -85,6 +85,7 @@ static void arm_auth_request_msg_send()
|
|||
|
||||
if (handle_vehicle_command_pub == nullptr) {
|
||||
handle_vehicle_command_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
|
||||
|
||||
} else {
|
||||
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:
|
||||
/* no authentication in process? handle bellow */
|
||||
break;
|
||||
|
||||
case ARM_AUTH_MISSION_APPROVED:
|
||||
return vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
|
||||
|
||||
default:
|
||||
return vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
|
||||
}
|
||||
|
|
@ -127,12 +130,13 @@ static uint8_t _auth_method_arm_req_check()
|
|||
state = ARM_AUTH_IDLE;
|
||||
mavlink_log_critical(mavlink_log_pub, "Arm auth: No response");
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
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()
|
||||
|
|
@ -141,11 +145,14 @@ static uint8_t _auth_method_two_arm_check()
|
|||
case ARM_AUTH_IDLE:
|
||||
/* no authentication in process? handle bellow */
|
||||
break;
|
||||
|
||||
case ARM_AUTH_MISSION_APPROVED:
|
||||
return vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
|
||||
|
||||
case ARM_AUTH_WAITING_AUTH:
|
||||
case ARM_AUTH_WAITING_AUTH_WITH_ACK:
|
||||
return vehicle_command_ack_s::VEHICLE_RESULT_TEMPORARILY_REJECTED;
|
||||
|
||||
default:
|
||||
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)
|
||||
{
|
||||
if (param_update) {
|
||||
param_get(param_arm_parameters, (int32_t*)&arm_parameters);
|
||||
param_get(param_arm_parameters, (int32_t *)&arm_parameters);
|
||||
}
|
||||
|
||||
switch (state) {
|
||||
|
|
@ -182,11 +189,14 @@ void arm_auth_update(hrt_abstime now, bool param_update)
|
|||
case ARM_AUTH_WAITING_AUTH_WITH_ACK:
|
||||
/* handle bellow */
|
||||
break;
|
||||
|
||||
case ARM_AUTH_MISSION_APPROVED:
|
||||
if (now > auth_timeout) {
|
||||
state = ARM_AUTH_IDLE;
|
||||
}
|
||||
|
||||
return;
|
||||
|
||||
case ARM_AUTH_IDLE:
|
||||
default:
|
||||
return;
|
||||
|
|
@ -199,50 +209,60 @@ void arm_auth_update(hrt_abstime now, bool param_update)
|
|||
bool updated = false;
|
||||
|
||||
orb_check(command_ack_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(vehicle_command_ack), command_ack_sub, &command_ack);
|
||||
}
|
||||
|
||||
if (updated
|
||||
&& command_ack.command == vehicle_command_s::VEHICLE_CMD_ARM_AUTHORIZATION_REQUEST
|
||||
&& command_ack.target_system == *system_id) {
|
||||
&& command_ack.command == vehicle_command_s::VEHICLE_CMD_ARM_AUTHORIZATION_REQUEST
|
||||
&& command_ack.target_system == *system_id) {
|
||||
switch (command_ack.result) {
|
||||
case vehicle_command_ack_s::VEHICLE_RESULT_IN_PROGRESS:
|
||||
state = ARM_AUTH_WAITING_AUTH_WITH_ACK;
|
||||
break;
|
||||
|
||||
case vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED:
|
||||
mavlink_log_critical(mavlink_log_pub,
|
||||
"Arm auth: Authorized for the next %u seconds",
|
||||
command_ack.result_param2);
|
||||
"Arm auth: Authorized for the next %u seconds",
|
||||
command_ack.result_param2);
|
||||
state = ARM_AUTH_MISSION_APPROVED;
|
||||
auth_timeout = now + (command_ack.result_param2 * 1000000);
|
||||
return;
|
||||
|
||||
case vehicle_command_ack_s::VEHICLE_RESULT_TEMPORARILY_REJECTED:
|
||||
mavlink_log_critical(mavlink_log_pub, "Arm auth: Temporarily rejected");
|
||||
state = ARM_AUTH_IDLE;
|
||||
return;
|
||||
|
||||
case vehicle_command_ack_s::VEHICLE_RESULT_DENIED:
|
||||
default:
|
||||
switch (command_ack.result_param1) {
|
||||
case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_NONE:
|
||||
/* Authorizer will send reason to ground station */
|
||||
break;
|
||||
|
||||
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);
|
||||
break;
|
||||
|
||||
case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_TIMEOUT:
|
||||
mavlink_log_critical(mavlink_log_pub, "Arm auth: Denied by timeout in authorizer");
|
||||
break;
|
||||
|
||||
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");
|
||||
break;
|
||||
|
||||
case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_BAD_WEATHER:
|
||||
mavlink_log_critical(mavlink_log_pub, "Arm auth: Denied because of bad weather");
|
||||
break;
|
||||
|
||||
case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_GENERIC:
|
||||
default:
|
||||
mavlink_log_critical(mavlink_log_pub, "Arm auth: Denied");
|
||||
}
|
||||
|
||||
state = ARM_AUTH_IDLE;
|
||||
return;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -800,14 +800,17 @@ calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub,
|
|||
int calibrate_cancel_subscribe()
|
||||
{
|
||||
int vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));
|
||||
|
||||
if (vehicle_command_sub >= 0) {
|
||||
// make sure we won't read any old messages
|
||||
struct vehicle_command_s cmd;
|
||||
bool update;
|
||||
|
||||
while (orb_check(vehicle_command_sub, &update) == 0 && update) {
|
||||
orb_copy(ORB_ID(vehicle_command), vehicle_command_sub, &cmd);
|
||||
}
|
||||
}
|
||||
|
||||
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
|
||||
if (cmd.from_external) {
|
||||
if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION &&
|
||||
(int)cmd.param1 == 0 &&
|
||||
(int)cmd.param2 == 0 &&
|
||||
(int)cmd.param3 == 0 &&
|
||||
(int)cmd.param4 == 0 &&
|
||||
(int)cmd.param5 == 0 &&
|
||||
(int)cmd.param6 == 0) {
|
||||
(int)cmd.param1 == 0 &&
|
||||
(int)cmd.param2 == 0 &&
|
||||
(int)cmd.param3 == 0 &&
|
||||
(int)cmd.param4 == 0 &&
|
||||
(int)cmd.param5 == 0 &&
|
||||
(int)cmd.param6 == 0) {
|
||||
calibrate_answer_command(mavlink_log_pub, cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
mavlink_log_critical(mavlink_log_pub, CAL_QGC_CANCELLED_MSG);
|
||||
return true;
|
||||
|
|
|
|||
|
|
@ -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 *offdiag_z);
|
||||
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,
|
||||
float *sphere_radius, float *diag_x, float *diag_y, float *diag_z, float *offdiag_x, float *offdiag_y,
|
||||
float *offdiag_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 *offdiag_z);
|
||||
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
|
||||
static const unsigned max_accel_sens = 3;
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
{
|
||||
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 ||
|
||||
current_status->system_type == VEHICLE_TYPE_VTOL_QUADROTOR ||
|
||||
current_status->system_type == VEHICLE_TYPE_VTOL_TILTROTOR ||
|
||||
|
|
|
|||
|
|
@ -59,215 +59,261 @@ private:
|
|||
|
||||
bool StateMachineHelperTest::armingStateTransitionTest()
|
||||
{
|
||||
// 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
|
||||
// use to represent the expected machine state after the transition has been requested.
|
||||
typedef struct {
|
||||
arming_state_t arming_state; // vehicle_status_s.arming_state
|
||||
bool armed; // actuator_armed_s.armed
|
||||
bool ready_to_arm; // actuator_armed_s.ready_to_arm
|
||||
} ArmingTransitionVolatileState_t;
|
||||
// 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
|
||||
// use to represent the expected machine state after the transition has been requested.
|
||||
typedef struct {
|
||||
arming_state_t arming_state; // vehicle_status_s.arming_state
|
||||
bool armed; // actuator_armed_s.armed
|
||||
bool ready_to_arm; // actuator_armed_s.ready_to_arm
|
||||
} ArmingTransitionVolatileState_t;
|
||||
|
||||
// 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
|
||||
// machine state after transition.
|
||||
typedef struct {
|
||||
const char* assertMsg; // Text to show when test case fails
|
||||
ArmingTransitionVolatileState_t current_state; // Machine state prior to transition
|
||||
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 safety_switch_available; // Current safety_s.safety_switch_available
|
||||
bool safety_off; // Current safety_s.safety_off
|
||||
arming_state_t requested_state; // Requested arming state to transition to
|
||||
ArmingTransitionVolatileState_t expected_state; // Expected machine state after transition
|
||||
transition_result_t expected_transition_result; // Expected result from arming_state_transition
|
||||
} ArmingTransitionTest_t;
|
||||
// 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
|
||||
// machine state after transition.
|
||||
typedef struct {
|
||||
const char *assertMsg; // Text to show when test case fails
|
||||
ArmingTransitionVolatileState_t current_state; // Machine state prior to transition
|
||||
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 safety_switch_available; // Current safety_s.safety_switch_available
|
||||
bool safety_off; // Current safety_s.safety_off
|
||||
arming_state_t requested_state; // Requested arming state to transition to
|
||||
ArmingTransitionVolatileState_t expected_state; // Expected machine state after transition
|
||||
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
|
||||
#define ATT_ARMED true
|
||||
#define ATT_DISARMED false
|
||||
#define ATT_READY_TO_ARM true
|
||||
#define ATT_NOT_READY_TO_ARM false
|
||||
#define ATT_SENSORS_INITIALIZED true
|
||||
#define ATT_SENSORS_NOT_INITIALIZED false
|
||||
#define ATT_SAFETY_AVAILABLE true
|
||||
#define ATT_SAFETY_NOT_AVAILABLE true
|
||||
#define ATT_SAFETY_OFF true
|
||||
#define ATT_SAFETY_ON false
|
||||
// We use these defines so that our test cases are more readable
|
||||
#define ATT_ARMED true
|
||||
#define ATT_DISARMED false
|
||||
#define ATT_READY_TO_ARM true
|
||||
#define ATT_NOT_READY_TO_ARM false
|
||||
#define ATT_SENSORS_INITIALIZED true
|
||||
#define ATT_SENSORS_NOT_INITIALIZED false
|
||||
#define ATT_SAFETY_AVAILABLE true
|
||||
#define ATT_SAFETY_NOT_AVAILABLE true
|
||||
#define ATT_SAFETY_OFF true
|
||||
#define ATT_SAFETY_ON false
|
||||
|
||||
// These are test cases for arming_state_transition
|
||||
static const ArmingTransitionTest_t rgArmingTransitionTests[] = {
|
||||
// TRANSITION_NOT_CHANGED tests
|
||||
// These are test cases for arming_state_transition
|
||||
static const ArmingTransitionTest_t rgArmingTransitionTests[] = {
|
||||
// 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,
|
||||
vehicle_status_s::ARMING_STATE_INIT,
|
||||
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_NOT_CHANGED },
|
||||
{
|
||||
"no transition: identical states",
|
||||
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
|
||||
vehicle_status_s::ARMING_STATE_INIT,
|
||||
{ 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,
|
||||
vehicle_status_s::ARMING_STATE_STANDBY,
|
||||
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
|
||||
{
|
||||
"transition: init to standby",
|
||||
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
|
||||
vehicle_status_s::ARMING_STATE_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,
|
||||
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 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,
|
||||
{ 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,
|
||||
vehicle_status_s::ARMING_STATE_REBOOT,
|
||||
{ vehicle_status_s::ARMING_STATE_REBOOT, 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,
|
||||
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,
|
||||
vehicle_status_s::ARMING_STATE_INIT,
|
||||
{ vehicle_status_s::ARMING_STATE_INIT, 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,
|
||||
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,
|
||||
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 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,
|
||||
{ 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,
|
||||
vehicle_status_s::ARMING_STATE_REBOOT,
|
||||
{ vehicle_status_s::ARMING_STATE_REBOOT, 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,
|
||||
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,
|
||||
vehicle_status_s::ARMING_STATE_STANDBY,
|
||||
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_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,
|
||||
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,
|
||||
vehicle_status_s::ARMING_STATE_REBOOT,
|
||||
{ vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_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,
|
||||
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,
|
||||
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 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,
|
||||
{ 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,
|
||||
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 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,
|
||||
{ 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,
|
||||
vehicle_status_s::ARMING_STATE_STANDBY,
|
||||
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
|
||||
{
|
||||
"transition: standby error to standby, hil on",
|
||||
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
|
||||
vehicle_status_s::ARMING_STATE_STANDBY,
|
||||
{ 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,
|
||||
vehicle_status_s::ARMING_STATE_ARMED,
|
||||
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
|
||||
{
|
||||
"transition: standby to armed, no safety switch",
|
||||
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF,
|
||||
vehicle_status_s::ARMING_STATE_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,
|
||||
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,
|
||||
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,
|
||||
vehicle_status_s::ARMING_STATE_ARMED,
|
||||
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
|
||||
{
|
||||
"no transition: init to armed",
|
||||
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
|
||||
vehicle_status_s::ARMING_STATE_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,
|
||||
vehicle_status_s::ARMING_STATE_INIT,
|
||||
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_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,
|
||||
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,
|
||||
vehicle_status_s::ARMING_STATE_REBOOT,
|
||||
{ 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,
|
||||
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,
|
||||
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 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_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,
|
||||
vehicle_status_s::ARMING_STATE_STANDBY,
|
||||
{ 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,
|
||||
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,
|
||||
vehicle_status_s::ARMING_STATE_ARMED,
|
||||
{ vehicle_status_s::ARMING_STATE_REBOOT, 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,
|
||||
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,
|
||||
vehicle_status_s::ARMING_STATE_STANDBY,
|
||||
{ vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, 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,
|
||||
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",
|
||||
// { 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_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
|
||||
//{ "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_STANDBY,
|
||||
// { 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,
|
||||
vehicle_status_s::ARMING_STATE_ARMED,
|
||||
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
|
||||
};
|
||||
{
|
||||
"no transition: init to standby, safety switch on",
|
||||
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
|
||||
vehicle_status_s::ARMING_STATE_ARMED,
|
||||
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED
|
||||
},
|
||||
};
|
||||
|
||||
struct vehicle_status_s status = {};
|
||||
struct vehicle_status_flags_s status_flags = {};
|
||||
struct safety_s safety = {};
|
||||
struct actuator_armed_s armed = {};
|
||||
|
||||
size_t cArmingTransitionTests = sizeof(rgArmingTransitionTests) / sizeof(rgArmingTransitionTests[0]);
|
||||
for (size_t i=0; i<cArmingTransitionTests; i++) {
|
||||
const ArmingTransitionTest_t* test = &rgArmingTransitionTests[i];
|
||||
size_t cArmingTransitionTests = sizeof(rgArmingTransitionTests) / sizeof(rgArmingTransitionTests[0]);
|
||||
|
||||
const bool check_gps = false;
|
||||
for (size_t i = 0; i < cArmingTransitionTests; i++) {
|
||||
const ArmingTransitionTest_t *test = &rgArmingTransitionTests[i];
|
||||
|
||||
// Setup initial machine state
|
||||
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;
|
||||
const bool check_gps = false;
|
||||
|
||||
// Attempt transition
|
||||
transition_result_t result = arming_state_transition(&status, safety, test->requested_state, &armed,
|
||||
false /* no pre-arm checks */,
|
||||
nullptr /* no mavlink_log_pub */,
|
||||
&status_flags,
|
||||
(check_gps ? ARM_REQ_GPS_BIT : 0),
|
||||
2e6 /* 2 seconds after boot, everything should be checked */
|
||||
);
|
||||
// Setup initial machine state
|
||||
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;
|
||||
|
||||
// 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);
|
||||
}
|
||||
// Attempt transition
|
||||
transition_result_t result = arming_state_transition(&status, safety, test->requested_state, &armed,
|
||||
false /* no pre-arm checks */,
|
||||
nullptr /* no mavlink_log_pub */,
|
||||
&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;
|
||||
}
|
||||
|
|
@ -276,7 +322,7 @@ bool StateMachineHelperTest::mainStateTransitionTest()
|
|||
{
|
||||
// This structure represent a single test case for testing Main State transitions.
|
||||
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 from_state; // State prior to transition request
|
||||
main_state_t to_state; // State to transition to
|
||||
|
|
@ -284,125 +330,176 @@ bool StateMachineHelperTest::mainStateTransitionTest()
|
|||
} MainTransitionTest_t;
|
||||
|
||||
// Bits for condition_bits
|
||||
#define MTT_ALL_NOT_VALID 0
|
||||
#define MTT_ROTARY_WING 1 << 0
|
||||
#define MTT_LOC_ALT_VALID 1 << 1
|
||||
#define MTT_LOC_POS_VALID 1 << 2
|
||||
#define MTT_HOME_POS_VALID 1 << 3
|
||||
#define MTT_GLOBAL_POS_VALID 1 << 4
|
||||
#define MTT_ALL_NOT_VALID 0
|
||||
#define MTT_ROTARY_WING 1 << 0
|
||||
#define MTT_LOC_ALT_VALID 1 << 1
|
||||
#define MTT_LOC_POS_VALID 1 << 2
|
||||
#define MTT_HOME_POS_VALID 1 << 3
|
||||
#define MTT_GLOBAL_POS_VALID 1 << 4
|
||||
|
||||
static const MainTransitionTest_t rgMainTransitionTests[] = {
|
||||
|
||||
// TRANSITION_NOT_CHANGED tests
|
||||
|
||||
{ "no transition: identical states",
|
||||
{
|
||||
"no transition: identical states",
|
||||
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: MANUAL to ACRO - rotary",
|
||||
{
|
||||
"transition: MANUAL to ACRO - rotary",
|
||||
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,
|
||||
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,
|
||||
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,
|
||||
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,
|
||||
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,
|
||||
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,
|
||||
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,
|
||||
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,
|
||||
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,
|
||||
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,
|
||||
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,
|
||||
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,
|
||||
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,
|
||||
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,
|
||||
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,
|
||||
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
|
||||
|
||||
{ "no transition: MANUAL to AUTO_MISSION - global position not valid",
|
||||
{
|
||||
"no transition: MANUAL to AUTO_MISSION - global position 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,
|
||||
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,
|
||||
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,
|
||||
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,
|
||||
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,
|
||||
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,
|
||||
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,
|
||||
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]);
|
||||
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
|
||||
struct vehicle_status_s current_vehicle_status = {};
|
||||
|
|
@ -418,13 +515,16 @@ bool StateMachineHelperTest::mainStateTransitionTest()
|
|||
current_status_flags.condition_auto_mission_available = true;
|
||||
|
||||
// Attempt transition
|
||||
transition_result_t result = main_state_transition(current_vehicle_status, test->to_state, current_status_flags, ¤t_commander_state);
|
||||
transition_result_t result = main_state_transition(current_vehicle_status, test->to_state, current_status_flags,
|
||||
¤t_commander_state);
|
||||
|
||||
// Validate result of transition
|
||||
ut_compare(test->assertMsg, test->expected_transition_result, result);
|
||||
|
||||
if (test->expected_transition_result == result) {
|
||||
if (test->expected_transition_result == TRANSITION_CHANGED) {
|
||||
ut_compare(test->assertMsg, test->to_state, current_commander_state.main_state);
|
||||
|
||||
} else {
|
||||
ut_compare(test->assertMsg, test->from_state, current_commander_state.main_state);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -63,21 +63,22 @@
|
|||
|
||||
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;
|
||||
|
||||
|
||||
#if defined(__PX4_POSIX_OCPOC) || defined(__PX4_POSIX_BBBLUE)
|
||||
hrt_abstime timeout_start = 0;
|
||||
hrt_abstime timeout_wait = 60_s;
|
||||
armed->in_esc_calibration_mode = true;
|
||||
calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "begin esc");
|
||||
timeout_start = hrt_absolute_time();
|
||||
|
||||
|
||||
while (true) {
|
||||
if (hrt_absolute_time() - timeout_start > timeout_wait) {
|
||||
break;
|
||||
}else{
|
||||
|
||||
} else {
|
||||
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) {
|
||||
calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "esc");
|
||||
}
|
||||
|
||||
|
||||
return return_code;
|
||||
|
||||
#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");
|
||||
|
||||
batt_sub = orb_subscribe(ORB_ID(battery_status));
|
||||
|
||||
if (batt_sub < 0) {
|
||||
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Subscribe to battery");
|
||||
goto Error;
|
||||
|
|
@ -173,8 +175,10 @@ int do_esc_calibration(orb_advert_t *mavlink_log_pub, struct actuator_armed_s* a
|
|||
|
||||
if (!batt_connected) {
|
||||
orb_check(batt_sub, &batt_updated);
|
||||
|
||||
if (batt_updated) {
|
||||
orb_copy(ORB_ID(battery_status), batt_sub, &battery);
|
||||
|
||||
if (battery.connected) {
|
||||
// Battery is connected, signal to user and start waiting again
|
||||
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);
|
||||
}
|
||||
|
||||
Out:
|
||||
|
||||
if (batt_sub != -1) {
|
||||
orb_unsubscribe(batt_sub);
|
||||
}
|
||||
|
||||
if (fd != -1) {
|
||||
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");
|
||||
}
|
||||
|
||||
if (px4_ioctl(fd, PWM_SERVO_DISARM, 0) != PX4_OK) {
|
||||
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) {
|
||||
calibration_log_info(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Safety switch still deactivated");
|
||||
}
|
||||
|
||||
px4_close(fd);
|
||||
}
|
||||
|
||||
armed->in_esc_calibration_mode = false;
|
||||
|
||||
if (return_code == PX4_OK) {
|
||||
|
|
|
|||
|
|
@ -44,6 +44,6 @@
|
|||
|
||||
#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
|
||||
|
|
|
|||
|
|
@ -83,6 +83,7 @@ FailureDetector::update_attitude_status()
|
|||
if (roll_status) {
|
||||
_status |= FAILURE_ROLL;
|
||||
}
|
||||
|
||||
if (pitch_status) {
|
||||
_status |= FAILURE_PITCH;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,35 +1,35 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2017 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2017 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file gyro_calibration.cpp
|
||||
|
|
@ -74,18 +74,20 @@ typedef struct {
|
|||
sensor_gyro_s gyro_report_0;
|
||||
} 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;
|
||||
const unsigned calibration_count = 5000;
|
||||
sensor_gyro_s gyro_report;
|
||||
unsigned poll_errcount = 0;
|
||||
|
||||
struct sensor_correction_s sensor_correction; /**< sensor thermal corrections */
|
||||
|
||||
if (orb_copy(ORB_ID(sensor_correction), worker_data->sensor_correction_sub, &sensor_correction) != 0) {
|
||||
/* use default values */
|
||||
memset(&sensor_correction, 0, sizeof(sensor_correction));
|
||||
|
||||
for (unsigned i = 0; i < 3; i++) {
|
||||
sensor_correction.gyro_scale_0[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];
|
||||
|
||||
for (unsigned s = 0; s < max_gyros; s++) {
|
||||
fds[s].fd = worker_data->gyro_sensor_sub[s];
|
||||
fds[s].events = POLLIN;
|
||||
|
|
@ -120,6 +123,7 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data)
|
|||
|
||||
if (poll_ret > 0) {
|
||||
unsigned update_count = calibration_count;
|
||||
|
||||
for (unsigned s = 0; s < max_gyros; s++) {
|
||||
if (calibration_counter[s] >= calibration_count) {
|
||||
// Skip if instance has enough samples
|
||||
|
|
@ -134,9 +138,12 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data)
|
|||
|
||||
if (s == 0) {
|
||||
// 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].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];
|
||||
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].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
|
||||
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];
|
||||
|
||||
} 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].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];
|
||||
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].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) {
|
||||
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].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];
|
||||
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].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 {
|
||||
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) {
|
||||
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;
|
||||
(void)sprintf(str, "CAL_GYRO%u_ID", s);
|
||||
res = param_set_no_notification(param_find(str), &(worker_data.device_id[s]));
|
||||
|
||||
if (res != PX4_OK) {
|
||||
calibration_log_critical(mavlink_log_pub, "Unable to reset CAL_GYRO%u_ID", s);
|
||||
return PX4_ERROR;
|
||||
|
|
@ -246,6 +260,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
|
|||
#ifdef __PX4_NUTTX
|
||||
sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
|
||||
int fd = px4_open(str, 0);
|
||||
|
||||
if (fd >= 0) {
|
||||
worker_data.device_id[s] = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
#else
|
||||
(void)sprintf(str, "CAL_GYRO%u_XOFF", s);
|
||||
res = param_set_no_notification(param_find(str), &gyro_scale_zero.x_offset);
|
||||
|
||||
if (res != PX4_OK) {
|
||||
PX4_ERR("unable to reset %s", str);
|
||||
}
|
||||
|
||||
(void)sprintf(str, "CAL_GYRO%u_YOFF", s);
|
||||
res = param_set_no_notification(param_find(str), &gyro_scale_zero.y_offset);
|
||||
|
||||
if (res != PX4_OK) {
|
||||
PX4_ERR("unable to reset %s", str);
|
||||
}
|
||||
|
||||
(void)sprintf(str, "CAL_GYRO%u_ZOFF", s);
|
||||
res = param_set_no_notification(param_find(str), &gyro_scale_zero.z_offset);
|
||||
|
||||
if (res != PX4_OK) {
|
||||
PX4_ERR("unable to reset %s", str);
|
||||
}
|
||||
|
||||
(void)sprintf(str, "CAL_GYRO%u_XSCALE", s);
|
||||
res = param_set_no_notification(param_find(str), &gyro_scale_zero.x_scale);
|
||||
|
||||
if (res != PX4_OK) {
|
||||
PX4_ERR("unable to reset %s", str);
|
||||
}
|
||||
|
||||
(void)sprintf(str, "CAL_GYRO%u_YSCALE", s);
|
||||
res = param_set_no_notification(param_find(str), &gyro_scale_zero.y_scale);
|
||||
|
||||
if (res != PX4_OK) {
|
||||
PX4_ERR("unable to reset %s", str);
|
||||
}
|
||||
|
||||
(void)sprintf(str, "CAL_GYRO%u_ZSCALE", s);
|
||||
res = param_set_no_notification(param_find(str), &gyro_scale_zero.z_scale);
|
||||
|
||||
if (res != PX4_OK) {
|
||||
PX4_ERR("unable to reset %s", str);
|
||||
}
|
||||
|
||||
param_notify_changes();
|
||||
#endif
|
||||
|
||||
|
|
@ -304,7 +332,8 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
|
|||
|
||||
// Lock in to correct ORB instance
|
||||
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);
|
||||
|
||||
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]) {
|
||||
// Device IDs match, correct ORB instance for this gyro
|
||||
found_cur_gyro = true;
|
||||
|
||||
} else {
|
||||
orb_unsubscribe(worker_data.gyro_sensor_sub[cur_gyro]);
|
||||
}
|
||||
|
|
@ -332,8 +362,9 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
|
|||
#endif
|
||||
}
|
||||
|
||||
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]);
|
||||
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]);
|
||||
res = calibrate_return_error;
|
||||
break;
|
||||
}
|
||||
|
|
@ -347,6 +378,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
|
|||
device_prio_max = prio;
|
||||
device_id_primary = worker_data.device_id[cur_gyro];
|
||||
}
|
||||
|
||||
} else {
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
try_count++;
|
||||
|
||||
} 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 */
|
||||
int32_t tc_enabled_int;
|
||||
param_get(param_find("TC_G_ENABLE"), &(tc_enabled_int));
|
||||
|
||||
if (tc_enabled_int == 1) {
|
||||
/* Get struct containing sensor thermal compensation data */
|
||||
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 */
|
||||
int32_t handle;
|
||||
float val;
|
||||
|
||||
for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
|
||||
val = 0.0f;
|
||||
(void)sprintf(str, "TC_G%u_X0_%u", sensor_correction.gyro_mapping[uorb_index], axis_index);
|
||||
handle = param_find(str);
|
||||
param_get(handle, &val);
|
||||
|
||||
if (axis_index == 0) {
|
||||
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;
|
||||
|
||||
}
|
||||
|
||||
failed |= (PX4_OK != param_set_no_notification(handle, &val));
|
||||
}
|
||||
|
||||
param_notify_changes();
|
||||
}
|
||||
|
||||
|
|
@ -489,6 +527,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
|
|||
if (res != PX4_OK) {
|
||||
calibration_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG, 1);
|
||||
}
|
||||
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
|
@ -504,6 +543,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
|
|||
|
||||
if (res == PX4_OK) {
|
||||
calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);
|
||||
|
||||
} else {
|
||||
calibration_log_info(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
(void)sprintf(str, "EKF2_MAGBIAS_X");
|
||||
result = param_set_no_notification(param_find(str), &mscale_null.x_offset);
|
||||
|
||||
if (result != PX4_OK) {
|
||||
PX4_ERR("unable to reset %s", str);
|
||||
}
|
||||
|
||||
(void)sprintf(str, "EKF2_MAGBIAS_Y");
|
||||
result = param_set_no_notification(param_find(str), &mscale_null.y_offset);
|
||||
|
||||
if (result != PX4_OK) {
|
||||
PX4_ERR("unable to reset %s", str);
|
||||
}
|
||||
|
||||
(void)sprintf(str, "EKF2_MAGBIAS_Z");
|
||||
result = param_set_no_notification(param_find(str), &mscale_null.z_offset);
|
||||
|
||||
if (result != PX4_OK) {
|
||||
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
|
||||
// Logs if parameters are out of range
|
||||
static calibrate_return check_calibration_result(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 offdiag_z,
|
||||
orb_advert_t *mavlink_log_pub, size_t cur_mag)
|
||||
float sphere_radius,
|
||||
float diag_x, float diag_y, float diag_z,
|
||||
float offdiag_x, float offdiag_y, float offdiag_z,
|
||||
orb_advert_t *mavlink_log_pub, size_t cur_mag)
|
||||
{
|
||||
float must_be_finite[] = {offset_x, offset_y, offset_z,
|
||||
sphere_radius,
|
||||
diag_x, diag_y, diag_z,
|
||||
offdiag_x, offdiag_y, offdiag_z};
|
||||
sphere_radius,
|
||||
diag_x, diag_y, diag_z,
|
||||
offdiag_x, offdiag_y, offdiag_z
|
||||
};
|
||||
|
||||
float should_be_not_huge[] = {offset_x, offset_y, offset_z};
|
||||
float should_be_positive[] = {sphere_radius, diag_x, diag_y, diag_z};
|
||||
|
||||
// Make sure every parameter is finite
|
||||
const int num_finite = sizeof(must_be_finite) / sizeof(*must_be_finite);
|
||||
|
||||
for (unsigned i = 0; i < num_finite; ++i) {
|
||||
if (!PX4_ISFINITE(must_be_finite[i])) {
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
// Notify if offsets are too large
|
||||
const int num_not_huge = sizeof(should_be_not_huge) / sizeof(*should_be_not_huge);
|
||||
|
||||
for (unsigned i = 0; i < num_not_huge; ++i) {
|
||||
if (fabsf(should_be_not_huge[i]) > MAG_MAX_OFFSET_LEN) {
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
// Notify if a parameter which should be positive is non-positive
|
||||
const int num_positive = sizeof(should_be_positive) / sizeof(*should_be_positive);
|
||||
|
||||
for (unsigned i = 0; i < num_positive; ++i) {
|
||||
if (should_be_positive[i] <= 0.0f) {
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
|
@ -597,7 +606,8 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
|
|||
|
||||
// Lock in to correct ORB instance
|
||||
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);
|
||||
|
||||
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]) {
|
||||
// Device IDs match, correct ORB instance for this mag
|
||||
found_cur_mag = true;
|
||||
|
||||
} else {
|
||||
orb_unsubscribe(worker_data.sub_mag[cur_mag]);
|
||||
}
|
||||
|
|
@ -625,7 +636,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
|
|||
#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]);
|
||||
result = calibrate_return_error;
|
||||
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]);
|
||||
|
||||
result = check_calibration_result(sphere_x[cur_mag], sphere_y[cur_mag], sphere_z[cur_mag],
|
||||
sphere_radius[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],
|
||||
mavlink_log_pub, cur_mag);
|
||||
sphere_radius[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],
|
||||
mavlink_log_pub, cur_mag);
|
||||
|
||||
if (result == calibrate_return_error) {
|
||||
break;
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
&& !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) {
|
||||
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 */
|
||||
if (!status_flags->condition_system_sensors_initialized
|
||||
&& fRunPreArmChecks
|
||||
&& ((new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) || (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY))
|
||||
&& !hil_enabled) {
|
||||
&& fRunPreArmChecks
|
||||
&& ((new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)
|
||||
|| (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY))
|
||||
&& !hil_enabled) {
|
||||
|
||||
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();
|
||||
}
|
||||
|
|
@ -205,7 +208,8 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const safe
|
|||
// Finish up the state transition
|
||||
if (valid_transition) {
|
||||
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;
|
||||
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;
|
||||
|
||||
case LOW_BAT_ACTION::RETURN:
|
||||
// FALLTHROUGH
|
||||
|
||||
// FALLTHROUGH
|
||||
case LOW_BAT_ACTION::RETURN_OR_LAND:
|
||||
|
||||
// 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);
|
||||
|
||||
} else {
|
||||
|
|
@ -986,7 +992,8 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta
|
|||
break;
|
||||
|
||||
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);
|
||||
|
||||
} else {
|
||||
|
|
@ -1008,7 +1015,8 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta
|
|||
break;
|
||||
|
||||
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);
|
||||
|
||||
} else {
|
||||
|
|
@ -1018,9 +1026,11 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta
|
|||
break;
|
||||
|
||||
case LOW_BAT_ACTION::RETURN_OR_LAND:
|
||||
// FALLTHROUGH
|
||||
|
||||
// FALLTHROUGH
|
||||
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);
|
||||
|
||||
} else {
|
||||
|
|
|
|||
Loading…
Reference in New Issue