mirror of https://github.com/ArduPilot/ardupilot
Copter: make sure takeoff check checks the right ESC channels
This commit is contained in:
parent
d93a87e9ee
commit
dfee3c03a4
|
@ -29,6 +29,12 @@ void Copter::takeoff_check()
|
||||||
|
|
||||||
// check ESCs are sending RPM at expected level
|
// check ESCs are sending RPM at expected level
|
||||||
uint32_t motor_mask = motors->get_motor_mask();
|
uint32_t motor_mask = motors->get_motor_mask();
|
||||||
|
#if HAL_WITH_IO_MCU
|
||||||
|
if (AP_BoardConfig::io_enabled()) {
|
||||||
|
// In 4.4 and earlier ESC telemetry is always indexed from 1 for servo channels 9+
|
||||||
|
motor_mask >>= 8;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
const bool telem_active = AP::esc_telem().is_telemetry_active(motor_mask);
|
const bool telem_active = AP::esc_telem().is_telemetry_active(motor_mask);
|
||||||
const bool rpm_adequate = AP::esc_telem().are_motors_running(motor_mask, g2.takeoff_rpm_min);
|
const bool rpm_adequate = AP::esc_telem().are_motors_running(motor_mask, g2.takeoff_rpm_min);
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue