mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
vehicle_status_flags.msg: remove condition_ prefix to reduce message size
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
Daniel Agar
parent
c30f2b9493
commit
a7ddaf08c4
@ -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
|
||||
|
||||
@ -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
|
||||
}
|
||||
|
||||
/* ---- 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;
|
||||
}
|
||||
|
||||
@ -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
|
||||
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
|
||||
|
||||
// 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
|
||||
// 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
|
||||
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
|
||||
|
||||
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
|
||||
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
|
||||
|
||||
}
|
||||
|
||||
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
|
||||
}
|
||||
}
|
||||
|
||||
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"); }
|
||||
|
||||
|
||||
@ -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() :
|
||||
_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() :
|
||||
|
||||
_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)
|
||||
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)
|
||||
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)
|
||||
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)
|
||||
} 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)
|
||||
} 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)
|
||||
}
|
||||
}
|
||||
|
||||
_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()
|
||||
// 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()
|
||||
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()
|
||||
_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()
|
||||
}
|
||||
}
|
||||
|
||||
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,
|
||||
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()
|
||||
!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()
|
||||
|
||||
} 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()
|
||||
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()
|
||||
}
|
||||
|
||||
/* 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()
|
||||
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()
|
||||
}
|
||||
|
||||
/* 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()
|
||||
&& (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()
|
||||
* 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()
|
||||
/* 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()
|
||||
_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()
|
||||
}
|
||||
|
||||
/* 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()
|
||||
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()
|
||||
_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)
|
||||
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)
|
||||
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)
|
||||
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()
|
||||
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()
|
||||
_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)
|
||||
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)
|
||||
|
||||
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)
|
||||
&& (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)
|
||||
&& (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)
|
||||
}
|
||||
}
|
||||
|
||||
_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)
|
||||
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)
|
||||
|
||||
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()
|
||||
}
|
||||
}
|
||||
|
||||
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()
|
||||
// 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()
|
||||
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]) {
|
||||
|
||||
|
||||
@ -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};
|
||||
|
||||
|
||||
@ -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()
|
||||
|
||||
// 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()
|
||||
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,
|
||||
|
||||
@ -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,
|
||||
|
||||
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,
|
||||
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,
|
||||
(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
|
||||
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
|
||||
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
|
||||
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
|
||||
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
|
||||
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
|
||||
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
|
||||
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
|
||||
{
|
||||
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
|
||||
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
|
||||
|
||||
} 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,
|
||||
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,
|
||||
|
||||
// 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
|
||||
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 &
|
||||
|
||||
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 &
|
||||
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
|
||||
|
||||
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)) {
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
|
||||
@ -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…
x
Reference in New Issue
Block a user