forked from Archive/PX4-Autopilot
vehicle_status_flags: cleanup message, move non-failsafe flags to vehicle_status
This commit is contained in:
Beat Küng
Daniel Agar
parent
ae6377dfa0
commit
e4bb219d10
|
|
@ -85,10 +85,8 @@ uint8 VEHICLE_TYPE_AIRSHIP = 4
|
|||
|
||||
bool failsafe # true if system is in failsafe state (e.g.:RTL, Hover, Terminate, ...)
|
||||
bool failsafe_and_user_took_over # true if system is in failsafe state but the user took over control
|
||||
uint64 failsafe_timestamp # time when failsafe was activated
|
||||
|
||||
# Link loss
|
||||
bool rc_signal_lost # true if RC reception lost
|
||||
bool data_link_lost # datalink to GCS lost
|
||||
uint8 data_link_lost_counter # counts unique data link lost events
|
||||
bool high_latency_data_link_lost # Set to true if the high latency data link (eg. RockBlock Iridium 9603 telemetry module) is lost
|
||||
|
|
@ -99,7 +97,6 @@ bool is_vtol_tailsitter # True if the system performs a 90° pitch down rotatio
|
|||
bool in_transition_mode # True if VTOL is doing a transition
|
||||
bool in_transition_to_fw # True if VTOL is doing a transition from MC to FW
|
||||
|
||||
|
||||
# MAVLink identification
|
||||
uint8 system_type # system type, contains mavlink MAV_TYPE
|
||||
uint8 system_id # system id, contains MAVLink's system ID field
|
||||
|
|
@ -107,7 +104,6 @@ uint8 component_id # subsystem / component id, contains MAVLink's component ID f
|
|||
|
||||
bool safety_button_available # Set to true if a safety button is connected
|
||||
bool safety_off # Set to true if safety is off
|
||||
bool auto_mission_available
|
||||
|
||||
bool power_input_valid # set if input power is valid
|
||||
bool usb_connected # set to true (never cleared) once telemetry received from usb link
|
||||
|
|
@ -120,3 +116,9 @@ bool parachute_system_healthy
|
|||
|
||||
bool avoidance_system_required # Set to true if avoidance system is enabled via COM_OBS_AVOID parameter
|
||||
bool avoidance_system_valid # Status of the obstacle avoidance system
|
||||
|
||||
bool rc_calibration_in_progress
|
||||
bool calibration_enabled
|
||||
|
||||
bool pre_flight_checks_pass # true if all checks necessary to arm pass
|
||||
|
||||
|
|
|
|||
|
|
@ -1,6 +1,5 @@
|
|||
# TODO: rename to failsafe_flags (will be input to failsafe state machine)
|
||||
# Input flags for the failsafe state machine set by the arming & health checks.
|
||||
#
|
||||
# Input flags for the failsafe state machine.
|
||||
# Flags must be named such that false == no failure (e.g. _invalid, _unhealthy, _lost)
|
||||
# The flag comments are used as label for the failsafe state machine simulation
|
||||
|
||||
|
|
@ -9,10 +8,10 @@ uint64 timestamp # time since system start (microseconds)
|
|||
# Per-mode requirements
|
||||
uint32 mode_req_angular_velocity
|
||||
uint32 mode_req_attitude
|
||||
uint32 mode_req_local_alt
|
||||
uint32 mode_req_local_position
|
||||
uint32 mode_req_local_position_relaxed
|
||||
uint32 mode_req_global_position
|
||||
uint32 mode_req_local_alt
|
||||
uint32 mode_req_mission
|
||||
uint32 mode_req_offboard_signal
|
||||
uint32 mode_req_home_position
|
||||
|
|
@ -20,39 +19,39 @@ uint32 mode_req_prevent_arming # if set, cannot arm while in this mode
|
|||
uint32 mode_req_other # other requirements, not covered above (for external modes)
|
||||
|
||||
|
||||
bool calibration_enabled
|
||||
bool pre_flight_checks_pass # true if all checks necessary to arm pass
|
||||
bool auto_mission_available # Mission available
|
||||
bool angular_velocity_valid # Angular velocity valid
|
||||
bool attitude_valid # Attitude valid
|
||||
bool local_altitude_valid # Local altitude valid
|
||||
bool local_position_valid # Local position estimate valid
|
||||
bool local_position_valid_relaxed # Local position with reduced accuracy requirements (e.g. flying with optical flow)
|
||||
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 # Global position estimate valid
|
||||
bool gps_position_valid
|
||||
bool home_position_valid # Home position valid
|
||||
# Mode requirements
|
||||
bool angular_velocity_invalid # Angular velocity invalid
|
||||
bool attitude_invalid # Attitude invalid
|
||||
bool local_altitude_invalid # Local altitude invalid
|
||||
bool local_position_invalid # Local position estimate invalid
|
||||
bool local_position_invalid_relaxed # Local position with reduced accuracy requirements invalid (e.g. flying with optical flow)
|
||||
bool local_velocity_invalid # Local velocity estimate invalid
|
||||
bool global_position_invalid # Global position estimate invalid
|
||||
bool gps_position_invalid # GPS position invalid
|
||||
bool auto_mission_missing # No mission available
|
||||
bool offboard_control_signal_lost # Offboard signal lost
|
||||
bool home_position_invalid # No home position available
|
||||
|
||||
bool offboard_control_signal_lost # Offboard signal lost
|
||||
bool geofence_violated # Geofence violated
|
||||
# Control links
|
||||
bool rc_signal_lost # RC signal lost
|
||||
bool data_link_lost # Data link lost
|
||||
|
||||
bool rc_signal_lost # RC signal lost
|
||||
bool data_link_lost # Data link lost
|
||||
bool mission_failure # Mission failure
|
||||
bool rc_calibration_in_progress
|
||||
bool vtol_transition_failure # VTOL transition failed
|
||||
# Battery
|
||||
uint8 battery_warning # Battery warning level
|
||||
bool battery_low_remaining_time # Low battery based on remaining flight time
|
||||
bool battery_unhealthy # Battery unhealthy
|
||||
|
||||
bool battery_low_remaining_time
|
||||
bool battery_unhealthy
|
||||
bool geofence_violated # Geofence violated
|
||||
bool mission_failure # Mission failure
|
||||
bool vtol_transition_failure # VTOL transition failed (quadchute)
|
||||
|
||||
bool wind_limit_exceeded
|
||||
bool flight_time_limit_exceeded
|
||||
bool wind_limit_exceeded # Wind limit exceeded
|
||||
bool flight_time_limit_exceeded # Maximum flight time exceeded
|
||||
|
||||
# failure detector
|
||||
bool fd_esc_arming_failure
|
||||
bool fd_critical_failure # Critical failure (attitude/altitude limit exceeded, or external ATS)
|
||||
bool fd_imbalanced_prop # Imbalanced propeller detected
|
||||
bool fd_motor_failure
|
||||
# Failure detector
|
||||
bool fd_critical_failure # Critical failure (attitude/altitude limit exceeded, or external ATS)
|
||||
bool fd_esc_arming_failure # ESC failed to arm
|
||||
bool fd_imbalanced_prop # Imbalanced propeller detected
|
||||
bool fd_motor_failure # Motor failure
|
||||
|
||||
uint8 battery_warning # Battery warning level
|
||||
|
||||
|
|
|
|||
|
|
@ -292,11 +292,10 @@ int Commander::custom_command(int argc, char *argv[])
|
|||
}
|
||||
|
||||
if (!strcmp(argv[0], "check")) {
|
||||
uORB::SubscriptionData<vehicle_status_s> vehicle_status_sub{ORB_ID(vehicle_status)};
|
||||
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_RUN_PREARM_CHECKS);
|
||||
|
||||
uORB::SubscriptionData<vehicle_status_flags_s> vehicle_status_flags_sub{ORB_ID(vehicle_status_flags)};
|
||||
PX4_INFO("Preflight check: %s", vehicle_status_flags_sub.get().pre_flight_checks_pass ? "OK" : "FAILED");
|
||||
uORB::SubscriptionData<vehicle_status_s> vehicle_status_sub{ORB_ID(vehicle_status)};
|
||||
PX4_INFO("Preflight check: %s", vehicle_status_sub.get().pre_flight_checks_pass ? "OK" : "FAILED");
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -657,8 +656,6 @@ Commander::Commander() :
|
|||
/* mark all signals lost as long as they haven't been found */
|
||||
_vehicle_status.data_link_lost = true;
|
||||
|
||||
_vehicle_status_flags.offboard_control_signal_lost = true;
|
||||
|
||||
_vehicle_status.power_input_valid = true;
|
||||
|
||||
// default for vtol is rotary wing
|
||||
|
|
@ -1051,7 +1048,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
|||
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED;
|
||||
|
||||
// check if current mission and first item are valid
|
||||
if (_vehicle_status.auto_mission_available) {
|
||||
if (!_vehicle_status_flags.auto_mission_missing) {
|
||||
|
||||
// requested first mission item valid
|
||||
if (PX4_ISFINITE(cmd.param1) && (cmd.param1 >= -1) && (cmd.param1 < _mission_result_sub.get().seq_total)) {
|
||||
|
|
@ -1201,7 +1198,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
|||
if ((int)(cmd.param1) == 1) {
|
||||
/* gyro calibration */
|
||||
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
_vehicle_status_flags.calibration_enabled = true;
|
||||
_vehicle_status.calibration_enabled = true;
|
||||
_worker_thread.startTask(WorkerThread::Request::GyroCalibration);
|
||||
|
||||
} else if ((int)(cmd.param1) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION ||
|
||||
|
|
@ -1213,20 +1210,20 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
|||
} else if ((int)(cmd.param2) == 1) {
|
||||
/* magnetometer calibration */
|
||||
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
_vehicle_status_flags.calibration_enabled = true;
|
||||
_vehicle_status.calibration_enabled = true;
|
||||
_worker_thread.startTask(WorkerThread::Request::MagCalibration);
|
||||
|
||||
} else if ((int)(cmd.param3) == 1) {
|
||||
/* baro calibration */
|
||||
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
_vehicle_status_flags.calibration_enabled = true;
|
||||
_vehicle_status.calibration_enabled = true;
|
||||
_worker_thread.startTask(WorkerThread::Request::BaroCalibration);
|
||||
|
||||
} else if ((int)(cmd.param4) == 1) {
|
||||
/* RC calibration */
|
||||
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
/* disable RC control input completely */
|
||||
_vehicle_status_flags.rc_calibration_in_progress = true;
|
||||
_vehicle_status.rc_calibration_in_progress = true;
|
||||
mavlink_log_info(&_mavlink_log_pub, "Calibration: Disabling RC input\t");
|
||||
events::send(events::ID("commander_calib_rc_off"), events::Log::Info,
|
||||
"Calibration: Disabling RC input");
|
||||
|
|
@ -1234,32 +1231,32 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
|||
} else if ((int)(cmd.param4) == 2) {
|
||||
/* RC trim calibration */
|
||||
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
_vehicle_status_flags.calibration_enabled = true;
|
||||
_vehicle_status.calibration_enabled = true;
|
||||
_worker_thread.startTask(WorkerThread::Request::RCTrimCalibration);
|
||||
|
||||
} else if ((int)(cmd.param5) == 1) {
|
||||
/* accelerometer calibration */
|
||||
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
_vehicle_status_flags.calibration_enabled = true;
|
||||
_vehicle_status.calibration_enabled = true;
|
||||
_worker_thread.startTask(WorkerThread::Request::AccelCalibration);
|
||||
|
||||
} else if ((int)(cmd.param5) == 2) {
|
||||
// board offset calibration
|
||||
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
_vehicle_status_flags.calibration_enabled = true;
|
||||
_vehicle_status.calibration_enabled = true;
|
||||
_worker_thread.startTask(WorkerThread::Request::LevelCalibration);
|
||||
|
||||
} else if ((int)(cmd.param5) == 4) {
|
||||
// accelerometer quick calibration
|
||||
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
_vehicle_status_flags.calibration_enabled = true;
|
||||
_vehicle_status.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_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
_vehicle_status_flags.calibration_enabled = true;
|
||||
_vehicle_status.calibration_enabled = true;
|
||||
_worker_thread.startTask(WorkerThread::Request::AirspeedCalibration);
|
||||
|
||||
} else if ((int)(cmd.param7) == 1) {
|
||||
|
|
@ -1273,7 +1270,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
|||
"ESCs calibration denied");
|
||||
|
||||
} else {
|
||||
_vehicle_status_flags.calibration_enabled = true;
|
||||
_vehicle_status.calibration_enabled = true;
|
||||
_actuator_armed.in_esc_calibration_mode = true;
|
||||
_worker_thread.startTask(WorkerThread::Request::ESCCalibration);
|
||||
}
|
||||
|
|
@ -1284,9 +1281,9 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
|||
|
||||
} else if ((int)(cmd.param4) == 0) {
|
||||
/* RC calibration ended - have we been in one worth confirming? */
|
||||
if (_vehicle_status_flags.rc_calibration_in_progress) {
|
||||
if (_vehicle_status.rc_calibration_in_progress) {
|
||||
/* enable RC control input */
|
||||
_vehicle_status_flags.rc_calibration_in_progress = false;
|
||||
_vehicle_status.rc_calibration_in_progress = false;
|
||||
mavlink_log_info(&_mavlink_log_pub, "Calibration: Restoring RC input\t");
|
||||
events::send(events::ID("commander_calib_rc_on"), events::Log::Info,
|
||||
"Calibration: Restoring RC input");
|
||||
|
|
@ -1329,7 +1326,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
|||
}
|
||||
}
|
||||
|
||||
_vehicle_status_flags.calibration_enabled = true;
|
||||
_vehicle_status.calibration_enabled = true;
|
||||
_worker_thread.setMagQuickData(heading_radians, latitude, longitude);
|
||||
_worker_thread.startTask(WorkerThread::Request::MagCalibrationQuick);
|
||||
}
|
||||
|
|
@ -1495,7 +1492,7 @@ void Commander::executeActionRequest(const action_request_s &action_request)
|
|||
arm_disarm_reason_t arm_disarm_reason{};
|
||||
|
||||
// Silently ignore RC actions during RC calibration
|
||||
if (_vehicle_status_flags.rc_calibration_in_progress
|
||||
if (_vehicle_status.rc_calibration_in_progress
|
||||
&& (action_request.source == action_request_s::SOURCE_RC_STICK_GESTURE
|
||||
|| action_request.source == action_request_s::SOURCE_RC_SWITCH
|
||||
|| action_request.source == action_request_s::SOURCE_RC_BUTTON
|
||||
|
|
@ -1595,7 +1592,6 @@ void Commander::updateParameters()
|
|||
_vehicle_status.avoidance_system_required = _param_com_obs_avoid.get();
|
||||
|
||||
_auto_disarm_killed.set_hysteresis_time_from(false, _param_com_kill_disarm.get() * 1_s);
|
||||
_offboard_available.set_hysteresis_time_from(true, _param_com_of_loss_t.get() * 1_s);
|
||||
|
||||
const bool is_rotary = is_rotary_wing(_vehicle_status) || (is_vtol(_vehicle_status)
|
||||
&& _vtol_vehicle_status.vehicle_vtol_state != vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW);
|
||||
|
|
@ -1684,8 +1680,6 @@ Commander::run()
|
|||
|
||||
handlePowerButtonState();
|
||||
|
||||
offboard_control_update();
|
||||
|
||||
systemPowerUpdate();
|
||||
|
||||
landDetectorUpdate();
|
||||
|
|
@ -1701,7 +1695,7 @@ Commander::run()
|
|||
battery_status_check();
|
||||
|
||||
/* If in INIT state, try to proceed to STANDBY state */
|
||||
if (!_vehicle_status_flags.calibration_enabled && _arm_state_machine.isInit()) {
|
||||
if (!_vehicle_status.calibration_enabled && _arm_state_machine.isInit()) {
|
||||
|
||||
_arm_state_machine.arming_state_transition(_vehicle_status,
|
||||
vehicle_status_s::ARMING_STATE_STANDBY, _actuator_armed, _health_and_arming_checks,
|
||||
|
|
@ -1784,11 +1778,10 @@ Commander::run()
|
|||
|| !(_actuator_armed == actuator_armed_prev)) {
|
||||
|
||||
// Evaluate current prearm status (skip during arm <-> disarm transitions as checks are run there already)
|
||||
if (_actuator_armed.armed == actuator_armed_prev.armed && !_vehicle_status_flags.calibration_enabled) {
|
||||
if (_actuator_armed.armed == actuator_armed_prev.armed && !_vehicle_status.calibration_enabled) {
|
||||
perf_begin(_preflight_check_perf);
|
||||
_health_and_arming_checks.update();
|
||||
_vehicle_status_flags.pre_flight_checks_pass = _health_and_arming_checks.canArm(
|
||||
_vehicle_status.nav_state);
|
||||
_vehicle_status.pre_flight_checks_pass = _health_and_arming_checks.canArm(_vehicle_status.nav_state);
|
||||
perf_end(_preflight_check_perf);
|
||||
|
||||
check_and_inform_ready_for_takeoff();
|
||||
|
|
@ -1868,14 +1861,14 @@ void Commander::checkForMissionUpdate()
|
|||
const bool mission_result_ok = (mission_result.timestamp > _boot_timestamp)
|
||||
&& (mission_result.instance_count > 0);
|
||||
|
||||
_vehicle_status.auto_mission_available = mission_result_ok && mission_result.valid;
|
||||
bool auto_mission_available = mission_result_ok && mission_result.valid;
|
||||
|
||||
if (mission_result_ok) {
|
||||
/* Only evaluate mission state if home is set */
|
||||
if (_vehicle_status_flags.home_position_valid &&
|
||||
if (!_vehicle_status_flags.home_position_invalid &&
|
||||
(prev_mission_instance_count != mission_result.instance_count)) {
|
||||
|
||||
if (!_vehicle_status.auto_mission_available) {
|
||||
if (!auto_mission_available) {
|
||||
/* the mission is invalid */
|
||||
tune_mission_fail(true);
|
||||
|
||||
|
|
@ -1897,7 +1890,7 @@ void Commander::checkForMissionUpdate()
|
|||
&& (mission_result.timestamp >= _vehicle_status.nav_state_timestamp)
|
||||
&& mission_result.finished) {
|
||||
|
||||
if ((_param_takeoff_finished_action.get() == 1) && _vehicle_status.auto_mission_available) {
|
||||
if ((_param_takeoff_finished_action.get() == 1) && auto_mission_available) {
|
||||
_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION);
|
||||
|
||||
} else {
|
||||
|
|
@ -2112,8 +2105,8 @@ void Commander::checkWorkerThread()
|
|||
int ret = _worker_thread.getResultAndReset();
|
||||
_actuator_armed.in_esc_calibration_mode = false;
|
||||
|
||||
if (_vehicle_status_flags.calibration_enabled) { // did we do a calibration?
|
||||
_vehicle_status_flags.calibration_enabled = false;
|
||||
if (_vehicle_status.calibration_enabled) { // did we do a calibration?
|
||||
_vehicle_status.calibration_enabled = false;
|
||||
|
||||
if (ret == 0) {
|
||||
tune_positive(true);
|
||||
|
|
@ -2312,7 +2305,7 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning)
|
|||
led_mode = led_control_s::MODE_ON;
|
||||
set_normal_color = true;
|
||||
|
||||
} else if (!_vehicle_status_flags.pre_flight_checks_pass) {
|
||||
} else if (!_vehicle_status.pre_flight_checks_pass) {
|
||||
led_mode = led_control_s::MODE_BLINK_FAST;
|
||||
led_color = led_control_s::COLOR_RED;
|
||||
|
||||
|
|
@ -2342,7 +2335,7 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning)
|
|||
led_color = led_control_s::COLOR_RED;
|
||||
|
||||
} else {
|
||||
if (_vehicle_status_flags.home_position_valid && _vehicle_status_flags.global_position_valid) {
|
||||
if (!_vehicle_status_flags.home_position_invalid && !_vehicle_status_flags.global_position_invalid) {
|
||||
led_color = led_control_s::COLOR_GREEN;
|
||||
|
||||
} else {
|
||||
|
|
@ -2413,6 +2406,7 @@ void
|
|||
Commander::update_control_mode()
|
||||
{
|
||||
_vehicle_control_mode = {};
|
||||
_offboard_control_mode_sub.update();
|
||||
mode_util::getVehicleControlMode(_arm_state_machine.isArmed(), _vehicle_status.nav_state,
|
||||
_vehicle_status.vehicle_type, _offboard_control_mode_sub.get(), _vehicle_control_mode);
|
||||
_vehicle_control_mode.timestamp = hrt_absolute_time();
|
||||
|
|
@ -2786,54 +2780,6 @@ void Commander::manual_control_check()
|
|||
}
|
||||
}
|
||||
|
||||
void
|
||||
Commander::offboard_control_update()
|
||||
{
|
||||
bool offboard_available = false;
|
||||
|
||||
if (_offboard_control_mode_sub.updated()) {
|
||||
const offboard_control_mode_s old = _offboard_control_mode_sub.get();
|
||||
|
||||
if (_offboard_control_mode_sub.update()) {
|
||||
const offboard_control_mode_s &ocm = _offboard_control_mode_sub.get();
|
||||
|
||||
if (old.position != ocm.position ||
|
||||
old.velocity != ocm.velocity ||
|
||||
old.acceleration != ocm.acceleration ||
|
||||
old.attitude != ocm.attitude ||
|
||||
old.body_rate != ocm.body_rate ||
|
||||
old.actuator != ocm.actuator) {
|
||||
|
||||
_status_changed = true;
|
||||
}
|
||||
|
||||
if (ocm.position || ocm.velocity || ocm.acceleration || ocm.attitude || ocm.body_rate || ocm.actuator) {
|
||||
offboard_available = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (_offboard_control_mode_sub.get().position && !_vehicle_status_flags.local_position_valid) {
|
||||
offboard_available = false;
|
||||
|
||||
} else if (_offboard_control_mode_sub.get().velocity && !_vehicle_status_flags.local_velocity_valid) {
|
||||
offboard_available = false;
|
||||
|
||||
} else if (_offboard_control_mode_sub.get().acceleration && !_vehicle_status_flags.local_velocity_valid) {
|
||||
// OFFBOARD acceleration handled by position controller
|
||||
offboard_available = false;
|
||||
}
|
||||
|
||||
_offboard_available.set_state_and_update(offboard_available, hrt_absolute_time());
|
||||
|
||||
const bool offboard_lost = !_offboard_available.get_state();
|
||||
|
||||
if (_vehicle_status_flags.offboard_control_signal_lost != offboard_lost) {
|
||||
_vehicle_status_flags.offboard_control_signal_lost = offboard_lost;
|
||||
_status_changed = true;
|
||||
}
|
||||
}
|
||||
|
||||
void Commander::send_parachute_command()
|
||||
{
|
||||
vehicle_command_s vcmd{};
|
||||
|
|
|
|||
|
|
@ -147,8 +147,6 @@ private:
|
|||
|
||||
void executeActionRequest(const action_request_s &action_request);
|
||||
|
||||
void offboard_control_update();
|
||||
|
||||
void print_reject_mode(uint8_t nav_state);
|
||||
|
||||
void update_control_mode();
|
||||
|
|
@ -195,9 +193,6 @@ private:
|
|||
|
||||
(ParamFloat<px4::params::COM_OBC_LOSS_T>) _param_com_obc_loss_t,
|
||||
|
||||
// Offboard
|
||||
(ParamFloat<px4::params::COM_OF_LOSS_T>) _param_com_of_loss_t,
|
||||
|
||||
(ParamInt<px4::params::COM_PREARM_MODE>) _param_com_prearm_mode,
|
||||
(ParamBool<px4::params::COM_FORCE_SAFETY>) _param_com_force_safety,
|
||||
(ParamBool<px4::params::COM_MOT_TEST_EN>) _param_com_mot_test_en,
|
||||
|
|
@ -252,7 +247,6 @@ private:
|
|||
uint8_t _battery_warning{battery_status_s::BATTERY_WARNING_NONE};
|
||||
Hysteresis _auto_disarm_landed{false};
|
||||
Hysteresis _auto_disarm_killed{false};
|
||||
Hysteresis _offboard_available{false};
|
||||
|
||||
hrt_abstime _last_print_mode_reject_time{0}; ///< To remember when last notification was sent
|
||||
bool _mode_switch_mapped{false};
|
||||
|
|
|
|||
|
|
@ -60,6 +60,7 @@ px4_add_library(health_and_arming_checks
|
|||
checks/missionCheck.cpp
|
||||
checks/rcAndDataLinkCheck.cpp
|
||||
checks/vtolCheck.cpp
|
||||
checks/offboardCheck.cpp
|
||||
)
|
||||
add_dependencies(health_and_arming_checks mode_util)
|
||||
|
||||
|
|
|
|||
|
|
@ -39,6 +39,18 @@ HealthAndArmingChecks::HealthAndArmingChecks(ModuleParams *parent, vehicle_statu
|
|||
_context(status),
|
||||
_reporter(status_flags)
|
||||
{
|
||||
// Initialize mode requirements to invalid
|
||||
_failsafe_flags.angular_velocity_invalid = true;
|
||||
_failsafe_flags.attitude_invalid = true;
|
||||
_failsafe_flags.local_altitude_invalid = true;
|
||||
_failsafe_flags.local_position_invalid = true;
|
||||
_failsafe_flags.local_position_invalid_relaxed = true;
|
||||
_failsafe_flags.local_velocity_invalid = true;
|
||||
_failsafe_flags.global_position_invalid = true;
|
||||
_failsafe_flags.gps_position_invalid = true;
|
||||
_failsafe_flags.auto_mission_missing = true;
|
||||
_failsafe_flags.offboard_control_signal_lost = true;
|
||||
_failsafe_flags.home_position_invalid = true;
|
||||
}
|
||||
|
||||
bool HealthAndArmingChecks::update(bool force_reporting)
|
||||
|
|
|
|||
|
|
@ -65,6 +65,7 @@
|
|||
#include "checks/missionCheck.hpp"
|
||||
#include "checks/rcAndDataLinkCheck.hpp"
|
||||
#include "checks/vtolCheck.hpp"
|
||||
#include "checks/offboardCheck.hpp"
|
||||
|
||||
|
||||
class HealthAndArmingChecks : public ModuleParams
|
||||
|
|
@ -132,6 +133,7 @@ private:
|
|||
MissionChecks _mission_checks;
|
||||
RcAndDataLinkChecks _rc_and_data_link_checks;
|
||||
VtolChecks _vtol_checks;
|
||||
OffboardChecks _offboard_checks;
|
||||
|
||||
HealthAndArmingCheckBase *_checks[30] = {
|
||||
&_accelerometer_checks,
|
||||
|
|
@ -147,7 +149,9 @@ private:
|
|||
&_magnetometer_checks,
|
||||
&_manual_control_checks,
|
||||
&_home_position_checks,
|
||||
&_mode_checks, // must be after _estimator_checks & _home_position_checks
|
||||
&_mission_checks,
|
||||
&_offboard_checks, // must be after _estimator_checks
|
||||
&_mode_checks, // must be after _estimator_checks, _home_position_checks, _mission_checks, _offboard_checks
|
||||
&_parachute_checks,
|
||||
&_power_checks,
|
||||
&_rc_calibration_checks,
|
||||
|
|
@ -157,7 +161,6 @@ private:
|
|||
&_wind_checks,
|
||||
&_geofence_checks, // must be after _home_position_checks
|
||||
&_flight_time_checks,
|
||||
&_mission_checks,
|
||||
&_rc_and_data_link_checks,
|
||||
&_vtol_checks,
|
||||
};
|
||||
|
|
|
|||
|
|
@ -119,11 +119,11 @@ void EstimatorChecks::checkAndReport(const Context &context, Report &reporter)
|
|||
}
|
||||
|
||||
// set mode requirements
|
||||
const bool condition_gps_position_was_valid = reporter.failsafeFlags().gps_position_valid;
|
||||
const bool condition_gps_position_was_valid = !reporter.failsafeFlags().gps_position_invalid;
|
||||
setModeRequirementFlags(context, pre_flt_fail_innov_heading, pre_flt_fail_innov_vel_horiz, lpos, vehicle_gps_position,
|
||||
reporter.failsafeFlags());
|
||||
|
||||
if (condition_gps_position_was_valid && !reporter.failsafeFlags().gps_position_valid) {
|
||||
if (condition_gps_position_was_valid && reporter.failsafeFlags().gps_position_invalid) {
|
||||
gpsNoLongerValid(context, reporter);
|
||||
}
|
||||
}
|
||||
|
|
@ -658,7 +658,7 @@ void EstimatorChecks::gpsNoLongerValid(const Context &context, Report &reporter)
|
|||
PX4_DEBUG("GPS no longer valid");
|
||||
|
||||
// report GPS failure if armed and the global position estimate is still valid
|
||||
if (context.isArmed() && reporter.failsafeFlags().global_position_valid) {
|
||||
if (context.isArmed() && !reporter.failsafeFlags().global_position_invalid) {
|
||||
if (reporter.mavlink_log_pub()) {
|
||||
mavlink_log_warning(reporter.mavlink_log_pub(), "GPS no longer valid\t");
|
||||
}
|
||||
|
|
@ -706,26 +706,26 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f
|
|||
}
|
||||
}
|
||||
|
||||
failsafe_flags.global_position_valid =
|
||||
checkPosVelValidity(now, xy_valid, gpos.eph, _param_com_pos_fs_eph.get(), gpos.timestamp,
|
||||
_last_gpos_fail_time_us, failsafe_flags.global_position_valid);
|
||||
failsafe_flags.global_position_invalid =
|
||||
!checkPosVelValidity(now, xy_valid, gpos.eph, _param_com_pos_fs_eph.get(), gpos.timestamp,
|
||||
_last_gpos_fail_time_us, !failsafe_flags.global_position_invalid);
|
||||
|
||||
failsafe_flags.local_position_valid =
|
||||
checkPosVelValidity(now, xy_valid, lpos.eph, _param_com_pos_fs_eph.get(), lpos.timestamp,
|
||||
_last_lpos_fail_time_us, failsafe_flags.local_position_valid);
|
||||
failsafe_flags.local_position_invalid =
|
||||
!checkPosVelValidity(now, xy_valid, lpos.eph, _param_com_pos_fs_eph.get(), lpos.timestamp,
|
||||
_last_lpos_fail_time_us, !failsafe_flags.local_position_invalid);
|
||||
|
||||
failsafe_flags.local_position_valid_relaxed =
|
||||
checkPosVelValidity(now, xy_valid, lpos.eph, lpos_eph_threshold_relaxed, lpos.timestamp,
|
||||
_last_lpos_relaxed_fail_time_us, failsafe_flags.local_position_valid);
|
||||
failsafe_flags.local_position_invalid_relaxed =
|
||||
!checkPosVelValidity(now, xy_valid, lpos.eph, lpos_eph_threshold_relaxed, lpos.timestamp,
|
||||
_last_lpos_relaxed_fail_time_us, !failsafe_flags.local_position_invalid_relaxed);
|
||||
|
||||
failsafe_flags.local_velocity_valid =
|
||||
checkPosVelValidity(now, v_xy_valid, lpos.evh, _param_com_vel_fs_evh.get(), lpos.timestamp,
|
||||
_last_lvel_fail_time_us, failsafe_flags.local_velocity_valid);
|
||||
failsafe_flags.local_velocity_invalid =
|
||||
!checkPosVelValidity(now, v_xy_valid, lpos.evh, _param_com_vel_fs_evh.get(), lpos.timestamp,
|
||||
_last_lvel_fail_time_us, !failsafe_flags.local_velocity_invalid);
|
||||
|
||||
|
||||
// altitude
|
||||
failsafe_flags.local_altitude_valid = lpos.z_valid
|
||||
&& (now - lpos.timestamp < (_param_com_pos_fs_delay.get() * 1_s));
|
||||
failsafe_flags.local_altitude_invalid = !lpos.z_valid
|
||||
|| (now - lpos.timestamp > (_param_com_pos_fs_delay.get() * 1_s));
|
||||
|
||||
|
||||
// attitude
|
||||
|
|
@ -740,11 +740,11 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f
|
|||
&& (fabsf(q(3)) <= 1.f + eps);
|
||||
const bool norm_in_tolerance = fabsf(1.f - q.norm()) <= eps;
|
||||
|
||||
failsafe_flags.attitude_valid = now < attitude.timestamp + 1_s && norm_in_tolerance
|
||||
&& no_element_larger_than_one;
|
||||
failsafe_flags.attitude_invalid = now > attitude.timestamp + 1_s || !norm_in_tolerance
|
||||
|| !no_element_larger_than_one;
|
||||
|
||||
} else {
|
||||
failsafe_flags.attitude_valid = false;
|
||||
failsafe_flags.attitude_invalid = true;
|
||||
}
|
||||
|
||||
// angular velocity
|
||||
|
|
@ -754,13 +754,13 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f
|
|||
&& now < 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 angular_velocity_valid = condition_angular_velocity_time_valid
|
||||
&& condition_angular_velocity_finite;
|
||||
const bool angular_velocity_invalid = !condition_angular_velocity_time_valid
|
||||
|| !condition_angular_velocity_finite;
|
||||
|
||||
if (failsafe_flags.angular_velocity_valid && !angular_velocity_valid) {
|
||||
if (!failsafe_flags.angular_velocity_invalid && angular_velocity_invalid) {
|
||||
const char err_str[] {"angular velocity no longer valid"};
|
||||
|
||||
if (!condition_angular_velocity_time_valid) {
|
||||
if (!condition_angular_velocity_time_valid && angular_velocity.timestamp != 0) {
|
||||
PX4_ERR("%s (timeout)", err_str);
|
||||
|
||||
} else if (!condition_angular_velocity_finite) {
|
||||
|
|
@ -768,7 +768,7 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f
|
|||
}
|
||||
}
|
||||
|
||||
failsafe_flags.angular_velocity_valid = angular_velocity_valid;
|
||||
failsafe_flags.angular_velocity_invalid = angular_velocity_invalid;
|
||||
|
||||
|
||||
// gps
|
||||
|
|
@ -782,10 +782,10 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f
|
|||
bool no_jamming = (vehicle_gps_position.jamming_state != sensor_gps_s::JAMMING_STATE_CRITICAL);
|
||||
|
||||
_vehicle_gps_position_valid.set_state_and_update(time && fix && eph && epv && evh && no_jamming, now);
|
||||
failsafe_flags.gps_position_valid = _vehicle_gps_position_valid.get_state();
|
||||
failsafe_flags.gps_position_invalid = !_vehicle_gps_position_valid.get_state();
|
||||
|
||||
} else {
|
||||
failsafe_flags.gps_position_valid = false;
|
||||
failsafe_flags.gps_position_invalid = true;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -59,7 +59,7 @@ void GeofenceChecks::checkAndReport(const Context &context, Report &reporter)
|
|||
}
|
||||
|
||||
if (geofence_result.geofence_action == geofence_result_s::GF_ACTION_RTL
|
||||
&& !reporter.failsafeFlags().home_position_valid) {
|
||||
&& reporter.failsafeFlags().home_position_invalid) {
|
||||
/* EVENT
|
||||
* @description
|
||||
* <profile name="dev">
|
||||
|
|
|
|||
|
|
@ -38,10 +38,10 @@ void HomePositionChecks::checkAndReport(const Context &context, Report &reporter
|
|||
home_position_s home_position;
|
||||
|
||||
if (_home_position_sub.copy(&home_position)) {
|
||||
reporter.failsafeFlags().home_position_valid = home_position.valid_alt && home_position.valid_hpos;
|
||||
reporter.failsafeFlags().home_position_invalid = !home_position.valid_alt || !home_position.valid_hpos;
|
||||
|
||||
} else {
|
||||
reporter.failsafeFlags().home_position_valid = false;
|
||||
reporter.failsafeFlags().home_position_invalid = true;
|
||||
}
|
||||
|
||||
// No need to report, as it's a mode requirement
|
||||
|
|
|
|||
|
|
@ -36,6 +36,7 @@
|
|||
void MissionChecks::checkAndReport(const Context &context, Report &reporter)
|
||||
{
|
||||
reporter.failsafeFlags().mission_failure = false;
|
||||
reporter.failsafeFlags().auto_mission_missing = true;
|
||||
mission_result_s mission_result;
|
||||
|
||||
if (_mission_result_sub.copy(&mission_result) && mission_result.valid) {
|
||||
|
|
@ -53,5 +54,8 @@ void MissionChecks::checkAndReport(const Context &context, Report &reporter)
|
|||
mavlink_log_critical(reporter.mavlink_log_pub(), "Mission cannot be completed\t");
|
||||
}
|
||||
}
|
||||
|
||||
// This is a mode requirement, no need to report
|
||||
reporter.failsafeFlags().auto_mission_missing = mission_result.instance_count <= 0;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -42,7 +42,7 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter)
|
|||
// Failing mode requirements generally also clear the can_run bits which prevents mode switching and
|
||||
// might trigger a failsafe if already in that mode.
|
||||
|
||||
if (!reporter.failsafeFlags().angular_velocity_valid && reporter.failsafeFlags().mode_req_angular_velocity != 0) {
|
||||
if (reporter.failsafeFlags().angular_velocity_invalid && reporter.failsafeFlags().mode_req_angular_velocity != 0) {
|
||||
/* EVENT
|
||||
* @description
|
||||
* Make sure the gyroscope is providing valid data.
|
||||
|
|
@ -53,7 +53,7 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter)
|
|||
reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_angular_velocity);
|
||||
}
|
||||
|
||||
if (!reporter.failsafeFlags().attitude_valid && reporter.failsafeFlags().mode_req_attitude != 0) {
|
||||
if (reporter.failsafeFlags().attitude_invalid && reporter.failsafeFlags().mode_req_attitude != 0) {
|
||||
/* EVENT
|
||||
* @description
|
||||
* Wait until the estimator initialized
|
||||
|
|
@ -66,11 +66,11 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter)
|
|||
|
||||
NavModes local_position_modes = NavModes::None;
|
||||
|
||||
if (!reporter.failsafeFlags().local_position_valid && reporter.failsafeFlags().mode_req_local_position != 0) {
|
||||
if (reporter.failsafeFlags().local_position_invalid && reporter.failsafeFlags().mode_req_local_position != 0) {
|
||||
local_position_modes = (NavModes)reporter.failsafeFlags().mode_req_local_position;
|
||||
}
|
||||
|
||||
if (!reporter.failsafeFlags().local_position_valid_relaxed
|
||||
if (reporter.failsafeFlags().local_position_invalid_relaxed
|
||||
&& reporter.failsafeFlags().mode_req_local_position_relaxed != 0) {
|
||||
local_position_modes = local_position_modes | (NavModes)reporter.failsafeFlags().mode_req_local_position_relaxed;
|
||||
}
|
||||
|
|
@ -84,7 +84,7 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter)
|
|||
reporter.clearCanRunBits(local_position_modes);
|
||||
}
|
||||
|
||||
if (!reporter.failsafeFlags().global_position_valid && reporter.failsafeFlags().mode_req_global_position != 0) {
|
||||
if (reporter.failsafeFlags().global_position_invalid && reporter.failsafeFlags().mode_req_global_position != 0) {
|
||||
/* EVENT
|
||||
*/
|
||||
reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_global_position, health_component_t::system,
|
||||
|
|
@ -93,7 +93,7 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter)
|
|||
reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_global_position);
|
||||
}
|
||||
|
||||
if (!reporter.failsafeFlags().local_altitude_valid && reporter.failsafeFlags().mode_req_local_alt != 0) {
|
||||
if (reporter.failsafeFlags().local_altitude_invalid && reporter.failsafeFlags().mode_req_local_alt != 0) {
|
||||
/* EVENT
|
||||
*/
|
||||
reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_local_alt, health_component_t::system,
|
||||
|
|
@ -102,12 +102,22 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter)
|
|||
reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_local_alt);
|
||||
}
|
||||
|
||||
if (!reporter.failsafeFlags().auto_mission_available && reporter.failsafeFlags().mode_req_mission != 0) {
|
||||
NavModes mission_required_modes = (NavModes)reporter.failsafeFlags().mode_req_mission;
|
||||
|
||||
if (_param_com_arm_mis_req.get()) {
|
||||
mission_required_modes = NavModes::All;
|
||||
}
|
||||
|
||||
if (reporter.failsafeFlags().auto_mission_missing && mission_required_modes != NavModes::None) {
|
||||
/* EVENT
|
||||
* @description
|
||||
* Upload a mission first.
|
||||
*
|
||||
* <profile name="dev">
|
||||
* This check can be configured via <param>COM_ARM_MIS_REQ</param> parameter.
|
||||
* </profile>
|
||||
*/
|
||||
reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_mission, health_component_t::system,
|
||||
reporter.armingCheckFailure(mission_required_modes, health_component_t::system,
|
||||
events::ID("check_modes_mission"),
|
||||
events::Log::Info, "No valid mission available");
|
||||
reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_mission);
|
||||
|
|
@ -116,7 +126,7 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter)
|
|||
if (reporter.failsafeFlags().offboard_control_signal_lost && reporter.failsafeFlags().mode_req_offboard_signal != 0) {
|
||||
/* EVENT
|
||||
* @description
|
||||
* The offboard component is not sending setpoints.
|
||||
* The offboard component is not sending setpoints or the required estimate (e.g. position) is missing.
|
||||
*/
|
||||
reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_offboard_signal, health_component_t::system,
|
||||
events::ID("check_modes_offboard_signal"),
|
||||
|
|
@ -124,7 +134,7 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter)
|
|||
reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_offboard_signal);
|
||||
}
|
||||
|
||||
if (!reporter.failsafeFlags().home_position_valid && reporter.failsafeFlags().mode_req_home_position != 0) {
|
||||
if (reporter.failsafeFlags().home_position_invalid && reporter.failsafeFlags().mode_req_home_position != 0) {
|
||||
/* EVENT
|
||||
*/
|
||||
reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_home_position, health_component_t::system,
|
||||
|
|
|
|||
|
|
@ -48,4 +48,7 @@ public:
|
|||
private:
|
||||
void checkArmingRequirement(const Context &context, Report &reporter);
|
||||
|
||||
DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
|
||||
(ParamBool<px4::params::COM_ARM_MIS_REQ>) _param_com_arm_mis_req
|
||||
);
|
||||
};
|
||||
|
|
|
|||
|
|
@ -0,0 +1,70 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "offboardCheck.hpp"
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
OffboardChecks::OffboardChecks()
|
||||
{
|
||||
_offboard_available.set_hysteresis_time_from(true, _param_com_of_loss_t.get() * 1_s);
|
||||
}
|
||||
|
||||
void OffboardChecks::checkAndReport(const Context &context, Report &reporter)
|
||||
{
|
||||
reporter.failsafeFlags().offboard_control_signal_lost = true;
|
||||
|
||||
offboard_control_mode_s offboard_control_mode;
|
||||
|
||||
if (_offboard_control_mode_sub.copy(&offboard_control_mode)) {
|
||||
bool offboard_available = offboard_control_mode.position || offboard_control_mode.velocity
|
||||
|| offboard_control_mode.acceleration || offboard_control_mode.attitude || offboard_control_mode.body_rate
|
||||
|| offboard_control_mode.actuator;
|
||||
|
||||
if (offboard_control_mode.position && reporter.failsafeFlags().local_position_invalid) {
|
||||
offboard_available = false;
|
||||
|
||||
} else if (offboard_control_mode.velocity && reporter.failsafeFlags().local_velocity_invalid) {
|
||||
offboard_available = false;
|
||||
|
||||
} else if (offboard_control_mode.acceleration && reporter.failsafeFlags().local_velocity_invalid) {
|
||||
// OFFBOARD acceleration handled by position controller
|
||||
offboard_available = false;
|
||||
}
|
||||
|
||||
_offboard_available.set_state_and_update(offboard_available, hrt_absolute_time());
|
||||
|
||||
// This is a mode requirement, no need to report
|
||||
reporter.failsafeFlags().offboard_control_signal_lost = !_offboard_available.get_state();
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,57 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "../Common.hpp"
|
||||
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/offboard_control_mode.h>
|
||||
#include <lib/hysteresis/hysteresis.h>
|
||||
|
||||
class OffboardChecks : public HealthAndArmingCheckBase
|
||||
{
|
||||
public:
|
||||
OffboardChecks();
|
||||
~OffboardChecks() = default;
|
||||
|
||||
void checkAndReport(const Context &context, Report &reporter) override;
|
||||
|
||||
private:
|
||||
uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
|
||||
systemlib::Hysteresis _offboard_available{false};
|
||||
|
||||
DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
|
||||
(ParamFloat<px4::params::COM_OF_LOSS_T>) _param_com_of_loss_t
|
||||
);
|
||||
};
|
||||
|
|
@ -72,29 +72,9 @@ void SystemChecks::checkAndReport(const Context &context, Report &reporter)
|
|||
}
|
||||
}
|
||||
|
||||
// Arm Requirements: mission
|
||||
if (_param_com_arm_mis_req.get() && !context.isArmed()) {
|
||||
if (!context.status().auto_mission_available) {
|
||||
/* EVENT
|
||||
* @description
|
||||
* <profile name="dev">
|
||||
* This check can be configured via <param>COM_ARM_MIS_REQ</param> parameter.
|
||||
* </profile>
|
||||
*/
|
||||
reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_system_no_mission"),
|
||||
events::Log::Error, "No valid mission");
|
||||
|
||||
if (reporter.mavlink_log_pub()) {
|
||||
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: No valid mission");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
reporter.failsafeFlags().auto_mission_available = context.status().auto_mission_available;
|
||||
|
||||
// Global position required
|
||||
if (!_param_com_arm_wo_gps.get() && !context.isArmed()) {
|
||||
if (!reporter.failsafeFlags().global_position_valid) {
|
||||
if (reporter.failsafeFlags().global_position_invalid) {
|
||||
/* EVENT
|
||||
* @description
|
||||
* <profile name="dev">
|
||||
|
|
@ -108,7 +88,7 @@ void SystemChecks::checkAndReport(const Context &context, Report &reporter)
|
|||
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Global position required");
|
||||
}
|
||||
|
||||
} else if (!reporter.failsafeFlags().home_position_valid) {
|
||||
} else if (reporter.failsafeFlags().home_position_invalid) {
|
||||
/* EVENT
|
||||
* @description
|
||||
* <profile name="dev">
|
||||
|
|
|
|||
|
|
@ -52,7 +52,6 @@ private:
|
|||
DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
|
||||
(ParamInt<px4::params::CBRK_VTOLARMING>) _param_cbrk_vtolarming,
|
||||
(ParamInt<px4::params::CBRK_USB_CHK>) _param_cbrk_usb_chk,
|
||||
(ParamBool<px4::params::COM_ARM_MIS_REQ>) _param_com_arm_mis_req,
|
||||
(ParamBool<px4::params::COM_ARM_WO_GPS>) _param_com_arm_wo_gps,
|
||||
(ParamInt<px4::params::COM_ARM_AUTH_REQ>) _param_com_arm_auth_req,
|
||||
(ParamInt<px4::params::GF_ACTION>) _param_gf_action
|
||||
|
|
|
|||
|
|
@ -69,7 +69,7 @@ bool HomePosition::hasMovedFromCurrentHomeLocation()
|
|||
eph = gpos.eph;
|
||||
epv = gpos.epv;
|
||||
|
||||
} else if (_vehicle_status_flags.gps_position_valid) {
|
||||
} else if (!_vehicle_status_flags.gps_position_invalid) {
|
||||
sensor_gps_s gps;
|
||||
_vehicle_gps_position_sub.copy(&gps);
|
||||
const double lat = static_cast<double>(gps.lat) * 1e-7;
|
||||
|
|
@ -98,7 +98,7 @@ bool HomePosition::setHomePosition(bool force)
|
|||
bool updated = false;
|
||||
home_position_s home{};
|
||||
|
||||
if (_vehicle_status_flags.local_position_valid) {
|
||||
if (!_vehicle_status_flags.local_position_invalid) {
|
||||
// Set home position in local coordinates
|
||||
const vehicle_local_position_s &lpos = _local_position_sub.get();
|
||||
_heading_reset_counter = lpos.heading_reset_counter; // TODO: should not be here
|
||||
|
|
@ -107,14 +107,14 @@ bool HomePosition::setHomePosition(bool force)
|
|||
updated = true;
|
||||
}
|
||||
|
||||
if (_vehicle_status_flags.global_position_valid) {
|
||||
if (!_vehicle_status_flags.global_position_invalid) {
|
||||
// Set home using the global position estimate (fused INS/GNSS)
|
||||
const vehicle_global_position_s &gpos = _global_position_sub.get();
|
||||
fillGlobalHomePos(home, gpos);
|
||||
setHomePosValid();
|
||||
updated = true;
|
||||
|
||||
} else if (_vehicle_status_flags.gps_position_valid) {
|
||||
} else if (!_vehicle_status_flags.gps_position_invalid) {
|
||||
// Set home using GNSS position
|
||||
sensor_gps_s gps_pos;
|
||||
_vehicle_gps_position_sub.copy(&gps_pos);
|
||||
|
|
@ -184,7 +184,7 @@ void HomePosition::setInAirHomePosition()
|
|||
const bool local_home_valid = home.valid_lpos;
|
||||
|
||||
if (local_home_valid && !global_home_valid) {
|
||||
if (_vehicle_status_flags.local_position_valid && _vehicle_status_flags.global_position_valid) {
|
||||
if (!_vehicle_status_flags.local_position_invalid && !_vehicle_status_flags.global_position_invalid) {
|
||||
// Back-compute lon, lat and alt of home position given the local home position
|
||||
// and current positions in local and global (GNSS fused) frames
|
||||
const vehicle_local_position_s &lpos = _local_position_sub.get();
|
||||
|
|
@ -203,7 +203,7 @@ void HomePosition::setInAirHomePosition()
|
|||
home.timestamp = hrt_absolute_time();
|
||||
_home_position_pub.update();
|
||||
|
||||
} else if (_vehicle_status_flags.local_position_valid && _vehicle_status_flags.gps_position_valid) {
|
||||
} else if (!_vehicle_status_flags.local_position_invalid && !_vehicle_status_flags.gps_position_invalid) {
|
||||
// Back-compute lon, lat and alt of home position given the local home position
|
||||
// and current positions in local and global (GNSS raw) frames
|
||||
const vehicle_local_position_s &lpos = _local_position_sub.get();
|
||||
|
|
@ -231,7 +231,7 @@ void HomePosition::setInAirHomePosition()
|
|||
} else if (!local_home_valid && global_home_valid) {
|
||||
const vehicle_local_position_s &lpos = _local_position_sub.get();
|
||||
|
||||
if (_vehicle_status_flags.local_position_valid && lpos.xy_global && lpos.z_global) {
|
||||
if (!_vehicle_status_flags.local_position_invalid && lpos.xy_global && lpos.z_global) {
|
||||
// Back-compute x, y and z of home position given the global home position
|
||||
// and the global reference of the local frame
|
||||
MapProjection ref_pos{lpos.ref_lat, lpos.ref_lon};
|
||||
|
|
@ -326,9 +326,9 @@ void HomePosition::update(bool set_automatically, bool check_if_changed)
|
|||
}
|
||||
|
||||
if (check_if_changed && set_automatically) {
|
||||
const bool can_set_home_lpos_first_time = !home.valid_lpos && _vehicle_status_flags.local_position_valid;
|
||||
const bool can_set_home_lpos_first_time = !home.valid_lpos && !_vehicle_status_flags.local_position_invalid;
|
||||
const bool can_set_home_gpos_first_time = ((!home.valid_hpos || !home.valid_alt)
|
||||
&& (_vehicle_status_flags.global_position_valid || _vehicle_status_flags.gps_position_valid));
|
||||
&& (!_vehicle_status_flags.global_position_invalid || !_vehicle_status_flags.gps_position_invalid));
|
||||
const bool can_set_home_alt_first_time = (!home.valid_alt && lpos.z_global);
|
||||
|
||||
if (can_set_home_lpos_first_time
|
||||
|
|
|
|||
|
|
@ -615,14 +615,14 @@ bool FailsafeBase::modeCanRun(const vehicle_status_flags_s &status_flags, uint8_
|
|||
{
|
||||
uint32_t mode_mask = 1u << mode;
|
||||
return
|
||||
(status_flags.angular_velocity_valid || ((status_flags.mode_req_angular_velocity & mode_mask) == 0)) &&
|
||||
(status_flags.attitude_valid || ((status_flags.mode_req_attitude & mode_mask) == 0)) &&
|
||||
(status_flags.local_position_valid || ((status_flags.mode_req_local_position & mode_mask) == 0)) &&
|
||||
(status_flags.local_position_valid_relaxed || ((status_flags.mode_req_local_position_relaxed & mode_mask) == 0)) &&
|
||||
(status_flags.global_position_valid || ((status_flags.mode_req_global_position & mode_mask) == 0)) &&
|
||||
(status_flags.local_altitude_valid || ((status_flags.mode_req_local_alt & mode_mask) == 0)) &&
|
||||
(status_flags.auto_mission_available || ((status_flags.mode_req_mission & mode_mask) == 0)) &&
|
||||
(!status_flags.angular_velocity_invalid || ((status_flags.mode_req_angular_velocity & mode_mask) == 0)) &&
|
||||
(!status_flags.attitude_invalid || ((status_flags.mode_req_attitude & mode_mask) == 0)) &&
|
||||
(!status_flags.local_position_invalid || ((status_flags.mode_req_local_position & mode_mask) == 0)) &&
|
||||
(!status_flags.local_position_invalid_relaxed || ((status_flags.mode_req_local_position_relaxed & mode_mask) == 0)) &&
|
||||
(!status_flags.global_position_invalid || ((status_flags.mode_req_global_position & mode_mask) == 0)) &&
|
||||
(!status_flags.local_altitude_invalid || ((status_flags.mode_req_local_alt & mode_mask) == 0)) &&
|
||||
(!status_flags.auto_mission_missing || ((status_flags.mode_req_mission & mode_mask) == 0)) &&
|
||||
(!status_flags.offboard_control_signal_lost || ((status_flags.mode_req_offboard_signal & mode_mask) == 0)) &&
|
||||
(status_flags.home_position_valid || ((status_flags.mode_req_home_position & mode_mask) == 0)) &&
|
||||
(!status_flags.home_position_invalid || ((status_flags.mode_req_home_position & mode_mask) == 0)) &&
|
||||
((status_flags.mode_req_other & mode_mask) == 0);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -58,18 +58,22 @@ void RC_Loss_Alarm::process()
|
|||
return;
|
||||
}
|
||||
|
||||
vehicle_status_flags_s status_flags{};
|
||||
|
||||
_vehicle_status_flags_sub.copy(&status_flags);
|
||||
|
||||
if (!_was_armed &&
|
||||
status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
|
||||
|
||||
_was_armed = true; // Once true, impossible to go back to false
|
||||
}
|
||||
|
||||
if (!_had_rc && !status.rc_signal_lost) {
|
||||
if (!_had_rc && !status_flags.rc_signal_lost) {
|
||||
|
||||
_had_rc = true;
|
||||
}
|
||||
|
||||
if (_was_armed && _had_rc && status.rc_signal_lost &&
|
||||
if (_was_armed && _had_rc && status_flags.rc_signal_lost &&
|
||||
status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) {
|
||||
play_tune();
|
||||
_alarm_playing = true;
|
||||
|
|
|
|||
|
|
@ -42,6 +42,7 @@
|
|||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/vehicle_status_flags.h>
|
||||
#include <uORB/topics/tune_control.h>
|
||||
|
||||
namespace events
|
||||
|
|
@ -67,6 +68,7 @@ private:
|
|||
|
||||
uORB::Publication<tune_control_s> _tune_control_pub{ORB_ID(tune_control)};
|
||||
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
|
||||
uORB::Subscription _vehicle_status_flags_sub{ORB_ID(vehicle_status_flags)};
|
||||
|
||||
bool _was_armed = false;
|
||||
bool _had_rc = false; // Don't trigger alarm for systems without RC
|
||||
|
|
|
|||
|
|
@ -54,8 +54,8 @@ namespace status
|
|||
|
||||
void StatusDisplay::set_leds()
|
||||
{
|
||||
bool gps_lock_valid = _vehicle_status_flags_sub.get().global_position_valid;
|
||||
bool home_position_valid = _vehicle_status_flags_sub.get().home_position_valid;
|
||||
bool gps_lock_valid = !_vehicle_status_flags_sub.get().global_position_invalid;
|
||||
bool home_position_valid = !_vehicle_status_flags_sub.get().home_position_invalid;
|
||||
int nav_state = _vehicle_status_sub.get().nav_state;
|
||||
|
||||
#if defined(BOARD_FRONT_LED_MASK)
|
||||
|
|
|
|||
|
|
@ -72,10 +72,10 @@ bool Sticks::checkAndUpdateStickInputs()
|
|||
_input_available = valid_sticks;
|
||||
|
||||
} else {
|
||||
vehicle_status_s vehicle_status;
|
||||
vehicle_status_flags_s vehicle_status_flags;
|
||||
|
||||
if (_vehicle_status_sub.update(&vehicle_status)) {
|
||||
if (vehicle_status.rc_signal_lost) {
|
||||
if (_vehicle_status_flags_sub.update(&vehicle_status_flags)) {
|
||||
if (vehicle_status_flags.rc_signal_lost) {
|
||||
_input_available = false;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -45,7 +45,7 @@
|
|||
#include <matrix/matrix/math.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/vehicle_status_flags.h>
|
||||
|
||||
class Sticks : public ModuleParams
|
||||
{
|
||||
|
|
@ -92,7 +92,7 @@ private:
|
|||
matrix::Vector<float, 4> _positions_expo; ///< modified manual sticks using expo function
|
||||
|
||||
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
|
||||
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
|
||||
uORB::Subscription _vehicle_status_flags_sub{ORB_ID(vehicle_status_flags)};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::MPC_HOLD_DZ>) _param_mpc_hold_dz,
|
||||
|
|
|
|||
|
|
@ -86,6 +86,12 @@ void GyroCalibration::Run()
|
|||
Reset();
|
||||
return;
|
||||
}
|
||||
|
||||
if (_system_calibrating != vehicle_status.calibration_enabled) {
|
||||
_system_calibrating = vehicle_status.calibration_enabled;
|
||||
Reset();
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -94,18 +100,6 @@ void GyroCalibration::Run()
|
|||
return;
|
||||
}
|
||||
|
||||
if (_vehicle_status_flags_sub.updated()) {
|
||||
vehicle_status_flags_s vehicle_status_flags;
|
||||
|
||||
if (_vehicle_status_flags_sub.copy(&vehicle_status_flags)) {
|
||||
if (_system_calibrating != vehicle_status_flags.calibration_enabled) {
|
||||
_system_calibrating = vehicle_status_flags.calibration_enabled;
|
||||
Reset();
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (_system_calibrating) {
|
||||
// do nothing if system is calibrating
|
||||
Reset();
|
||||
|
|
|
|||
|
|
@ -49,7 +49,6 @@
|
|||
#include <uORB/topics/sensor_accel.h>
|
||||
#include <uORB/topics/sensor_gyro.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/vehicle_status_flags.h>
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
|
|
@ -87,7 +86,6 @@ private:
|
|||
|
||||
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
|
||||
uORB::Subscription _vehicle_status_sub{ORB_ID::vehicle_status};
|
||||
uORB::Subscription _vehicle_status_flags_sub{ORB_ID::vehicle_status_flags};
|
||||
|
||||
uORB::SubscriptionMultiArray<sensor_accel_s, MAX_SENSORS> _sensor_accel_subs{ORB_ID::sensor_accel};
|
||||
uORB::SubscriptionMultiArray<sensor_gyro_s, MAX_SENSORS> _sensor_gyro_subs{ORB_ID::sensor_gyro};
|
||||
|
|
|
|||
|
|
@ -101,6 +101,16 @@ void MagBiasEstimator::Run()
|
|||
ScheduleOnInterval(20_ms);
|
||||
}
|
||||
}
|
||||
|
||||
bool system_calibrating = vehicle_status.calibration_enabled;
|
||||
|
||||
if (system_calibrating != _system_calibrating) {
|
||||
_system_calibrating = system_calibrating;
|
||||
|
||||
for (auto &reset : _reset_field_estimator) {
|
||||
reset = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -130,22 +140,6 @@ void MagBiasEstimator::Run()
|
|||
}
|
||||
}
|
||||
|
||||
if (_vehicle_status_flags_sub.updated()) {
|
||||
vehicle_status_flags_s vehicle_status_flags;
|
||||
|
||||
if (_vehicle_status_flags_sub.copy(&vehicle_status_flags)) {
|
||||
bool system_calibrating = vehicle_status_flags.calibration_enabled;
|
||||
|
||||
if (system_calibrating != _system_calibrating) {
|
||||
_system_calibrating = system_calibrating;
|
||||
|
||||
for (auto &reset : _reset_field_estimator) {
|
||||
reset = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// do nothing during regular sensor calibration
|
||||
if (_system_calibrating) {
|
||||
return;
|
||||
|
|
|
|||
|
|
@ -51,7 +51,6 @@
|
|||
#include <uORB/topics/sensor_mag.h>
|
||||
#include <uORB/topics/vehicle_angular_velocity.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/vehicle_status_flags.h>
|
||||
|
||||
namespace mag_bias_estimator
|
||||
{
|
||||
|
|
@ -91,7 +90,6 @@ private:
|
|||
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
|
||||
uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
|
||||
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
|
||||
uORB::Subscription _vehicle_status_flags_sub{ORB_ID(vehicle_status_flags)};
|
||||
|
||||
uORB::Publication<magnetometer_bias_estimate_s> _magnetometer_bias_estimate_pub{ORB_ID(magnetometer_bias_estimate)};
|
||||
|
||||
|
|
|
|||
|
|
@ -37,7 +37,6 @@
|
|||
#include <uORB/topics/actuator_armed.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/vehicle_status_flags.h>
|
||||
|
||||
class MavlinkStreamHeartbeat : public MavlinkStream
|
||||
{
|
||||
|
|
@ -63,7 +62,6 @@ private:
|
|||
uORB::Subscription _acturator_armed_sub{ORB_ID(actuator_armed)};
|
||||
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
|
||||
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
|
||||
uORB::Subscription _vehicle_status_flags_sub{ORB_ID(vehicle_status_flags)};
|
||||
|
||||
bool send() override
|
||||
{
|
||||
|
|
@ -72,9 +70,6 @@ private:
|
|||
vehicle_status_s vehicle_status{};
|
||||
_vehicle_status_sub.copy(&vehicle_status);
|
||||
|
||||
vehicle_status_flags_s vehicle_status_flags{};
|
||||
_vehicle_status_flags_sub.copy(&vehicle_status_flags);
|
||||
|
||||
vehicle_control_mode_s vehicle_control_mode{};
|
||||
_vehicle_control_mode_sub.copy(&vehicle_control_mode);
|
||||
|
||||
|
|
@ -131,7 +126,7 @@ private:
|
|||
|| vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_TERMINATION) {
|
||||
system_status = MAV_STATE_FLIGHT_TERMINATION;
|
||||
|
||||
} else if (vehicle_status_flags.calibration_enabled) {
|
||||
} else if (vehicle_status.calibration_enabled) {
|
||||
system_status = MAV_STATE_CALIBRATING;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -429,10 +429,6 @@ private:
|
|||
}
|
||||
}
|
||||
|
||||
if (status.rc_signal_lost) {
|
||||
msg->failure_flags |= HL_FAILURE_FLAG_RC_RECEIVER;
|
||||
}
|
||||
|
||||
if (status.failure_detector_status & vehicle_status_s::FAILURE_MOTOR) {
|
||||
msg->failure_flags |= HL_FAILURE_FLAG_ENGINE;
|
||||
}
|
||||
|
|
@ -452,7 +448,7 @@ private:
|
|||
vehicle_status_flags_s status_flags;
|
||||
|
||||
if (_status_flags_sub.update(&status_flags)) {
|
||||
if (!status_flags.global_position_valid) { //TODO check if there is a better way to get only GPS failure
|
||||
if (status_flags.gps_position_invalid) {
|
||||
msg->failure_flags |= HL_FAILURE_FLAG_GPS;
|
||||
}
|
||||
|
||||
|
|
@ -464,6 +460,11 @@ private:
|
|||
msg->failure_flags |= HL_FAILURE_FLAG_MISSION;
|
||||
}
|
||||
|
||||
if (status_flags.rc_signal_lost) {
|
||||
msg->failure_flags |= HL_FAILURE_FLAG_RC_RECEIVER;
|
||||
}
|
||||
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue