Browse Source

ArduCopter: Mark motors un-healthy if any motors are not producing thrust

c415-sdk
Dr.-Ing. Amilcar do Carmo Lucas 4 years ago committed by Peter Barker
parent
commit
2261f94361
  1. 8
      ArduCopter/GCS_Copter.cpp

8
ArduCopter/GCS_Copter.cpp

@ -150,4 +150,12 @@ void GCS_Copter::update_vehicle_sensor_status_flags(void) @@ -150,4 +150,12 @@ void GCS_Copter::update_vehicle_sensor_status_flags(void)
break;
}
#endif
control_sensors_present |= MAV_SYS_STATUS_SENSOR_PROPULSION;
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_PROPULSION;
// only mark propulsion healthy if all of the motors are producing
// nominal thrust
if (!copter.motors->get_thrust_boost()) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_PROPULSION;
}
}

Loading…
Cancel
Save