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
commit
a7ddaf08c4
  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

@ -2,22 +2,22 @@ @@ -2,22 +2,22 @@
uint64 timestamp # time since system start (microseconds)
bool condition_calibration_enabled
bool condition_system_sensors_initialized
bool condition_system_hotplug_timeout # true if the hotplug sensor search is over
bool condition_auto_mission_available
bool condition_angular_velocity_valid
bool condition_attitude_valid
bool condition_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 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 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 condition_gps_position_valid
bool condition_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 condition_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 condition_escs_failure # set to true if one or more ESCs reporting esc_status has a failure
bool calibration_enabled
bool system_sensors_initialized
bool system_hotplug_timeout # true if the hotplug sensor search is over
bool auto_mission_available
bool angular_velocity_valid
bool attitude_valid
bool local_altitude_valid
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 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 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 gps_position_valid
bool home_position_valid # indicates a valid home position (a valid home position is not always a valid launch)
bool power_input_valid # set if input power is valid
bool battery_healthy # set if battery is available and not low
bool escs_error # set to true if one or more ESCs reporting esc_status are offline
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_optical_flow

6
src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp

@ -54,8 +54,8 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu @@ -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,
bool report_failures, const bool prearm, const hrt_abstime &time_since_boot)
{
report_failures = (report_failures && status_flags.condition_system_hotplug_timeout
&& !status_flags.condition_calibration_enabled);
report_failures = (report_failures && status_flags.system_hotplug_timeout
&& !status_flags.calibration_enabled);
bool failed = false;
@ -173,7 +173,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu @@ -173,7 +173,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
}
/* ---- 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)) {
failed = true;
}

26
src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp

@ -45,28 +45,28 @@ bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_st @@ -45,28 +45,28 @@ bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_st
bool prearm_ok = true;
// 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"); }
prearm_ok = false;
}
// 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"); }
prearm_ok = false;
}
// 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"); }
prearm_ok = false;
}
// 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"); }
prearm_ok = false;
@ -90,7 +90,7 @@ bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_st @@ -90,7 +90,7 @@ bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_st
if (!status_flags.circuit_breaker_engaged_power_check) {
// 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"); }
prearm_ok = false;
@ -98,10 +98,10 @@ bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_st @@ -98,10 +98,10 @@ bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_st
// main battery level
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
if (!status_flags.condition_battery_healthy) {
if (!status_flags.battery_healthy) {
if (prearm_ok) {
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 @@ -113,7 +113,7 @@ bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_st
// Arm Requirements: mission
if (arm_requirements.mission) {
if (!status_flags.condition_auto_mission_available) {
if (!status_flags.auto_mission_available) {
if (prearm_ok) {
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 @@ -121,7 +121,7 @@ bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_st
prearm_ok = false;
}
if (!status_flags.condition_global_position_valid) {
if (!status_flags.global_position_valid) {
if (prearm_ok) {
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 @@ -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 (!status_flags.condition_global_position_valid) {
if (!status_flags.global_position_valid) {
if (prearm_ok) {
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 @@ -140,7 +140,7 @@ bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_st
prearm_ok = false;
}
if (!status_flags.condition_home_position_valid) {
if (!status_flags.home_position_valid) {
if (prearm_ok) {
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 @@ -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 (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 @@ -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 (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! One or more ESCs have a failure"); }

154
src/modules/commander/Commander.cpp

@ -716,7 +716,7 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_ @@ -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)
&& !_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");
events::send(events::ID("commander_arm_denied_geofence_rtl"),
{events::Log::Critical, events::LogInternal::Info},
@ -794,7 +794,7 @@ Commander::Commander() : @@ -794,7 +794,7 @@ Commander::Commander() :
_vehicle_land_detected.landed = true;
// 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
_status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
@ -807,7 +807,7 @@ Commander::Commander() : @@ -807,7 +807,7 @@ Commander::Commander() :
_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;
// default for vtol is rotary wing
@ -1095,7 +1095,7 @@ Commander::handle_command(const vehicle_command_s &cmd) @@ -1095,7 +1095,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
fillLocalHomePos(home, home_x, home_y, home_z, yaw);
/* 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;
@ -1219,7 +1219,7 @@ Commander::handle_command(const vehicle_command_s &cmd) @@ -1219,7 +1219,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
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) {
if (_status_flags.auto_mission_available) {
// requested first mission item valid
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) @@ -1369,7 +1369,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
if ((int)(cmd.param1) == 1) {
/* gyro calibration */
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);
} else if ((int)(cmd.param1) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION ||
@ -1381,7 +1381,7 @@ Commander::handle_command(const vehicle_command_s &cmd) @@ -1381,7 +1381,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
} else if ((int)(cmd.param2) == 1) {
/* magnetometer calibration */
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);
} else if ((int)(cmd.param3) == 1) {
@ -1400,39 +1400,39 @@ Commander::handle_command(const vehicle_command_s &cmd) @@ -1400,39 +1400,39 @@ Commander::handle_command(const vehicle_command_s &cmd)
} else if ((int)(cmd.param4) == 2) {
/* RC trim calibration */
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);
} else if ((int)(cmd.param5) == 1) {
/* accelerometer calibration */
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);
} else if ((int)(cmd.param5) == 2) {
// board offset calibration
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);
} else if ((int)(cmd.param5) == 4) {
// accelerometer quick calibration
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);
} 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)
/* airspeed calibration */
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);
} else if ((int)(cmd.param7) == 1) {
/* do esc calibration */
if (check_battery_disconnected(&_mavlink_log_pub)) {
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;
_worker_thread.startTask(WorkerThread::Request::ESCCalibration);
@ -1489,7 +1489,7 @@ Commander::handle_command(const vehicle_command_s &cmd) @@ -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.startTask(WorkerThread::Request::MagCalibrationQuick);
}
@ -1787,7 +1787,7 @@ Commander::set_home_position() @@ -1787,7 +1787,7 @@ Commander::set_home_position()
// Need global and local position fix to be able to set home
// but already set the home position in local coordinates if available
// 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
const vehicle_local_position_s &lpos = _local_position_sub.get();
@ -1798,7 +1798,7 @@ Commander::set_home_position() @@ -1798,7 +1798,7 @@ Commander::set_home_position()
home.manual_home = false;
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();
@ -1812,15 +1812,15 @@ Commander::set_home_position() @@ -1812,15 +1812,15 @@ Commander::set_home_position()
_home_pub.update(home);
}
return _status_flags.condition_home_position_valid;
return _status_flags.home_position_valid;
}
bool
Commander::set_in_air_home_position()
{
if (_param_com_home_en.get()
&& _status_flags.condition_local_position_valid
&& _status_flags.condition_global_position_valid) {
&& _status_flags.local_position_valid
&& _status_flags.global_position_valid) {
const vehicle_global_position_s &gpos = _global_position_sub.get();
home_position_s home{};
@ -1853,7 +1853,7 @@ Commander::set_in_air_home_position() @@ -1853,7 +1853,7 @@ Commander::set_in_air_home_position()
}
}
return _status_flags.condition_home_position_valid;
return _status_flags.home_position_valid;
}
bool
@ -1897,12 +1897,12 @@ void Commander::fillGlobalHomePos(home_position_s &home, double lat, double lon, @@ -1897,12 +1897,12 @@ void Commander::fillGlobalHomePos(home_position_s &home, double lat, double lon,
void Commander::setHomePosValid()
{
// play tune first time we initialize HOME
if (!_status_flags.condition_home_position_valid) {
if (!_status_flags.home_position_valid) {
tune_home_set(true);
}
// mark home position as set
_status_flags.condition_home_position_valid = true;
_status_flags.home_position_valid = true;
}
bool
@ -2137,10 +2137,10 @@ Commander::run() @@ -2137,10 +2137,10 @@ Commander::run()
!system_power.brick_valid &&
!system_power.usb_connected) {
/* flying only on servo rail, this is unsafe */
_status_flags.condition_power_input_valid = false;
_status_flags.power_input_valid = false;
} else {
_status_flags.condition_power_input_valid = true;
_status_flags.power_input_valid = true;
}
_system_power_usb_connected = system_power.usb_connected;
@ -2273,7 +2273,7 @@ Commander::run() @@ -2273,7 +2273,7 @@ Commander::run()
} 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)) {
/* Detect timeout after first telemetry packet received
@ -2283,14 +2283,14 @@ Commander::run() @@ -2283,14 +2283,14 @@ Commander::run()
mavlink_log_critical(&_mavlink_log_pub, "ESCs telemetry timeout\t");
events::send(events::ID("commander_esc_telemetry_timeout"), events::Log::Critical,
"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) {
/* Detect if esc telemetry is not connected after reboot */
mavlink_log_critical(&_mavlink_log_pub, "ESCs telemetry not connected\t");
events::send(events::ID("commander_esc_telemetry_not_con"), events::Log::Critical,
"ESCs telemetry not connected");
_status_flags.condition_escs_error = true;
_status_flags.escs_error = true;
}
}
}
@ -2363,7 +2363,7 @@ Commander::run() @@ -2363,7 +2363,7 @@ Commander::run()
}
/* 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,
true /* fRunPreArmChecks */, &_mavlink_log_pub, _status_flags,
@ -2382,7 +2382,7 @@ Commander::run() @@ -2382,7 +2382,7 @@ Commander::run()
const bool mission_result_ok = (mission_result.timestamp > _boot_timestamp)
&& (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 (_status.mission_failure != mission_result.failure) {
@ -2398,10 +2398,10 @@ Commander::run() @@ -2398,10 +2398,10 @@ Commander::run()
}
/* 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)) {
if (!_status_flags.condition_auto_mission_available) {
if (!_status_flags.auto_mission_available) {
/* the mission is invalid */
tune_mission_fail(true);
@ -2423,7 +2423,7 @@ Commander::run() @@ -2423,7 +2423,7 @@ Commander::run()
&& (mission_result.timestamp >= _status.nav_state_timestamp)
&& 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);
} else {
@ -2711,7 +2711,7 @@ Commander::run() @@ -2711,7 +2711,7 @@ Commander::run()
* 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)
&& _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);
}
@ -2844,7 +2844,7 @@ Commander::run() @@ -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.
* This allows home altitude to be used in the calculation of height above takeoff location when GPS
* use has commenced after takeoff. */
if (!_status_flags.condition_home_position_valid) {
if (!_status_flags.home_position_valid) {
set_home_position_alt_only();
}
}
@ -2960,7 +2960,7 @@ Commander::run() @@ -2960,7 +2960,7 @@ Commander::run()
_commander_state_pub.publish(_internal_state);
// 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,
false, true, hrt_elapsed_time(&_boot_timestamp));
@ -3031,10 +3031,10 @@ Commander::run() @@ -3031,10 +3031,10 @@ Commander::run()
}
/* 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
&& _status_flags.condition_system_hotplug_timeout)) {
if (!sensor_fail_tune_played && (!_status_flags.system_sensors_initialized
&& _status_flags.system_hotplug_timeout)) {
set_tune_override(tune_control_s::TUNE_ID_GPS_WARNING);
sensor_fail_tune_played = true;
@ -3062,8 +3062,8 @@ Commander::run() @@ -3062,8 +3062,8 @@ Commander::run()
int ret = _worker_thread.getResultAndReset();
_armed.in_esc_calibration_mode = false;
if (_status_flags.condition_calibration_enabled) { // did we do a calibration?
_status_flags.condition_calibration_enabled = false;
if (_status_flags.calibration_enabled) { // did we do a calibration?
_status_flags.calibration_enabled = false;
if (ret == 0) {
tune_positive(true);
@ -3077,9 +3077,9 @@ Commander::run() @@ -3077,9 +3077,9 @@ Commander::run()
_status_changed = false;
/* store last position lock state */
_last_condition_local_altitude_valid = _status_flags.condition_local_altitude_valid;
_last_condition_local_position_valid = _status_flags.condition_local_position_valid;
_last_condition_global_position_valid = _status_flags.condition_global_position_valid;
_last_local_altitude_valid = _status_flags.local_altitude_valid;
_last_local_position_valid = _status_flags.local_position_valid;
_last_global_position_valid = _status_flags.global_position_valid;
_was_armed = _armed.armed;
@ -3145,7 +3145,7 @@ Commander::control_status_leds(bool changed, const uint8_t battery_warning) @@ -3145,7 +3145,7 @@ Commander::control_status_leds(bool changed, const uint8_t battery_warning)
led_mode = led_control_s::MODE_ON;
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_color = led_control_s::COLOR_RED;
@ -3153,7 +3153,7 @@ Commander::control_status_leds(bool changed, const uint8_t battery_warning) @@ -3153,7 +3153,7 @@ Commander::control_status_leds(bool changed, const uint8_t battery_warning)
led_mode = led_control_s::MODE_BREATHE;
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;
set_normal_color = true;
@ -3178,7 +3178,7 @@ Commander::control_status_leds(bool changed, const uint8_t battery_warning) @@ -3178,7 +3178,7 @@ Commander::control_status_leds(bool changed, const uint8_t battery_warning)
led_color = led_control_s::COLOR_RED;
} 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;
} else {
@ -3603,7 +3603,7 @@ void Commander::data_link_check() @@ -3603,7 +3603,7 @@ void Commander::data_link_check()
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
PreFlightCheck::preflightCheck(&_mavlink_log_pub, _status, _status_flags, _vehicle_control_mode,
true, false, hrt_elapsed_time(&_boot_timestamp));
@ -3901,7 +3901,7 @@ void Commander::battery_status_check() @@ -3901,7 +3901,7 @@ void Commander::battery_status_check()
_battery_warning = worst_warning;
}
_status_flags.condition_battery_healthy =
_status_flags.battery_healthy =
// All connected batteries are regularly being published
(hrt_elapsed_time(&oldest_update) < 5_s)
// There is at least one connected battery (in any slot)
@ -3981,7 +3981,7 @@ void Commander::estimator_check(bool force) @@ -3981,7 +3981,7 @@ void Commander::estimator_check(bool force)
const vehicle_local_position_s &lpos = _local_position_sub.get();
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);
}
@ -4134,23 +4134,23 @@ void Commander::estimator_check(bool force) @@ -4134,23 +4134,23 @@ void Commander::estimator_check(bool force)
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,
&_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,
&_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,
&_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
_status_flags.condition_local_altitude_valid = lpos.z_valid
&& (hrt_elapsed_time(&lpos.timestamp) < (_param_com_pos_fs_delay.get() * 1_s));
_status_flags.local_altitude_valid = lpos.z_valid
&& (hrt_elapsed_time(&lpos.timestamp) < (_param_com_pos_fs_delay.get() * 1_s));
// attitude
@ -4163,14 +4163,14 @@ void Commander::estimator_check(bool force) @@ -4163,14 +4163,14 @@ void Commander::estimator_check(bool force)
&& (fabsf(q(3)) <= 1.f);
const bool norm_in_tolerance = (fabsf(1.f - q.norm()) <= 1e-6f);
const bool condition_attitude_valid = (hrt_elapsed_time(&attitude.timestamp) < 1_s)
&& norm_in_tolerance && no_element_larger_than_one;
const bool attitude_valid = (hrt_elapsed_time(&attitude.timestamp) < 1_s)
&& 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");
}
_status_flags.condition_attitude_valid = condition_attitude_valid;
_status_flags.attitude_valid = attitude_valid;
// angular velocity
@ -4180,10 +4180,10 @@ void Commander::estimator_check(bool force) @@ -4180,10 +4180,10 @@ void Commander::estimator_check(bool force)
&& (hrt_elapsed_time(&angular_velocity.timestamp) < 1_s);
const bool condition_angular_velocity_finite = PX4_ISFINITE(angular_velocity.xyz[0])
&& PX4_ISFINITE(angular_velocity.xyz[1]) && PX4_ISFINITE(angular_velocity.xyz[2]);
const bool condition_angular_velocity_valid = condition_angular_velocity_time_valid
&& condition_angular_velocity_finite;
const bool angular_velocity_valid = condition_angular_velocity_time_valid
&& 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"};
if (!condition_angular_velocity_time_valid) {
@ -4194,11 +4194,11 @@ void Commander::estimator_check(bool force) @@ -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
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) {
vehicle_gps_position_s vehicle_gps_position;
@ -4213,7 +4213,7 @@ void Commander::estimator_check(bool force) @@ -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();
_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;
}
@ -4223,11 +4223,11 @@ void Commander::estimator_check(bool force) @@ -4223,11 +4223,11 @@ void Commander::estimator_check(bool force)
if (now_us > _vehicle_gps_position_timestamp_last + GPS_VALID_TIME) {
_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");
}
}
@ -4259,13 +4259,13 @@ Commander::offboard_control_update() @@ -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;
} 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;
} 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_available = false;
}
@ -4296,14 +4296,14 @@ void Commander::esc_status_check() @@ -4296,14 +4296,14 @@ void Commander::esc_status_check()
// Check if ALL the ESCs are online
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;
} 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
_status_flags.condition_escs_error = true;
_status_flags.escs_error = true;
} else if (esc_status.esc_online_flags < _last_esc_online_flags) {
@ -4323,14 +4323,14 @@ void Commander::esc_status_check() @@ -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!" : "");
_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++) {
_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]) {

6
src/modules/commander/Commander.hpp

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

14
src/modules/commander/commander_tests/state_machine_helper_test.cpp

@ -76,7 +76,7 @@ bool StateMachineHelperTest::armingStateTransitionTest() @@ -76,7 +76,7 @@ bool StateMachineHelperTest::armingStateTransitionTest()
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
system_sensors_initialized; // Current vehicle_status_s.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
@ -290,7 +290,7 @@ bool StateMachineHelperTest::armingStateTransitionTest() @@ -290,7 +290,7 @@ bool StateMachineHelperTest::armingStateTransitionTest()
// Setup initial machine 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;
// The power status of the test unit is not relevant for the unit test
status_flags.circuit_breaker_engaged_power_check = true;
@ -512,11 +512,11 @@ bool StateMachineHelperTest::mainStateTransitionTest() @@ -512,11 +512,11 @@ bool StateMachineHelperTest::mainStateTransitionTest()
current_commander_state.main_state = test->from_state;
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;
current_status_flags.condition_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.condition_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.condition_auto_mission_available = true;
current_status_flags.local_altitude_valid = test->condition_bits & MTT_LOC_ALT_VALID;
current_status_flags.local_position_valid = test->condition_bits & MTT_LOC_POS_VALID;
current_status_flags.home_position_valid = test->condition_bits & MTT_HOME_POS_VALID;
current_status_flags.global_position_valid = test->condition_bits & MTT_GLOBAL_POS_VALID;
current_status_flags.auto_mission_available = true;
// Attempt transition
transition_result_t result = main_state_transition(current_vehicle_status, test->to_state, current_status_flags,

78
src/modules/commander/state_machine_helper.cpp

@ -192,14 +192,14 @@ transition_result_t arming_state_transition(vehicle_status_s &status, @@ -192,14 +192,14 @@ transition_result_t arming_state_transition(vehicle_status_s &status,
true, true, time_since_boot);
if (preflight_check_ret) {
status_flags.condition_system_sensors_initialized = true;
status_flags.system_sensors_initialized = true;
}
feedback_provided = true;
}
/* 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
&& ((new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)
|| (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY))
@ -207,7 +207,7 @@ transition_result_t arming_state_transition(vehicle_status_s &status, @@ -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)) {
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,
time_since_boot);
@ -246,7 +246,7 @@ transition_result_t arming_state_transition(vehicle_status_s &status, @@ -246,7 +246,7 @@ transition_result_t arming_state_transition(vehicle_status_s &status,
if (hil_enabled) {
/* enforce lockdown in HIL */
armed.lockdown = true;
status_flags.condition_system_sensors_initialized = true;
status_flags.system_sensors_initialized = true;
/* recover from a prearm fail */
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, @@ -264,7 +264,7 @@ transition_result_t arming_state_transition(vehicle_status_s &status,
(status.arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR)) {
// 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;
valid_transition = false;
}
@ -333,8 +333,8 @@ main_state_transition(const vehicle_status_s &status, const main_state_t new_mai @@ -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:
/* need at minimum altitude estimate */
if (status_flags.condition_local_altitude_valid ||
status_flags.condition_global_position_valid) {
if (status_flags.local_altitude_valid ||
status_flags.global_position_valid) {
ret = TRANSITION_CHANGED;
}
@ -343,8 +343,8 @@ main_state_transition(const vehicle_status_s &status, const main_state_t new_mai @@ -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:
/* need at minimum local position estimate */
if (status_flags.condition_local_position_valid ||
status_flags.condition_global_position_valid) {
if (status_flags.local_position_valid ||
status_flags.global_position_valid) {
ret = TRANSITION_CHANGED;
}
@ -353,7 +353,7 @@ main_state_transition(const vehicle_status_s &status, const main_state_t new_mai @@ -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:
/* need global position estimate */
if (status_flags.condition_global_position_valid) {
if (status_flags.global_position_valid) {
ret = TRANSITION_CHANGED;
}
@ -379,8 +379,8 @@ main_state_transition(const vehicle_status_s &status, const main_state_t new_mai @@ -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:
/* need global position, home position, and a valid mission */
if (status_flags.condition_global_position_valid &&
status_flags.condition_auto_mission_available) {
if (status_flags.global_position_valid &&
status_flags.auto_mission_available) {
ret = TRANSITION_CHANGED;
}
@ -390,7 +390,7 @@ main_state_transition(const vehicle_status_s &status, const main_state_t new_mai @@ -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:
/* 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;
}
@ -400,7 +400,7 @@ main_state_transition(const vehicle_status_s &status, const main_state_t new_mai @@ -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:
/* need local position */
if (status_flags.condition_local_position_valid) {
if (status_flags.local_position_valid) {
ret = TRANSITION_CHANGED;
}
@ -409,8 +409,8 @@ main_state_transition(const vehicle_status_s &status, const main_state_t new_mai @@ -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:
/* need local and global position, and precision land only implemented for multicopters */
if (status_flags.condition_local_position_valid
&& status_flags.condition_global_position_valid
if (status_flags.local_position_valid
&& status_flags.global_position_valid
&& status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
ret = TRANSITION_CHANGED;
@ -863,11 +863,11 @@ bool check_invalid_pos_nav_state(vehicle_status_s &status, bool old_failsafe, or @@ -863,11 +863,11 @@ bool check_invalid_pos_nav_state(vehicle_status_s &status, bool old_failsafe, or
{
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;
} 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;
}
@ -876,10 +876,10 @@ bool check_invalid_pos_nav_state(vehicle_status_s &status, bool old_failsafe, or @@ -876,10 +876,10 @@ bool check_invalid_pos_nav_state(vehicle_status_s &status, bool old_failsafe, or
if (use_rc) {
// fallback to a mode that gives the operator stick control
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;
} else if (status_flags.condition_local_altitude_valid) {
} else if (status_flags.local_altitude_valid) {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
} else {
@ -888,10 +888,10 @@ bool check_invalid_pos_nav_state(vehicle_status_s &status, bool old_failsafe, or @@ -888,10 +888,10 @@ bool check_invalid_pos_nav_state(vehicle_status_s &status, bool old_failsafe, or
} else {
// 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;
} else if (status_flags.condition_local_altitude_valid) {
} else if (status_flags.local_altitude_valid) {
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, @@ -930,7 +930,7 @@ void set_link_loss_nav_state(vehicle_status_s &status, actuator_armed_s &armed,
break;
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);
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
return;
@ -938,23 +938,23 @@ void set_link_loss_nav_state(vehicle_status_s &status, actuator_armed_s &armed, @@ -938,23 +938,23 @@ void set_link_loss_nav_state(vehicle_status_s &status, actuator_armed_s &armed,
// FALLTHROUGH
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;
return;
}
// FALLTHROUGH
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);
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
return;
} else {
if (status_flags.condition_local_position_valid) {
if (status_flags.local_position_valid) {
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;
}
@ -1004,28 +1004,28 @@ void set_offboard_loss_nav_state(vehicle_status_s &status, actuator_armed_s &arm @@ -1004,28 +1004,28 @@ void set_offboard_loss_nav_state(vehicle_status_s &status, actuator_armed_s &arm
return;
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;
return;
}
// FALLTHROUGH
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;
return;
}
// FALLTHROUGH
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;
return;
}
}
// 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;
} else {
@ -1053,19 +1053,19 @@ void set_offboard_loss_rc_nav_state(vehicle_status_s &status, actuator_armed_s & @@ -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:
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;
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;
return;
}
// FALLTHROUGH
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;
return;
}
@ -1076,28 +1076,28 @@ void set_offboard_loss_rc_nav_state(vehicle_status_s &status, actuator_armed_s & @@ -1076,28 +1076,28 @@ void set_offboard_loss_rc_nav_state(vehicle_status_s &status, actuator_armed_s &
return;
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;
return;
}
// FALLTHROUGH
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;
return;
}
// FALLTHROUGH
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;
return;
}
}
// 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
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 @@ -1261,7 +1261,7 @@ void imbalanced_prop_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_statu
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 ||
internal_state->main_state == commander_state_s::MAIN_STATE_AUTO_LAND ||
internal_state->main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND)) {

4
src/modules/events/set_leds.cpp

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

4
src/modules/gyro_calibration/GyroCalibration.cpp

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

2
src/modules/mag_bias_estimator/MagBiasEstimator.cpp

@ -134,7 +134,7 @@ void MagBiasEstimator::Run() @@ -134,7 +134,7 @@ void MagBiasEstimator::Run()
vehicle_status_flags_s 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) {
_system_calibrating = system_calibrating;

2
src/modules/mavlink/streams/HEARTBEAT.hpp

@ -134,7 +134,7 @@ private: @@ -134,7 +134,7 @@ private:
} else if (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL) {
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;
}

2
src/modules/mavlink/streams/HIGH_LATENCY2.hpp

@ -460,7 +460,7 @@ private: @@ -460,7 +460,7 @@ private:
vehicle_status_flags_s 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;
}

Loading…
Cancel
Save