zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

vehicle_status_flags.msg: remove condition_ prefix to reduce message size 

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
v1.13.0-BW
Silvan Fuhrer 3 years ago committed by Daniel Agar
parent
c30f2b9493
commit
a7ddaf08c4
12 changed files with 165 additions and 165 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 32
      msg/vehicle_status_flags.msg
  2. 6
      src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp
  3. 26
      src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp
  4. 154
      src/modules/commander/Commander.cpp
  5. 6
      src/modules/commander/Commander.hpp
  6. 14
      src/modules/commander/commander_tests/state_machine_helper_test.cpp
  7. 78
      src/modules/commander/state_machine_helper.cpp
  8. 4
      src/modules/events/set_leds.cpp
  9. 4
      src/modules/gyro_calibration/GyroCalibration.cpp
  10. 2
      src/modules/mag_bias_estimator/MagBiasEstimator.cpp
  11. 2
      src/modules/mavlink/streams/HEARTBEAT.hpp
  12. 2
      src/modules/mavlink/streams/HIGH_LATENCY2.hpp

32
msg/vehicle_status_flags.msg
Unescape View File

@ -2,22 +2,22 @@
uint64 timestamp # time since system start (microseconds) uint64 timestamp # time since system start (microseconds)
bool condition_calibration_enabled bool calibration_enabled
bool condition_system_sensors_initialized bool system_sensors_initialized
bool condition_system_hotplug_timeout # true if the hotplug sensor search is over bool system_hotplug_timeout # true if the hotplug sensor search is over
bool condition_auto_mission_available bool auto_mission_available
bool condition_angular_velocity_valid bool angular_velocity_valid
bool condition_attitude_valid bool attitude_valid
bool condition_local_altitude_valid bool local_altitude_valid
bool condition_local_position_valid # set to true by the commander app if the quality of the local position estimate is good enough to use for navigation bool local_position_valid # set to true by the commander app if the quality of the local position estimate is good enough to use for navigation
bool condition_local_velocity_valid # set to true by the commander app if the quality of the local horizontal velocity data is good enough to use for navigation bool local_velocity_valid # set to true by the commander app if the quality of the local horizontal velocity data is good enough to use for navigation
bool condition_global_position_valid # set to true by the commander app if the quality of the global position estimate is good enough to use for navigation bool global_position_valid # set to true by the commander app if the quality of the global position estimate is good enough to use for navigation
bool condition_gps_position_valid bool gps_position_valid
bool condition_home_position_valid # indicates a valid home position (a valid home position is not always a valid launch) bool home_position_valid # indicates a valid home position (a valid home position is not always a valid launch)
bool condition_power_input_valid # set if input power is valid bool power_input_valid # set if input power is valid
bool condition_battery_healthy # set if battery is available and not low bool battery_healthy # set if battery is available and not low
bool condition_escs_error # set to true if one or more ESCs reporting esc_status are offline bool escs_error # set to true if one or more ESCs reporting esc_status are offline
bool condition_escs_failure # set to true if one or more ESCs reporting esc_status has a failure bool escs_failure # set to true if one or more ESCs reporting esc_status has a failure
bool position_reliant_on_gps bool position_reliant_on_gps
bool position_reliant_on_optical_flow bool position_reliant_on_optical_flow

6
src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp
Unescape View File

@ -54,8 +54,8 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
vehicle_status_flags_s &status_flags, const vehicle_control_mode_s &control_mode, vehicle_status_flags_s &status_flags, const vehicle_control_mode_s &control_mode,
bool report_failures, const bool prearm, const hrt_abstime &time_since_boot) bool report_failures, const bool prearm, const hrt_abstime &time_since_boot)
{ {
report_failures = (report_failures && status_flags.condition_system_hotplug_timeout report_failures = (report_failures && status_flags.system_hotplug_timeout
&& !status_flags.condition_calibration_enabled); && !status_flags.calibration_enabled);
bool failed = false; bool failed = false;
@ -173,7 +173,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
} }
/* ---- SYSTEM POWER ---- */ /* ---- SYSTEM POWER ---- */
if (status_flags.condition_power_input_valid && !status_flags.circuit_breaker_engaged_power_check) { if (status_flags.power_input_valid && !status_flags.circuit_breaker_engaged_power_check) {
if (!powerCheck(mavlink_log_pub, status, report_failures, prearm)) { if (!powerCheck(mavlink_log_pub, status, report_failures, prearm)) {
failed = true; failed = true;
} }

26
src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp
Unescape View File

@ -45,28 +45,28 @@ bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_st
bool prearm_ok = true; bool prearm_ok = true;
// rate control mode require valid angular velocity // rate control mode require valid angular velocity
if (control_mode.flag_control_rates_enabled && !status_flags.condition_angular_velocity_valid) { if (control_mode.flag_control_rates_enabled && !status_flags.angular_velocity_valid) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! angular velocity invalid"); } if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! angular velocity invalid"); }
prearm_ok = false; prearm_ok = false;
} }
// attitude control mode require valid attitude // attitude control mode require valid attitude
if (control_mode.flag_control_attitude_enabled && !status_flags.condition_attitude_valid) { if (control_mode.flag_control_attitude_enabled && !status_flags.attitude_valid) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! attitude invalid"); } if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! attitude invalid"); }
prearm_ok = false; prearm_ok = false;
} }
// velocity control mode require valid velocity // velocity control mode require valid velocity
if (control_mode.flag_control_velocity_enabled && !status_flags.condition_local_velocity_valid) { if (control_mode.flag_control_velocity_enabled && !status_flags.local_velocity_valid) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! velocity invalid"); } if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! velocity invalid"); }
prearm_ok = false; prearm_ok = false;
} }
// position control mode require valid position // position control mode require valid position
if (control_mode.flag_control_position_enabled && !status_flags.condition_local_position_valid) { if (control_mode.flag_control_position_enabled && !status_flags.local_position_valid) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! position invalid"); } if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! position invalid"); }
prearm_ok = false; prearm_ok = false;
@ -90,7 +90,7 @@ bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_st
if (!status_flags.circuit_breaker_engaged_power_check) { if (!status_flags.circuit_breaker_engaged_power_check) {
// Fail transition if power is not good // Fail transition if power is not good
if (!status_flags.condition_power_input_valid) { if (!status_flags.power_input_valid) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Connect power module"); } if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Connect power module"); }
prearm_ok = false; prearm_ok = false;
@ -98,10 +98,10 @@ bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_st
// main battery level // main battery level
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_SENSORBATTERY, true, true, set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_SENSORBATTERY, true, true,
status_flags.condition_battery_healthy, status); status_flags.battery_healthy, status);
// Only arm if healthy // Only arm if healthy
if (!status_flags.condition_battery_healthy) { if (!status_flags.battery_healthy) {
if (prearm_ok) { if (prearm_ok) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Check battery"); } if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Check battery"); }
} }
@ -113,7 +113,7 @@ bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_st
// Arm Requirements: mission // Arm Requirements: mission
if (arm_requirements.mission) { if (arm_requirements.mission) {
if (!status_flags.condition_auto_mission_available) { if (!status_flags.auto_mission_available) {
if (prearm_ok) { if (prearm_ok) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! No valid mission"); } if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! No valid mission"); }
} }
@ -121,7 +121,7 @@ bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_st
prearm_ok = false; prearm_ok = false;
} }
if (!status_flags.condition_global_position_valid) { if (!status_flags.global_position_valid) {
if (prearm_ok) { if (prearm_ok) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Missions require a global position"); } if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Missions require a global position"); }
} }
@ -132,7 +132,7 @@ bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_st
if (arm_requirements.global_position && !status_flags.circuit_breaker_engaged_posfailure_check) { if (arm_requirements.global_position && !status_flags.circuit_breaker_engaged_posfailure_check) {
if (!status_flags.condition_global_position_valid) { if (!status_flags.global_position_valid) {
if (prearm_ok) { if (prearm_ok) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Global position required"); } if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Global position required"); }
} }
@ -140,7 +140,7 @@ bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_st
prearm_ok = false; prearm_ok = false;
} }
if (!status_flags.condition_home_position_valid) { if (!status_flags.home_position_valid) {
if (prearm_ok) { if (prearm_ok) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Home position invalid"); } if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Home position invalid"); }
} }
@ -168,7 +168,7 @@ bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_st
} }
if (arm_requirements.esc_check && status_flags.condition_escs_error) { if (arm_requirements.esc_check && status_flags.escs_error) {
if (prearm_ok) { if (prearm_ok) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! One or more ESCs are offline"); } if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! One or more ESCs are offline"); }
@ -176,7 +176,7 @@ bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_st
} }
} }
if (arm_requirements.esc_check && status_flags.condition_escs_failure) { if (arm_requirements.esc_check && status_flags.escs_failure) {
if (prearm_ok) { if (prearm_ok) {
if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! One or more ESCs have a failure"); } if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! One or more ESCs have a failure"); }

154
src/modules/commander/Commander.cpp
Unescape View File

@ -716,7 +716,7 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_
} }
if ((_param_geofence_action.get() == geofence_result_s::GF_ACTION_RTL) if ((_param_geofence_action.get() == geofence_result_s::GF_ACTION_RTL)
&& !_status_flags.condition_home_position_valid) { && !_status_flags.home_position_valid) {
mavlink_log_critical(&_mavlink_log_pub, "Arming denied: Geofence RTL requires valid home\t"); mavlink_log_critical(&_mavlink_log_pub, "Arming denied: Geofence RTL requires valid home\t");
events::send(events::ID("commander_arm_denied_geofence_rtl"), events::send(events::ID("commander_arm_denied_geofence_rtl"),
{events::Log::Critical, events::LogInternal::Info}, {events::Log::Critical, events::LogInternal::Info},
@ -794,7 +794,7 @@ Commander::Commander() :
_vehicle_land_detected.landed = true; _vehicle_land_detected.landed = true;
// XXX for now just set sensors as initialized // XXX for now just set sensors as initialized
_status_flags.condition_system_sensors_initialized = true; _status_flags.system_sensors_initialized = true;
// We want to accept RC inputs as default // We want to accept RC inputs as default
_status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; _status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
@ -807,7 +807,7 @@ Commander::Commander() :
_status_flags.offboard_control_signal_lost = true; _status_flags.offboard_control_signal_lost = true;
_status_flags.condition_power_input_valid = true; _status_flags.power_input_valid = true;
_status_flags.rc_calibration_valid = true; _status_flags.rc_calibration_valid = true;
// default for vtol is rotary wing // default for vtol is rotary wing
@ -1095,7 +1095,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
fillLocalHomePos(home, home_x, home_y, home_z, yaw); fillLocalHomePos(home, home_x, home_y, home_z, yaw);
/* mark home position as set */ /* mark home position as set */
_status_flags.condition_home_position_valid = _home_pub.update(home); _status_flags.home_position_valid = _home_pub.update(home);
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
@ -1219,7 +1219,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED; cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
// check if current mission and first item are valid // check if current mission and first item are valid
if (_status_flags.condition_auto_mission_available) { if (_status_flags.auto_mission_available) {
// requested first mission item valid // requested first mission item valid
if (PX4_ISFINITE(cmd.param1) && (cmd.param1 >= -1) && (cmd.param1 < _mission_result_sub.get().seq_total)) { if (PX4_ISFINITE(cmd.param1) && (cmd.param1 >= -1) && (cmd.param1 < _mission_result_sub.get().seq_total)) {
@ -1369,7 +1369,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
if ((int)(cmd.param1) == 1) { if ((int)(cmd.param1) == 1) {
/* gyro calibration */ /* gyro calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
_status_flags.condition_calibration_enabled = true; _status_flags.calibration_enabled = true;
_worker_thread.startTask(WorkerThread::Request::GyroCalibration); _worker_thread.startTask(WorkerThread::Request::GyroCalibration);
} else if ((int)(cmd.param1) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION || } else if ((int)(cmd.param1) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION ||
@ -1381,7 +1381,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
} else if ((int)(cmd.param2) == 1) { } else if ((int)(cmd.param2) == 1) {
/* magnetometer calibration */ /* magnetometer calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
_status_flags.condition_calibration_enabled = true; _status_flags.calibration_enabled = true;
_worker_thread.startTask(WorkerThread::Request::MagCalibration); _worker_thread.startTask(WorkerThread::Request::MagCalibration);
} else if ((int)(cmd.param3) == 1) { } else if ((int)(cmd.param3) == 1) {
@ -1400,39 +1400,39 @@ Commander::handle_command(const vehicle_command_s &cmd)
} else if ((int)(cmd.param4) == 2) { } else if ((int)(cmd.param4) == 2) {
/* RC trim calibration */ /* RC trim calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
_status_flags.condition_calibration_enabled = true; _status_flags.calibration_enabled = true;
_worker_thread.startTask(WorkerThread::Request::RCTrimCalibration); _worker_thread.startTask(WorkerThread::Request::RCTrimCalibration);
} else if ((int)(cmd.param5) == 1) { } else if ((int)(cmd.param5) == 1) {
/* accelerometer calibration */ /* accelerometer calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
_status_flags.condition_calibration_enabled = true; _status_flags.calibration_enabled = true;
_worker_thread.startTask(WorkerThread::Request::AccelCalibration); _worker_thread.startTask(WorkerThread::Request::AccelCalibration);
} else if ((int)(cmd.param5) == 2) { } else if ((int)(cmd.param5) == 2) {
// board offset calibration // board offset calibration
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
_status_flags.condition_calibration_enabled = true; _status_flags.calibration_enabled = true;
_worker_thread.startTask(WorkerThread::Request::LevelCalibration); _worker_thread.startTask(WorkerThread::Request::LevelCalibration);
} else if ((int)(cmd.param5) == 4) { } else if ((int)(cmd.param5) == 4) {
// accelerometer quick calibration // accelerometer quick calibration
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
_status_flags.condition_calibration_enabled = true; _status_flags.calibration_enabled = true;
_worker_thread.startTask(WorkerThread::Request::AccelCalibrationQuick); _worker_thread.startTask(WorkerThread::Request::AccelCalibrationQuick);
} else if ((int)(cmd.param6) == 1 || (int)(cmd.param6) == 2) { } else if ((int)(cmd.param6) == 1 || (int)(cmd.param6) == 2) {
// TODO: param6 == 1 is deprecated, but we still accept it for a while (feb 2017) // TODO: param6 == 1 is deprecated, but we still accept it for a while (feb 2017)
/* airspeed calibration */ /* airspeed calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
_status_flags.condition_calibration_enabled = true; _status_flags.calibration_enabled = true;
_worker_thread.startTask(WorkerThread::Request::AirspeedCalibration); _worker_thread.startTask(WorkerThread::Request::AirspeedCalibration);
} else if ((int)(cmd.param7) == 1) { } else if ((int)(cmd.param7) == 1) {
/* do esc calibration */ /* do esc calibration */
if (check_battery_disconnected(&_mavlink_log_pub)) { if (check_battery_disconnected(&_mavlink_log_pub)) {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
_status_flags.condition_calibration_enabled = true; _status_flags.calibration_enabled = true;
_armed.in_esc_calibration_mode = true; _armed.in_esc_calibration_mode = true;
_worker_thread.startTask(WorkerThread::Request::ESCCalibration); _worker_thread.startTask(WorkerThread::Request::ESCCalibration);
@ -1489,7 +1489,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
} }
} }
_status_flags.condition_calibration_enabled = true; _status_flags.calibration_enabled = true;
_worker_thread.setMagQuickData(heading_radians, latitude, longitude); _worker_thread.setMagQuickData(heading_radians, latitude, longitude);
_worker_thread.startTask(WorkerThread::Request::MagCalibrationQuick); _worker_thread.startTask(WorkerThread::Request::MagCalibrationQuick);
} }
@ -1787,7 +1787,7 @@ Commander::set_home_position()
// Need global and local position fix to be able to set home // Need global and local position fix to be able to set home
// but already set the home position in local coordinates if available // but already set the home position in local coordinates if available
// in case the global position is only valid after takeoff // in case the global position is only valid after takeoff
if (_param_com_home_en.get() && _status_flags.condition_local_position_valid) { if (_param_com_home_en.get() && _status_flags.local_position_valid) {
// Set home position in local coordinates // Set home position in local coordinates
const vehicle_local_position_s &lpos = _local_position_sub.get(); const vehicle_local_position_s &lpos = _local_position_sub.get();
@ -1798,7 +1798,7 @@ Commander::set_home_position()
home.manual_home = false; home.manual_home = false;
fillLocalHomePos(home, lpos); fillLocalHomePos(home, lpos);
if (_status_flags.condition_global_position_valid) { if (_status_flags.global_position_valid) {
const vehicle_global_position_s &gpos = _global_position_sub.get(); const vehicle_global_position_s &gpos = _global_position_sub.get();
@ -1812,15 +1812,15 @@ Commander::set_home_position()
_home_pub.update(home); _home_pub.update(home);
} }
return _status_flags.condition_home_position_valid; return _status_flags.home_position_valid;
} }
bool bool
Commander::set_in_air_home_position() Commander::set_in_air_home_position()
{ {
if (_param_com_home_en.get() if (_param_com_home_en.get()
&& _status_flags.condition_local_position_valid && _status_flags.local_position_valid
&& _status_flags.condition_global_position_valid) { && _status_flags.global_position_valid) {
const vehicle_global_position_s &gpos = _global_position_sub.get(); const vehicle_global_position_s &gpos = _global_position_sub.get();
home_position_s home{}; home_position_s home{};
@ -1853,7 +1853,7 @@ Commander::set_in_air_home_position()
} }
} }
return _status_flags.condition_home_position_valid; return _status_flags.home_position_valid;
} }
bool bool
@ -1897,12 +1897,12 @@ void Commander::fillGlobalHomePos(home_position_s &home, double lat, double lon,
void Commander::setHomePosValid() void Commander::setHomePosValid()
{ {
// play tune first time we initialize HOME // play tune first time we initialize HOME
if (!_status_flags.condition_home_position_valid) { if (!_status_flags.home_position_valid) {
tune_home_set(true); tune_home_set(true);
} }
// mark home position as set // mark home position as set
_status_flags.condition_home_position_valid = true; _status_flags.home_position_valid = true;
} }
bool bool
@ -2137,10 +2137,10 @@ Commander::run()
!system_power.brick_valid && !system_power.brick_valid &&
!system_power.usb_connected) { !system_power.usb_connected) {
/* flying only on servo rail, this is unsafe */ /* flying only on servo rail, this is unsafe */
_status_flags.condition_power_input_valid = false; _status_flags.power_input_valid = false;
} else { } else {
_status_flags.condition_power_input_valid = true; _status_flags.power_input_valid = true;
} }
_system_power_usb_connected = system_power.usb_connected; _system_power_usb_connected = system_power.usb_connected;
@ -2273,7 +2273,7 @@ Commander::run()
} else if (_param_escs_checks_required.get() != 0) { } else if (_param_escs_checks_required.get() != 0) {
if (!_status_flags.condition_escs_error) { if (!_status_flags.escs_error) {
if ((_last_esc_status_updated != 0) && (hrt_elapsed_time(&_last_esc_status_updated) > 700_ms)) { if ((_last_esc_status_updated != 0) && (hrt_elapsed_time(&_last_esc_status_updated) > 700_ms)) {
/* Detect timeout after first telemetry packet received /* Detect timeout after first telemetry packet received
@ -2283,14 +2283,14 @@ Commander::run()
mavlink_log_critical(&_mavlink_log_pub, "ESCs telemetry timeout\t"); mavlink_log_critical(&_mavlink_log_pub, "ESCs telemetry timeout\t");
events::send(events::ID("commander_esc_telemetry_timeout"), events::Log::Critical, events::send(events::ID("commander_esc_telemetry_timeout"), events::Log::Critical,
"ESCs telemetry timeout"); "ESCs telemetry timeout");
_status_flags.condition_escs_error = true; _status_flags.escs_error = true;
} else if (_last_esc_status_updated == 0 && hrt_elapsed_time(&_boot_timestamp) > 5000_ms) { } else if (_last_esc_status_updated == 0 && hrt_elapsed_time(&_boot_timestamp) > 5000_ms) {
/* Detect if esc telemetry is not connected after reboot */ /* Detect if esc telemetry is not connected after reboot */
mavlink_log_critical(&_mavlink_log_pub, "ESCs telemetry not connected\t"); mavlink_log_critical(&_mavlink_log_pub, "ESCs telemetry not connected\t");
events::send(events::ID("commander_esc_telemetry_not_con"), events::Log::Critical, events::send(events::ID("commander_esc_telemetry_not_con"), events::Log::Critical,
"ESCs telemetry not connected"); "ESCs telemetry not connected");
_status_flags.condition_escs_error = true; _status_flags.escs_error = true;
} }
} }
} }
@ -2363,7 +2363,7 @@ Commander::run()
} }
/* If in INIT state, try to proceed to STANDBY state */ /* If in INIT state, try to proceed to STANDBY state */
if (!_status_flags.condition_calibration_enabled && _status.arming_state == vehicle_status_s::ARMING_STATE_INIT) { if (!_status_flags.calibration_enabled && _status.arming_state == vehicle_status_s::ARMING_STATE_INIT) {
arming_state_transition(_status, _vehicle_control_mode, _safety, vehicle_status_s::ARMING_STATE_STANDBY, _armed, arming_state_transition(_status, _vehicle_control_mode, _safety, vehicle_status_s::ARMING_STATE_STANDBY, _armed,
true /* fRunPreArmChecks */, &_mavlink_log_pub, _status_flags, true /* fRunPreArmChecks */, &_mavlink_log_pub, _status_flags,
@ -2382,7 +2382,7 @@ Commander::run()
const bool mission_result_ok = (mission_result.timestamp > _boot_timestamp) const bool mission_result_ok = (mission_result.timestamp > _boot_timestamp)
&& (mission_result.instance_count > 0); && (mission_result.instance_count > 0);
_status_flags.condition_auto_mission_available = mission_result_ok && mission_result.valid; _status_flags.auto_mission_available = mission_result_ok && mission_result.valid;
if (mission_result_ok) { if (mission_result_ok) {
if (_status.mission_failure != mission_result.failure) { if (_status.mission_failure != mission_result.failure) {
@ -2398,10 +2398,10 @@ Commander::run()
} }
/* Only evaluate mission state if home is set */ /* Only evaluate mission state if home is set */
if (_status_flags.condition_home_position_valid && if (_status_flags.home_position_valid &&
(prev_mission_instance_count != mission_result.instance_count)) { (prev_mission_instance_count != mission_result.instance_count)) {
if (!_status_flags.condition_auto_mission_available) { if (!_status_flags.auto_mission_available) {
/* the mission is invalid */ /* the mission is invalid */
tune_mission_fail(true); tune_mission_fail(true);
@ -2423,7 +2423,7 @@ Commander::run()
&& (mission_result.timestamp >= _status.nav_state_timestamp) && (mission_result.timestamp >= _status.nav_state_timestamp)
&& mission_result.finished) { && mission_result.finished) {
if ((_param_takeoff_finished_action.get() == 1) && _status_flags.condition_auto_mission_available) { if ((_param_takeoff_finished_action.get() == 1) && _status_flags.auto_mission_available) {
main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_MISSION, _status_flags, _internal_state); main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_MISSION, _status_flags, _internal_state);
} else { } else {
@ -2711,7 +2711,7 @@ Commander::run()
* we can as well just wait in a hold mode which enables tablet control. * we can as well just wait in a hold mode which enables tablet control.
*/ */
if (_status.rc_signal_lost && (_internal_state.main_state == commander_state_s::MAIN_STATE_MANUAL) if (_status.rc_signal_lost && (_internal_state.main_state == commander_state_s::MAIN_STATE_MANUAL)
&& _status_flags.condition_global_position_valid) { && _status_flags.global_position_valid) {
main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LOITER, _status_flags, _internal_state); main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LOITER, _status_flags, _internal_state);
} }
@ -2844,7 +2844,7 @@ Commander::run()
/* Set home position altitude to EKF origin height if home is not set and the EKF has a global origin. /* Set home position altitude to EKF origin height if home is not set and the EKF has a global origin.
* This allows home altitude to be used in the calculation of height above takeoff location when GPS * This allows home altitude to be used in the calculation of height above takeoff location when GPS
* use has commenced after takeoff. */ * use has commenced after takeoff. */
if (!_status_flags.condition_home_position_valid) { if (!_status_flags.home_position_valid) {
set_home_position_alt_only(); set_home_position_alt_only();
} }
} }
@ -2960,7 +2960,7 @@ Commander::run()
_commander_state_pub.publish(_internal_state); _commander_state_pub.publish(_internal_state);
// Evaluate current prearm status // Evaluate current prearm status
if (!_armed.armed && !_status_flags.condition_calibration_enabled) { if (!_armed.armed && !_status_flags.calibration_enabled) {
bool preflight_check_res = PreFlightCheck::preflightCheck(nullptr, _status, _status_flags, _vehicle_control_mode, bool preflight_check_res = PreFlightCheck::preflightCheck(nullptr, _status, _status_flags, _vehicle_control_mode,
false, true, hrt_elapsed_time(&_boot_timestamp)); false, true, hrt_elapsed_time(&_boot_timestamp));
@ -3031,10 +3031,10 @@ Commander::run()
} }
/* play sensor failure tunes if we already waited for hotplug sensors to come up and failed */ /* play sensor failure tunes if we already waited for hotplug sensors to come up and failed */
_status_flags.condition_system_hotplug_timeout = (hrt_elapsed_time(&_boot_timestamp) > HOTPLUG_SENS_TIMEOUT); _status_flags.system_hotplug_timeout = (hrt_elapsed_time(&_boot_timestamp) > HOTPLUG_SENS_TIMEOUT);
if (!sensor_fail_tune_played && (!_status_flags.condition_system_sensors_initialized if (!sensor_fail_tune_played && (!_status_flags.system_sensors_initialized
&& _status_flags.condition_system_hotplug_timeout)) { && _status_flags.system_hotplug_timeout)) {
set_tune_override(tune_control_s::TUNE_ID_GPS_WARNING); set_tune_override(tune_control_s::TUNE_ID_GPS_WARNING);
sensor_fail_tune_played = true; sensor_fail_tune_played = true;
@ -3062,8 +3062,8 @@ Commander::run()
int ret = _worker_thread.getResultAndReset(); int ret = _worker_thread.getResultAndReset();
_armed.in_esc_calibration_mode = false; _armed.in_esc_calibration_mode = false;
if (_status_flags.condition_calibration_enabled) { // did we do a calibration? if (_status_flags.calibration_enabled) { // did we do a calibration?
_status_flags.condition_calibration_enabled = false; _status_flags.calibration_enabled = false;
if (ret == 0) { if (ret == 0) {
tune_positive(true); tune_positive(true);
@ -3077,9 +3077,9 @@ Commander::run()
_status_changed = false; _status_changed = false;
/* store last position lock state */ /* store last position lock state */
_last_condition_local_altitude_valid = _status_flags.condition_local_altitude_valid; _last_local_altitude_valid = _status_flags.local_altitude_valid;
_last_condition_local_position_valid = _status_flags.condition_local_position_valid; _last_local_position_valid = _status_flags.local_position_valid;
_last_condition_global_position_valid = _status_flags.condition_global_position_valid; _last_global_position_valid = _status_flags.global_position_valid;
_was_armed = _armed.armed; _was_armed = _armed.armed;
@ -3145,7 +3145,7 @@ Commander::control_status_leds(bool changed, const uint8_t battery_warning)
led_mode = led_control_s::MODE_ON; led_mode = led_control_s::MODE_ON;
set_normal_color = true; set_normal_color = true;
} else if (!_status_flags.condition_system_sensors_initialized && _status_flags.condition_system_hotplug_timeout) { } else if (!_status_flags.system_sensors_initialized && _status_flags.system_hotplug_timeout) {
led_mode = led_control_s::MODE_BLINK_FAST; led_mode = led_control_s::MODE_BLINK_FAST;
led_color = led_control_s::COLOR_RED; led_color = led_control_s::COLOR_RED;
@ -3153,7 +3153,7 @@ Commander::control_status_leds(bool changed, const uint8_t battery_warning)
led_mode = led_control_s::MODE_BREATHE; led_mode = led_control_s::MODE_BREATHE;
set_normal_color = true; set_normal_color = true;
} else if (!_status_flags.condition_system_sensors_initialized && !_status_flags.condition_system_hotplug_timeout) { } else if (!_status_flags.system_sensors_initialized && !_status_flags.system_hotplug_timeout) {
led_mode = led_control_s::MODE_BREATHE; led_mode = led_control_s::MODE_BREATHE;
set_normal_color = true; set_normal_color = true;
@ -3178,7 +3178,7 @@ Commander::control_status_leds(bool changed, const uint8_t battery_warning)
led_color = led_control_s::COLOR_RED; led_color = led_control_s::COLOR_RED;
} else { } else {
if (_status_flags.condition_home_position_valid && _status_flags.condition_global_position_valid) { if (_status_flags.home_position_valid && _status_flags.global_position_valid) {
led_color = led_control_s::COLOR_GREEN; led_color = led_control_s::COLOR_GREEN;
} else { } else {
@ -3603,7 +3603,7 @@ void Commander::data_link_check()
events::send(events::ID("commander_dl_regained"), events::Log::Info, "Data link regained"); events::send(events::ID("commander_dl_regained"), events::Log::Info, "Data link regained");
} }
if (!_armed.armed && !_status_flags.condition_calibration_enabled) { if (!_armed.armed && !_status_flags.calibration_enabled) {
// make sure to report preflight check failures to a connecting GCS // make sure to report preflight check failures to a connecting GCS
PreFlightCheck::preflightCheck(&_mavlink_log_pub, _status, _status_flags, _vehicle_control_mode, PreFlightCheck::preflightCheck(&_mavlink_log_pub, _status, _status_flags, _vehicle_control_mode,
true, false, hrt_elapsed_time(&_boot_timestamp)); true, false, hrt_elapsed_time(&_boot_timestamp));
@ -3901,7 +3901,7 @@ void Commander::battery_status_check()
_battery_warning = worst_warning; _battery_warning = worst_warning;
} }
_status_flags.condition_battery_healthy = _status_flags.battery_healthy =
// All connected batteries are regularly being published // All connected batteries are regularly being published
(hrt_elapsed_time(&oldest_update) < 5_s) (hrt_elapsed_time(&oldest_update) < 5_s)
// There is at least one connected battery (in any slot) // There is at least one connected battery (in any slot)
@ -3981,7 +3981,7 @@ void Commander::estimator_check(bool force)
const vehicle_local_position_s &lpos = _local_position_sub.get(); const vehicle_local_position_s &lpos = _local_position_sub.get();
if (lpos.heading_reset_counter != _heading_reset_counter) { if (lpos.heading_reset_counter != _heading_reset_counter) {
if (_status_flags.condition_home_position_valid) { if (_status_flags.home_position_valid) {
updateHomePositionYaw(_home_pub.get().yaw + lpos.delta_heading); updateHomePositionYaw(_home_pub.get().yaw + lpos.delta_heading);
} }
@ -4134,23 +4134,23 @@ void Commander::estimator_check(bool force)
const vehicle_global_position_s &gpos = _global_position_sub.get(); const vehicle_global_position_s &gpos = _global_position_sub.get();
_status_flags.condition_global_position_valid = _status_flags.global_position_valid =
check_posvel_validity(xy_valid, gpos.eph, _param_com_pos_fs_eph.get(), gpos.timestamp, check_posvel_validity(xy_valid, gpos.eph, _param_com_pos_fs_eph.get(), gpos.timestamp,
&_last_gpos_fail_time_us, &_gpos_probation_time_us, _status_flags.condition_global_position_valid); &_last_gpos_fail_time_us, &_gpos_probation_time_us, _status_flags.global_position_valid);
_status_flags.condition_local_position_valid = _status_flags.local_position_valid =
check_posvel_validity(xy_valid, lpos.eph, lpos_eph_threshold_adj, lpos.timestamp, check_posvel_validity(xy_valid, lpos.eph, lpos_eph_threshold_adj, lpos.timestamp,
&_last_lpos_fail_time_us, &_lpos_probation_time_us, _status_flags.condition_local_position_valid); &_last_lpos_fail_time_us, &_lpos_probation_time_us, _status_flags.local_position_valid);
_status_flags.condition_local_velocity_valid = _status_flags.local_velocity_valid =
check_posvel_validity(v_xy_valid, lpos.evh, _param_com_vel_fs_evh.get(), lpos.timestamp, check_posvel_validity(v_xy_valid, lpos.evh, _param_com_vel_fs_evh.get(), lpos.timestamp,
&_last_lvel_fail_time_us, &_lvel_probation_time_us, _status_flags.condition_local_velocity_valid); &_last_lvel_fail_time_us, &_lvel_probation_time_us, _status_flags.local_velocity_valid);
} }
// altitude // altitude
_status_flags.condition_local_altitude_valid = lpos.z_valid _status_flags.local_altitude_valid = lpos.z_valid
&& (hrt_elapsed_time(&lpos.timestamp) < (_param_com_pos_fs_delay.get() * 1_s)); && (hrt_elapsed_time(&lpos.timestamp) < (_param_com_pos_fs_delay.get() * 1_s));
// attitude // attitude
@ -4163,14 +4163,14 @@ void Commander::estimator_check(bool force)
&& (fabsf(q(3)) <= 1.f); && (fabsf(q(3)) <= 1.f);
const bool norm_in_tolerance = (fabsf(1.f - q.norm()) <= 1e-6f); const bool norm_in_tolerance = (fabsf(1.f - q.norm()) <= 1e-6f);
const bool condition_attitude_valid = (hrt_elapsed_time(&attitude.timestamp) < 1_s) const bool attitude_valid = (hrt_elapsed_time(&attitude.timestamp) < 1_s)
&& norm_in_tolerance && no_element_larger_than_one; && norm_in_tolerance && no_element_larger_than_one;
if (_status_flags.condition_attitude_valid && !condition_attitude_valid) { if (_status_flags.attitude_valid && !attitude_valid) {
PX4_ERR("attitude estimate no longer valid"); PX4_ERR("attitude estimate no longer valid");
} }
_status_flags.condition_attitude_valid = condition_attitude_valid; _status_flags.attitude_valid = attitude_valid;
// angular velocity // angular velocity
@ -4180,10 +4180,10 @@ void Commander::estimator_check(bool force)
&& (hrt_elapsed_time(&angular_velocity.timestamp) < 1_s); && (hrt_elapsed_time(&angular_velocity.timestamp) < 1_s);
const bool condition_angular_velocity_finite = PX4_ISFINITE(angular_velocity.xyz[0]) const bool condition_angular_velocity_finite = PX4_ISFINITE(angular_velocity.xyz[0])
&& PX4_ISFINITE(angular_velocity.xyz[1]) && PX4_ISFINITE(angular_velocity.xyz[2]); && PX4_ISFINITE(angular_velocity.xyz[1]) && PX4_ISFINITE(angular_velocity.xyz[2]);
const bool condition_angular_velocity_valid = condition_angular_velocity_time_valid const bool angular_velocity_valid = condition_angular_velocity_time_valid
&& condition_angular_velocity_finite; && condition_angular_velocity_finite;
if (_status_flags.condition_angular_velocity_valid && !condition_angular_velocity_valid) { if (_status_flags.angular_velocity_valid && !angular_velocity_valid) {
const char err_str[] {"angular velocity no longer valid"}; const char err_str[] {"angular velocity no longer valid"};
if (!condition_angular_velocity_time_valid) { if (!condition_angular_velocity_time_valid) {
@ -4194,11 +4194,11 @@ void Commander::estimator_check(bool force)
} }
} }
_status_flags.condition_angular_velocity_valid = condition_angular_velocity_valid; _status_flags.angular_velocity_valid = angular_velocity_valid;
// gps // gps
const bool condition_gps_position_was_valid = _status_flags.condition_gps_position_valid; const bool condition_gps_position_was_valid = _status_flags.gps_position_valid;
if (_vehicle_gps_position_sub.updated() || force) { if (_vehicle_gps_position_sub.updated() || force) {
vehicle_gps_position_s vehicle_gps_position; vehicle_gps_position_s vehicle_gps_position;
@ -4213,7 +4213,7 @@ void Commander::estimator_check(bool force)
bool evh = vehicle_gps_position.s_variance_m_s < _param_com_vel_fs_evh.get(); bool evh = vehicle_gps_position.s_variance_m_s < _param_com_vel_fs_evh.get();
_vehicle_gps_position_valid.set_state_and_update(time && fix && eph && epv && evh, hrt_absolute_time()); _vehicle_gps_position_valid.set_state_and_update(time && fix && eph && epv && evh, hrt_absolute_time());
_status_flags.condition_gps_position_valid = _vehicle_gps_position_valid.get_state(); _status_flags.gps_position_valid = _vehicle_gps_position_valid.get_state();
_vehicle_gps_position_timestamp_last = vehicle_gps_position.timestamp; _vehicle_gps_position_timestamp_last = vehicle_gps_position.timestamp;
} }
@ -4223,11 +4223,11 @@ void Commander::estimator_check(bool force)
if (now_us > _vehicle_gps_position_timestamp_last + GPS_VALID_TIME) { if (now_us > _vehicle_gps_position_timestamp_last + GPS_VALID_TIME) {
_vehicle_gps_position_valid.set_state_and_update(false, now_us); _vehicle_gps_position_valid.set_state_and_update(false, now_us);
_status_flags.condition_gps_position_valid = false; _status_flags.gps_position_valid = false;
} }
} }
if (condition_gps_position_was_valid && !_status_flags.condition_gps_position_valid) { if (condition_gps_position_was_valid && !_status_flags.gps_position_valid) {
PX4_WARN("GPS no longer valid"); PX4_WARN("GPS no longer valid");
} }
} }
@ -4259,13 +4259,13 @@ Commander::offboard_control_update()
} }
} }
if (_offboard_control_mode_sub.get().position && !_status_flags.condition_local_position_valid) { if (_offboard_control_mode_sub.get().position && !_status_flags.local_position_valid) {
offboard_available = false; offboard_available = false;
} else if (_offboard_control_mode_sub.get().velocity && !_status_flags.condition_local_velocity_valid) { } else if (_offboard_control_mode_sub.get().velocity && !_status_flags.local_velocity_valid) {
offboard_available = false; offboard_available = false;
} else if (_offboard_control_mode_sub.get().acceleration && !_status_flags.condition_local_velocity_valid) { } else if (_offboard_control_mode_sub.get().acceleration && !_status_flags.local_velocity_valid) {
// OFFBOARD acceleration handled by position controller // OFFBOARD acceleration handled by position controller
offboard_available = false; offboard_available = false;
} }
@ -4296,14 +4296,14 @@ void Commander::esc_status_check()
// Check if ALL the ESCs are online // Check if ALL the ESCs are online
if (online_bitmask == esc_status.esc_online_flags) { if (online_bitmask == esc_status.esc_online_flags) {
_status_flags.condition_escs_error = false; _status_flags.escs_error = false;
_last_esc_online_flags = esc_status.esc_online_flags; _last_esc_online_flags = esc_status.esc_online_flags;
} else if (_last_esc_online_flags == esc_status.esc_online_flags) { } else if (_last_esc_online_flags == esc_status.esc_online_flags) {
// Avoid checking the status if the flags are the same or if the mixer has not yet been loaded in the ESC driver // Avoid checking the status if the flags are the same or if the mixer has not yet been loaded in the ESC driver
_status_flags.condition_escs_error = true; _status_flags.escs_error = true;
} else if (esc_status.esc_online_flags < _last_esc_online_flags) { } else if (esc_status.esc_online_flags < _last_esc_online_flags) {
@ -4323,14 +4323,14 @@ void Commander::esc_status_check()
mavlink_log_critical(&_mavlink_log_pub, "%soffline. %s\t", esc_fail_msg, _armed.armed ? "Land now!" : ""); mavlink_log_critical(&_mavlink_log_pub, "%soffline. %s\t", esc_fail_msg, _armed.armed ? "Land now!" : "");
_last_esc_online_flags = esc_status.esc_online_flags; _last_esc_online_flags = esc_status.esc_online_flags;
_status_flags.condition_escs_error = true; _status_flags.escs_error = true;
} }
_status_flags.condition_escs_failure = false; _status_flags.escs_failure = false;
for (int index = 0; index < esc_status.esc_count; index++) { for (int index = 0; index < esc_status.esc_count; index++) {
_status_flags.condition_escs_failure |= esc_status.esc[index].failures > 0; _status_flags.escs_failure |= esc_status.esc[index].failures > 0;
if (esc_status.esc[index].failures != _last_esc_failure[index]) { if (esc_status.esc[index].failures != _last_esc_failure[index]) {

6
src/modules/commander/Commander.hpp
Unescape View File

@ -361,9 +361,9 @@ private:
hrt_abstime _last_print_mode_reject_time{0}; ///< To remember when last notification was sent hrt_abstime _last_print_mode_reject_time{0}; ///< To remember when last notification was sent
bool _last_condition_local_altitude_valid{false}; bool _last_local_altitude_valid{false};
bool _last_condition_local_position_valid{false}; bool _last_local_position_valid{false};
bool _last_condition_global_position_valid{false}; bool _last_global_position_valid{false};
bool _last_overload{false}; bool _last_overload{false};

14
src/modules/commander/commander_tests/state_machine_helper_test.cpp
Unescape View File

@ -76,7 +76,7 @@ bool StateMachineHelperTest::armingStateTransitionTest()
ArmingTransitionVolatileState_t current_state; // Machine state prior to transition ArmingTransitionVolatileState_t current_state; // Machine state prior to transition
hil_state_t hil_state; // Current vehicle_status_s.hil_state hil_state_t hil_state; // Current vehicle_status_s.hil_state
bool bool
condition_system_sensors_initialized; // Current vehicle_status_s.condition_system_sensors_initialized system_sensors_initialized; // Current vehicle_status_s.system_sensors_initialized
bool safety_switch_available; // Current safety_s.safety_switch_available bool safety_switch_available; // Current safety_s.safety_switch_available
bool safety_off; // Current safety_s.safety_off bool safety_off; // Current safety_s.safety_off
arming_state_t requested_state; // Requested arming state to transition to arming_state_t requested_state; // Requested arming state to transition to
@ -290,7 +290,7 @@ bool StateMachineHelperTest::armingStateTransitionTest()
// Setup initial machine state // Setup initial machine state
status.arming_state = test->current_state.arming_state; status.arming_state = test->current_state.arming_state;
status_flags.condition_system_sensors_initialized = test->condition_system_sensors_initialized; status_flags.system_sensors_initialized = test->system_sensors_initialized;
status.hil_state = test->hil_state; status.hil_state = test->hil_state;
// The power status of the test unit is not relevant for the unit test // The power status of the test unit is not relevant for the unit test
status_flags.circuit_breaker_engaged_power_check = true; status_flags.circuit_breaker_engaged_power_check = true;
@ -512,11 +512,11 @@ bool StateMachineHelperTest::mainStateTransitionTest()
current_commander_state.main_state = test->from_state; current_commander_state.main_state = test->from_state;
current_vehicle_status.vehicle_type = (test->condition_bits & MTT_ROTARY_WING) ? current_vehicle_status.vehicle_type = (test->condition_bits & MTT_ROTARY_WING) ?
vehicle_status_s::VEHICLE_TYPE_ROTARY_WING : vehicle_status_s::VEHICLE_TYPE_FIXED_WING; vehicle_status_s::VEHICLE_TYPE_ROTARY_WING : vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
current_status_flags.condition_local_altitude_valid = test->condition_bits & MTT_LOC_ALT_VALID; current_status_flags.local_altitude_valid = test->condition_bits & MTT_LOC_ALT_VALID;
current_status_flags.condition_local_position_valid = test->condition_bits & MTT_LOC_POS_VALID; current_status_flags.local_position_valid = test->condition_bits & MTT_LOC_POS_VALID;
current_status_flags.condition_home_position_valid = test->condition_bits & MTT_HOME_POS_VALID; current_status_flags.home_position_valid = test->condition_bits & MTT_HOME_POS_VALID;
current_status_flags.condition_global_position_valid = test->condition_bits & MTT_GLOBAL_POS_VALID; current_status_flags.global_position_valid = test->condition_bits & MTT_GLOBAL_POS_VALID;
current_status_flags.condition_auto_mission_available = true; current_status_flags.auto_mission_available = true;
// Attempt transition // Attempt transition
transition_result_t result = main_state_transition(current_vehicle_status, test->to_state, current_status_flags, transition_result_t result = main_state_transition(current_vehicle_status, test->to_state, current_status_flags,

78
src/modules/commander/state_machine_helper.cpp
Unescape View File

@ -192,14 +192,14 @@ transition_result_t arming_state_transition(vehicle_status_s &status,
true, true, time_since_boot); true, true, time_since_boot);
if (preflight_check_ret) { if (preflight_check_ret) {
status_flags.condition_system_sensors_initialized = true; status_flags.system_sensors_initialized = true;
} }
feedback_provided = true; feedback_provided = true;
} }
/* re-run the pre-flight check as long as sensors are failing */ /* re-run the pre-flight check as long as sensors are failing */
if (!status_flags.condition_system_sensors_initialized if (!status_flags.system_sensors_initialized
&& fRunPreArmChecks && fRunPreArmChecks
&& ((new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) && ((new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)
|| (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY)) || (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY))
@ -207,7 +207,7 @@ transition_result_t arming_state_transition(vehicle_status_s &status,
if ((last_preflight_check == 0) || (hrt_elapsed_time(&last_preflight_check) > 1000 * 1000)) { if ((last_preflight_check == 0) || (hrt_elapsed_time(&last_preflight_check) > 1000 * 1000)) {
status_flags.condition_system_sensors_initialized = PreFlightCheck::preflightCheck(mavlink_log_pub, status, status_flags.system_sensors_initialized = PreFlightCheck::preflightCheck(mavlink_log_pub, status,
status_flags, control_mode, false, status.arming_state != vehicle_status_s::ARMING_STATE_ARMED, status_flags, control_mode, false, status.arming_state != vehicle_status_s::ARMING_STATE_ARMED,
time_since_boot); time_since_boot);
@ -246,7 +246,7 @@ transition_result_t arming_state_transition(vehicle_status_s &status,
if (hil_enabled) { if (hil_enabled) {
/* enforce lockdown in HIL */ /* enforce lockdown in HIL */
armed.lockdown = true; armed.lockdown = true;
status_flags.condition_system_sensors_initialized = true; status_flags.system_sensors_initialized = true;
/* recover from a prearm fail */ /* recover from a prearm fail */
if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) { if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {
@ -264,7 +264,7 @@ transition_result_t arming_state_transition(vehicle_status_s &status,
(status.arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR)) { (status.arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR)) {
// Sensors need to be initialized for STANDBY state, except for HIL // Sensors need to be initialized for STANDBY state, except for HIL
if (!status_flags.condition_system_sensors_initialized) { if (!status_flags.system_sensors_initialized) {
feedback_provided = true; feedback_provided = true;
valid_transition = false; valid_transition = false;
} }
@ -333,8 +333,8 @@ main_state_transition(const vehicle_status_s &status, const main_state_t new_mai
case commander_state_s::MAIN_STATE_ALTCTL: case commander_state_s::MAIN_STATE_ALTCTL:
/* need at minimum altitude estimate */ /* need at minimum altitude estimate */
if (status_flags.condition_local_altitude_valid || if (status_flags.local_altitude_valid ||
status_flags.condition_global_position_valid) { status_flags.global_position_valid) {
ret = TRANSITION_CHANGED; ret = TRANSITION_CHANGED;
} }
@ -343,8 +343,8 @@ main_state_transition(const vehicle_status_s &status, const main_state_t new_mai
case commander_state_s::MAIN_STATE_POSCTL: case commander_state_s::MAIN_STATE_POSCTL:
/* need at minimum local position estimate */ /* need at minimum local position estimate */
if (status_flags.condition_local_position_valid || if (status_flags.local_position_valid ||
status_flags.condition_global_position_valid) { status_flags.global_position_valid) {
ret = TRANSITION_CHANGED; ret = TRANSITION_CHANGED;
} }
@ -353,7 +353,7 @@ main_state_transition(const vehicle_status_s &status, const main_state_t new_mai
case commander_state_s::MAIN_STATE_AUTO_LOITER: case commander_state_s::MAIN_STATE_AUTO_LOITER:
/* need global position estimate */ /* need global position estimate */
if (status_flags.condition_global_position_valid) { if (status_flags.global_position_valid) {
ret = TRANSITION_CHANGED; ret = TRANSITION_CHANGED;
} }
@ -379,8 +379,8 @@ main_state_transition(const vehicle_status_s &status, const main_state_t new_mai
case commander_state_s::MAIN_STATE_AUTO_MISSION: case commander_state_s::MAIN_STATE_AUTO_MISSION:
/* need global position, home position, and a valid mission */ /* need global position, home position, and a valid mission */
if (status_flags.condition_global_position_valid && if (status_flags.global_position_valid &&
status_flags.condition_auto_mission_available) { status_flags.auto_mission_available) {
ret = TRANSITION_CHANGED; ret = TRANSITION_CHANGED;
} }
@ -390,7 +390,7 @@ main_state_transition(const vehicle_status_s &status, const main_state_t new_mai
case commander_state_s::MAIN_STATE_AUTO_RTL: case commander_state_s::MAIN_STATE_AUTO_RTL:
/* need global position and home position */ /* need global position and home position */
if (status_flags.condition_global_position_valid && status_flags.condition_home_position_valid) { if (status_flags.global_position_valid && status_flags.home_position_valid) {
ret = TRANSITION_CHANGED; ret = TRANSITION_CHANGED;
} }
@ -400,7 +400,7 @@ main_state_transition(const vehicle_status_s &status, const main_state_t new_mai
case commander_state_s::MAIN_STATE_AUTO_LAND: case commander_state_s::MAIN_STATE_AUTO_LAND:
/* need local position */ /* need local position */
if (status_flags.condition_local_position_valid) { if (status_flags.local_position_valid) {
ret = TRANSITION_CHANGED; ret = TRANSITION_CHANGED;
} }
@ -409,8 +409,8 @@ main_state_transition(const vehicle_status_s &status, const main_state_t new_mai
case commander_state_s::MAIN_STATE_AUTO_PRECLAND: case commander_state_s::MAIN_STATE_AUTO_PRECLAND:
/* need local and global position, and precision land only implemented for multicopters */ /* need local and global position, and precision land only implemented for multicopters */
if (status_flags.condition_local_position_valid if (status_flags.local_position_valid
&& status_flags.condition_global_position_valid && status_flags.global_position_valid
&& status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { && status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
ret = TRANSITION_CHANGED; ret = TRANSITION_CHANGED;
@ -863,11 +863,11 @@ bool check_invalid_pos_nav_state(vehicle_status_s &status, bool old_failsafe, or
{ {
bool fallback_required = false; bool fallback_required = false;
if (using_global_pos && !status_flags.condition_global_position_valid) { if (using_global_pos && !status_flags.global_position_valid) {
fallback_required = true; fallback_required = true;
} else if (!using_global_pos } else if (!using_global_pos
&& (!status_flags.condition_local_position_valid || !status_flags.condition_local_velocity_valid)) { && (!status_flags.local_position_valid || !status_flags.local_velocity_valid)) {
fallback_required = true; fallback_required = true;
} }
@ -876,10 +876,10 @@ bool check_invalid_pos_nav_state(vehicle_status_s &status, bool old_failsafe, or
if (use_rc) { if (use_rc) {
// fallback to a mode that gives the operator stick control // fallback to a mode that gives the operator stick control
if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
&& status_flags.condition_local_position_valid) { && status_flags.local_position_valid) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL; status.nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
} else if (status_flags.condition_local_altitude_valid) { } else if (status_flags.local_altitude_valid) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL; status.nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
} else { } else {
@ -888,10 +888,10 @@ bool check_invalid_pos_nav_state(vehicle_status_s &status, bool old_failsafe, or
} else { } else {
// go into a descent that does not require stick control // go into a descent that does not require stick control
if (status_flags.condition_local_position_valid) { if (status_flags.local_position_valid) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
} else if (status_flags.condition_local_altitude_valid) { } else if (status_flags.local_altitude_valid) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; status.nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
@ -930,7 +930,7 @@ void set_link_loss_nav_state(vehicle_status_s &status, actuator_armed_s &armed,
break; break;
case link_loss_actions_t::AUTO_RTL: case link_loss_actions_t::AUTO_RTL:
if (status_flags.condition_global_position_valid && status_flags.condition_home_position_valid) { if (status_flags.global_position_valid && status_flags.home_position_valid) {
main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags, internal_state); main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags, internal_state);
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
return; return;
@ -938,23 +938,23 @@ void set_link_loss_nav_state(vehicle_status_s &status, actuator_armed_s &armed,
// FALLTHROUGH // FALLTHROUGH
case link_loss_actions_t::AUTO_LOITER: case link_loss_actions_t::AUTO_LOITER:
if (status_flags.condition_global_position_valid) { if (status_flags.global_position_valid) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
return; return;
} }
// FALLTHROUGH // FALLTHROUGH
case link_loss_actions_t::AUTO_LAND: case link_loss_actions_t::AUTO_LAND:
if (status_flags.condition_global_position_valid) { if (status_flags.global_position_valid) {
main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LAND, status_flags, internal_state); main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LAND, status_flags, internal_state);
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
return; return;
} else { } else {
if (status_flags.condition_local_position_valid) { if (status_flags.local_position_valid) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
} else if (status_flags.condition_local_altitude_valid) { } else if (status_flags.local_altitude_valid) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; status.nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} }
@ -1004,28 +1004,28 @@ void set_offboard_loss_nav_state(vehicle_status_s &status, actuator_armed_s &arm
return; return;
case offboard_loss_actions_t::AUTO_RTL: case offboard_loss_actions_t::AUTO_RTL:
if (status_flags.condition_global_position_valid && status_flags.condition_home_position_valid) { if (status_flags.global_position_valid && status_flags.home_position_valid) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
return; return;
} }
// FALLTHROUGH // FALLTHROUGH
case offboard_loss_actions_t::AUTO_LOITER: case offboard_loss_actions_t::AUTO_LOITER:
if (status_flags.condition_global_position_valid) { if (status_flags.global_position_valid) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
return; return;
} }
// FALLTHROUGH // FALLTHROUGH
case offboard_loss_actions_t::AUTO_LAND: case offboard_loss_actions_t::AUTO_LAND:
if (status_flags.condition_global_position_valid) { if (status_flags.global_position_valid) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
return; return;
} }
} }
// If none of the above worked, try to mitigate // If none of the above worked, try to mitigate
if (status_flags.condition_local_altitude_valid) { if (status_flags.local_altitude_valid) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; status.nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
} else { } else {
@ -1053,19 +1053,19 @@ void set_offboard_loss_rc_nav_state(vehicle_status_s &status, actuator_armed_s &
case offboard_loss_rc_actions_t::MANUAL_POSITION: case offboard_loss_rc_actions_t::MANUAL_POSITION:
if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
&& status_flags.condition_local_position_valid) { && status_flags.local_position_valid) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL; status.nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
return; return;
} else if (status_flags.condition_global_position_valid) { } else if (status_flags.global_position_valid) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL; status.nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
return; return;
} }
// FALLTHROUGH // FALLTHROUGH
case offboard_loss_rc_actions_t::MANUAL_ALTITUDE: case offboard_loss_rc_actions_t::MANUAL_ALTITUDE:
if (status_flags.condition_local_altitude_valid) { if (status_flags.local_altitude_valid) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL; status.nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
return; return;
} }
@ -1076,28 +1076,28 @@ void set_offboard_loss_rc_nav_state(vehicle_status_s &status, actuator_armed_s &
return; return;
case offboard_loss_rc_actions_t::AUTO_RTL: case offboard_loss_rc_actions_t::AUTO_RTL:
if (status_flags.condition_global_position_valid && status_flags.condition_home_position_valid) { if (status_flags.global_position_valid && status_flags.home_position_valid) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
return; return;
} }
// FALLTHROUGH // FALLTHROUGH
case offboard_loss_rc_actions_t::AUTO_LOITER: case offboard_loss_rc_actions_t::AUTO_LOITER:
if (status_flags.condition_global_position_valid) { if (status_flags.global_position_valid) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
return; return;
} }
// FALLTHROUGH // FALLTHROUGH
case offboard_loss_rc_actions_t::AUTO_LAND: case offboard_loss_rc_actions_t::AUTO_LAND:
if (status_flags.condition_global_position_valid) { if (status_flags.global_position_valid) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
return; return;
} }
} }
// If none of the above worked, try to mitigate // If none of the above worked, try to mitigate
if (status_flags.condition_local_altitude_valid) { if (status_flags.local_altitude_valid) {
//TODO: Add case for rover //TODO: Add case for rover
status.nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; status.nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
@ -1261,7 +1261,7 @@ void imbalanced_prop_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_statu
case imbalanced_propeller_action_t::RETURN: case imbalanced_propeller_action_t::RETURN:
if (status_flags.condition_global_position_valid && status_flags.condition_home_position_valid) { if (status_flags.global_position_valid && status_flags.home_position_valid) {
if (!(internal_state->main_state == commander_state_s::MAIN_STATE_AUTO_RTL || if (!(internal_state->main_state == commander_state_s::MAIN_STATE_AUTO_RTL ||
internal_state->main_state == commander_state_s::MAIN_STATE_AUTO_LAND || internal_state->main_state == commander_state_s::MAIN_STATE_AUTO_LAND ||
internal_state->main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND)) { internal_state->main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND)) {

4
src/modules/events/set_leds.cpp
Unescape View File

@ -54,8 +54,8 @@ namespace status
void StatusDisplay::set_leds() void StatusDisplay::set_leds()
{ {
bool gps_lock_valid = _vehicle_status_flags_sub.get().condition_global_position_valid; bool gps_lock_valid = _vehicle_status_flags_sub.get().global_position_valid;
bool home_position_valid = _vehicle_status_flags_sub.get().condition_home_position_valid; bool home_position_valid = _vehicle_status_flags_sub.get().home_position_valid;
int nav_state = _vehicle_status_sub.get().nav_state; int nav_state = _vehicle_status_sub.get().nav_state;
#if defined(BOARD_FRONT_LED_MASK) #if defined(BOARD_FRONT_LED_MASK)

4
src/modules/gyro_calibration/GyroCalibration.cpp
Unescape View File

@ -98,8 +98,8 @@ void GyroCalibration::Run()
vehicle_status_flags_s vehicle_status_flags; vehicle_status_flags_s vehicle_status_flags;
if (_vehicle_status_flags_sub.copy(&vehicle_status_flags)) { if (_vehicle_status_flags_sub.copy(&vehicle_status_flags)) {
if (_system_calibrating != vehicle_status_flags.condition_calibration_enabled) { if (_system_calibrating != vehicle_status_flags.calibration_enabled) {
_system_calibrating = vehicle_status_flags.condition_calibration_enabled; _system_calibrating = vehicle_status_flags.calibration_enabled;
Reset(); Reset();
return; return;
} }

2
src/modules/mag_bias_estimator/MagBiasEstimator.cpp
Unescape View File

@ -134,7 +134,7 @@ void MagBiasEstimator::Run()
vehicle_status_flags_s vehicle_status_flags; vehicle_status_flags_s vehicle_status_flags;
if (_vehicle_status_flags_sub.copy(&vehicle_status_flags)) { if (_vehicle_status_flags_sub.copy(&vehicle_status_flags)) {
bool system_calibrating = vehicle_status_flags.condition_calibration_enabled; bool system_calibrating = vehicle_status_flags.calibration_enabled;
if (system_calibrating != _system_calibrating) { if (system_calibrating != _system_calibrating) {
_system_calibrating = system_calibrating; _system_calibrating = system_calibrating;

2
src/modules/mavlink/streams/HEARTBEAT.hpp
Unescape View File

@ -134,7 +134,7 @@ private:
} else if (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL) { } else if (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL) {
system_status = MAV_STATE_EMERGENCY; system_status = MAV_STATE_EMERGENCY;
} else if (vehicle_status_flags.condition_calibration_enabled) { } else if (vehicle_status_flags.calibration_enabled) {
system_status = MAV_STATE_CALIBRATING; system_status = MAV_STATE_CALIBRATING;
} }

2
src/modules/mavlink/streams/HIGH_LATENCY2.hpp
Unescape View File

@ -460,7 +460,7 @@ private:
vehicle_status_flags_s status_flags; vehicle_status_flags_s status_flags;
if (_status_flags_sub.update(&status_flags)) { if (_status_flags_sub.update(&status_flags)) {
if (!status_flags.condition_global_position_valid) { //TODO check if there is a better way to get only GPS failure if (!status_flags.global_position_valid) { //TODO check if there is a better way to get only GPS failure
msg->failure_flags |= HL_FAILURE_FLAG_GPS; msg->failure_flags |= HL_FAILURE_FLAG_GPS;
} }

Write Preview
Loading…
Cancel
Save