SRV_Channels: refactor zero_rc_outputs() out of GCS_Mavlink

This commit is contained in:
yaapu 2020-09-10 13:17:56 +02:00 committed by Peter Barker
parent d37624ccd6
commit bc7c5a0c8e
2 changed files with 16 additions and 0 deletions

View File

@ -504,6 +504,8 @@ public:
return _singleton;
}
static void zero_rc_outputs();
private:
static bool disabled_passthrough;

View File

@ -328,3 +328,17 @@ void SRV_Channels::push()
}
#endif // HAL_NUM_CAN_IFACES
}
void SRV_Channels::zero_rc_outputs()
{
/* Send an invalid signal to the motors to prevent spinning due to
* neutral (1500) pwm pulse being cut short. For that matter,
* send an invalid signal to all channels to prevent
* undesired/unexpected behavior
*/
cork();
for (uint8_t i=0; i<NUM_RC_CHANNELS; i++) {
hal.rcout->write(i, 0);
}
push();
}