// motors_output - send output to motors library which will adjust and send to ESCs and servos
void Copter::motors_output()
{
#if ADVANCED_FAILSAFE == ENABLED
// this is to allow the failsafe module to deliberately crash
// the vehicle. Only used in extreme circumstances to meet the
// OBC rules
if (g2.afs.should_crash_vehicle()) {
g2.afs.terminate_vehicle();
return;
}
#endif
// Update arming delay state
if (ap.in_arming_delay && (!motors.armed() || millis()-arm_time_ms > ARMING_DELAY_SEC*1.0e3f || control_mode == THROW)) {
ap.in_arming_delay = false;
}
// check if we are performing the motor test
if (ap.motor_test) {
motor_test_output();
} else {
bool interlock = motors.armed() && !ap.in_arming_delay && (!ap.using_interlock || ap.motor_interlock_switch) && !ap.motor_emergency_stop;
if (!motors.get_interlock() && interlock) {
motors.set_interlock(true);
Log_Write_Event(DATA_MOTORS_INTERLOCK_ENABLED);
} else if (motors.get_interlock() && !interlock) {
motors.set_interlock(false);
Log_Write_Event(DATA_MOTORS_INTERLOCK_DISABLED);
}
// send output signals to motors
motors.output();
}
}
/*****************************************
Set the flight control servos based on the current calculated values
*****************************************/
void Rover::set_servos(void)
{
// send output signals to motors
if (motor_test) {
motor_test_output();
} else {
g2.motors.output(arming.is_armed() && hal.util->get_soft_armed(), G_Dt);
}
}
// motors_output - send output to motors library which will adjust and send to ESCs and servos
void Copter::motors_output()
{
#if ADVANCED_FAILSAFE == ENABLED
// this is to allow the failsafe module to deliberately crash
// the vehicle. Only used in extreme circumstances to meet the
// OBC rules
if (g2.afs.should_crash_vehicle()) {
g2.afs.terminate_vehicle();
if (!g2.afs.terminating_vehicle_via_landing()) {
return;
}
// landing must continue to run the motors output
}
#endif
// Update arming delay state
if (ap.in_arming_delay && (!motors->armed() || millis()-arm_time_ms > ARMING_DELAY_SEC*1.0e3f || control_mode == THROW)) {
ap.in_arming_delay = false;
}
// output any servo channels
SRV_Channels::calc_pwm();
// cork now, so that all channel outputs happen at once
SRV_Channels::cork();
// update output on any aux channels, for manual passthru
SRV_Channels::output_ch_all();
// check if we are performing the motor test
if (ap.motor_test) {
motor_test_output();
} else {
bool interlock = motors->armed() && !ap.in_arming_delay && (!ap.using_interlock || ap.motor_interlock_switch) && !SRV_Channels::get_emergency_stop();
if (!motors->get_interlock() && interlock) {
motors->set_interlock(true);
Log_Write_Event(DATA_MOTORS_INTERLOCK_ENABLED);
} else if (motors->get_interlock() && !interlock) {
motors->set_interlock(false);
Log_Write_Event(DATA_MOTORS_INTERLOCK_DISABLED);
}
// send output signals to motors
motors->output();
}
// push all channels
SRV_Channels::push();
}
// motors_output - send output to motors library which will adjust and send to ESCs and servos
void Copter::motors_output()
{
// check if we are performing the motor test
if (ap.motor_test) {
motor_test_output();
} else {
if (!ap.using_interlock){
// if not using interlock switch, set according to Emergency Stop status
// where Emergency Stop is forced false during arming if Emergency Stop switch
// is not used. Interlock enabled means motors run, so we must
// invert motor_emergency_stop status for motors to run.
motors.set_interlock(!ap.motor_emergency_stop);
}
motors.output();
}
}
