diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 98db6fd08732c4e06243f5824f2af6aab02c1386..fbbb2890aa5e3e667dcff22fce29150367c7b68e 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -1186,6 +1186,13 @@ void Vehicle::_handleWind(mavlink_message_t& message) } #endif +bool Vehicle::_apmArmingNotRequired(void) +{ + QString armingRequireParam("ARMING_REQUIRE"); + return _parameterManager->parameterExists(FactSystem::defaultComponentId, armingRequireParam) && + _parameterManager->getParameter(FactSystem::defaultComponentId, armingRequireParam)->rawValue().toInt() == 0; +} + void Vehicle::_handleSysStatus(mavlink_message_t& message) { mavlink_sys_status_t sysStatus; @@ -1218,6 +1225,13 @@ void Vehicle::_handleSysStatus(mavlink_message_t& message) _onboardControlSensorsEnabled = sysStatus.onboard_control_sensors_enabled; _onboardControlSensorsHealth = sysStatus.onboard_control_sensors_health; + // ArduPilot firmare has a strange case when ARMING_REQUIRE=0. This means the vehicle is always armed but the motors are not + // really powered up until the safety button is pressed. Because of this we can't depend on the heartbeat to tell us the true + // armed (and dangerous) state. We must instead rely on SYS_STATUS telling us that the motors are enabled. + if (apmFirmware() && _apmArmingNotRequired()) { + _updateArmed(_onboardControlSensorsEnabled & MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS); + } + uint32_t newSensorsUnhealthy = _onboardControlSensorsEnabled & ~_onboardControlSensorsHealth; if (newSensorsUnhealthy != _onboardControlSensorsUnhealthy) { _onboardControlSensorsUnhealthy = newSensorsUnhealthy; @@ -1274,20 +1288,10 @@ void Vehicle::_handleHomePosition(mavlink_message_t& message) _setHomePosition(newHomePosition); } -void Vehicle::_handleHeartbeat(mavlink_message_t& message) +void Vehicle::_updateArmed(bool armed) { - if (message.compid != _defaultComponentId) { - return; - } - - mavlink_heartbeat_t heartbeat; - - mavlink_msg_heartbeat_decode(&message, &heartbeat); - - bool newArmed = heartbeat.base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY; - - if (_armed != newArmed) { - _armed = newArmed; + if (_armed != armed) { + _armed = armed; emit armedChanged(_armed); // We are transitioning to the armed state, begin tracking trajectory points for the map @@ -1303,6 +1307,32 @@ void Vehicle::_handleHeartbeat(mavlink_message_t& message) } } } +} + +void Vehicle::_handleHeartbeat(mavlink_message_t& message) +{ + if (message.compid != _defaultComponentId) { + return; + } + + mavlink_heartbeat_t heartbeat; + + mavlink_msg_heartbeat_decode(&message, &heartbeat); + + bool newArmed = heartbeat.base_mode & MAV_MODE_FLAG_DECODE_POSITION_SAFETY; + + // ArduPilot firmare has a strange case when ARMING_REQUIRE=0. This means the vehicle is always armed but the motors are not + // really powered up until the safety button is pressed. Because of this we can't depend on the heartbeat to tell us the true + // armed (and dangerous) state. We must instead rely on SYS_STATUS telling us that the motors are enabled. + if (apmFirmware()) { + if (!_apmArmingNotRequired() || !(_onboardControlSensorsPresent & MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS)) { + // If ARMING_REQUIRE!=0 or we haven't seen motor output status yet we use the hearbeat info for armed + _updateArmed(newArmed); + } + } else { + // Non-ArduPilot always updates from armed state in heartbeat + _updateArmed(newArmed); + } if (heartbeat.base_mode != _base_mode || heartbeat.custom_mode != _custom_mode) { QString previousFlightMode; diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 65d4b5ec21928bbce91e4dade8acebcd201720e2..827444ac6e83d91111effc8c2d81d8298a58391b 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -987,6 +987,8 @@ private: void _startPlanRequest(void); void _setupAutoDisarmSignalling(void); void _setCapabilities(uint64_t capabilityBits); + void _updateArmed(bool armed); + bool _apmArmingNotRequired(void); int _id; ///< Mavlink system id int _defaultComponentId;