Browse Source

Plane: inform dataflash of vehicle arm state

mission-4.1.18
Peter Barker 9 years ago committed by Tom Pittenger
parent
commit
17f5ef4610
  1. 10
      ArduPlane/ArduPlane.cpp
  2. 1
      ArduPlane/Plane.h
  3. 3
      ArduPlane/system.cpp

10
ArduPlane/ArduPlane.cpp

@ -151,11 +151,17 @@ void Plane::loop() @@ -151,11 +151,17 @@ void Plane::loop()
scheduler.run(loop_us);
}
void Plane::update_soft_armed()
{
hal.util->set_soft_armed(arming.is_armed() &&
hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED);
DataFlash.set_vehicle_armed(hal.util->get_soft_armed());
}
// update AHRS system
void Plane::ahrs_update()
{
hal.util->set_soft_armed(arming.is_armed() &&
hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED);
update_soft_armed();
#if HIL_SUPPORT
if (g.hil_mode == 1) {

1
ArduPlane/Plane.h

@ -1095,6 +1095,7 @@ private: @@ -1095,6 +1095,7 @@ private:
void parachute_release();
bool parachute_manual_release();
void accel_cal_update(void);
void update_soft_armed();
// support for AP_Avoidance custom flight mode, AVOID_ADSB
bool avoid_adsb_init(bool ignore_checks);

3
ArduPlane/system.cpp

@ -815,8 +815,7 @@ int8_t Plane::throttle_percentage(void) @@ -815,8 +815,7 @@ int8_t Plane::throttle_percentage(void)
void Plane::change_arm_state(void)
{
Log_Arm_Disarm();
hal.util->set_soft_armed(arming.is_armed() &&
hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED);
update_soft_armed();
quadplane.set_armed(hal.util->get_soft_armed());
}

Loading…
Cancel
Save