mirror of https://github.com/ArduPilot/ardupilot
AP_Motors: Heli: add helper to convert from AP_Motors::SpoolState to AP_MotorsHeli_RSC::RotorControlState
This commit is contained in:
parent
287e9726d7
commit
1bcf69e0c7
|
@ -590,3 +590,26 @@ void AP_MotorsHeli::set_desired_rotor_speed(float desired_speed)
|
||||||
{
|
{
|
||||||
_main_rotor.set_desired_speed(desired_speed);
|
_main_rotor.set_desired_speed(desired_speed);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Converts AP_Motors::SpoolState from _spool_state variable to AP_MotorsHeli_RSC::RotorControlState
|
||||||
|
AP_MotorsHeli_RSC::RotorControlState AP_MotorsHeli::get_rotor_control_state() const
|
||||||
|
{
|
||||||
|
switch (_spool_state) {
|
||||||
|
case SpoolState::SHUT_DOWN:
|
||||||
|
// sends minimum values out to the motors
|
||||||
|
return AP_MotorsHeli_RSC::RotorControlState::STOP;
|
||||||
|
case SpoolState::GROUND_IDLE:
|
||||||
|
// sends idle output to motors when armed. rotor could be static or turning (autorotation)
|
||||||
|
return AP_MotorsHeli_RSC::RotorControlState::IDLE;
|
||||||
|
case SpoolState::SPOOLING_UP:
|
||||||
|
case SpoolState::THROTTLE_UNLIMITED:
|
||||||
|
// set motor output based on thrust requests
|
||||||
|
return AP_MotorsHeli_RSC::RotorControlState::ACTIVE;
|
||||||
|
case SpoolState::SPOOLING_DOWN:
|
||||||
|
// sends idle output to motors and wait for rotor to stop
|
||||||
|
return AP_MotorsHeli_RSC::RotorControlState::IDLE;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Should be unreachable, but needed to keep the compiler happy
|
||||||
|
return AP_MotorsHeli_RSC::RotorControlState::STOP;
|
||||||
|
}
|
||||||
|
|
|
@ -191,6 +191,9 @@ protected:
|
||||||
// update_motor_controls - sends commands to motor controllers
|
// update_motor_controls - sends commands to motor controllers
|
||||||
virtual void update_motor_control(AP_MotorsHeli_RSC::RotorControlState state) = 0;
|
virtual void update_motor_control(AP_MotorsHeli_RSC::RotorControlState state) = 0;
|
||||||
|
|
||||||
|
// Converts AP_Motors::SpoolState from _spool_state variable to AP_MotorsHeli_RSC::RotorControlState
|
||||||
|
AP_MotorsHeli_RSC::RotorControlState get_rotor_control_state() const;
|
||||||
|
|
||||||
// run spool logic
|
// run spool logic
|
||||||
void output_logic();
|
void output_logic();
|
||||||
|
|
||||||
|
|
|
@ -525,25 +525,8 @@ void AP_MotorsHeli_Dual::output_to_motors()
|
||||||
_swashplate1.output();
|
_swashplate1.output();
|
||||||
_swashplate2.output();
|
_swashplate2.output();
|
||||||
|
|
||||||
switch (_spool_state) {
|
update_motor_control(get_rotor_control_state());
|
||||||
case SpoolState::SHUT_DOWN:
|
|
||||||
// sends minimum values out to the motors
|
|
||||||
update_motor_control(AP_MotorsHeli_RSC::RotorControlState::STOP);
|
|
||||||
break;
|
|
||||||
case SpoolState::GROUND_IDLE:
|
|
||||||
// sends idle output to motors when armed. rotor could be static or turning (autorotation)
|
|
||||||
update_motor_control(AP_MotorsHeli_RSC::RotorControlState::IDLE);
|
|
||||||
break;
|
|
||||||
case SpoolState::SPOOLING_UP:
|
|
||||||
case SpoolState::THROTTLE_UNLIMITED:
|
|
||||||
// set motor output based on thrust requests
|
|
||||||
update_motor_control(AP_MotorsHeli_RSC::RotorControlState::ACTIVE);
|
|
||||||
break;
|
|
||||||
case SpoolState::SPOOLING_DOWN:
|
|
||||||
// sends idle output to motors and wait for rotor to stop
|
|
||||||
update_motor_control(AP_MotorsHeli_RSC::RotorControlState::IDLE);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// servo_test - move servos through full range of movement
|
// servo_test - move servos through full range of movement
|
||||||
|
|
|
@ -273,25 +273,8 @@ void AP_MotorsHeli_Quad::output_to_motors()
|
||||||
rc_write_angle(AP_MOTORS_MOT_1+i, _out[i] * QUAD_SERVO_MAX_ANGLE);
|
rc_write_angle(AP_MOTORS_MOT_1+i, _out[i] * QUAD_SERVO_MAX_ANGLE);
|
||||||
}
|
}
|
||||||
|
|
||||||
switch (_spool_state) {
|
update_motor_control(get_rotor_control_state());
|
||||||
case SpoolState::SHUT_DOWN:
|
|
||||||
// sends minimum values out to the motors
|
|
||||||
update_motor_control(AP_MotorsHeli_RSC::RotorControlState::STOP);
|
|
||||||
break;
|
|
||||||
case SpoolState::GROUND_IDLE:
|
|
||||||
// sends idle output to motors when armed. rotor could be static or turning (autorotation)
|
|
||||||
update_motor_control(AP_MotorsHeli_RSC::RotorControlState::IDLE);
|
|
||||||
break;
|
|
||||||
case SpoolState::SPOOLING_UP:
|
|
||||||
case SpoolState::THROTTLE_UNLIMITED:
|
|
||||||
// set motor output based on thrust requests
|
|
||||||
update_motor_control(AP_MotorsHeli_RSC::RotorControlState::ACTIVE);
|
|
||||||
break;
|
|
||||||
case SpoolState::SPOOLING_DOWN:
|
|
||||||
// sends idle output to motors and wait for rotor to stop
|
|
||||||
update_motor_control(AP_MotorsHeli_RSC::RotorControlState::IDLE);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// servo_test - move servos through full range of movement
|
// servo_test - move servos through full range of movement
|
||||||
|
|
|
@ -501,25 +501,7 @@ void AP_MotorsHeli_Single::output_to_motors()
|
||||||
_swashplate.output();
|
_swashplate.output();
|
||||||
|
|
||||||
// Output main rotor
|
// Output main rotor
|
||||||
switch (_spool_state) {
|
update_motor_control(get_rotor_control_state());
|
||||||
case SpoolState::SHUT_DOWN:
|
|
||||||
// sends minimum values out to the motors
|
|
||||||
update_motor_control(AP_MotorsHeli_RSC::RotorControlState::STOP);
|
|
||||||
break;
|
|
||||||
case SpoolState::GROUND_IDLE:
|
|
||||||
// sends idle output to motors when armed. rotor could be static or turning (autorotation)
|
|
||||||
update_motor_control(AP_MotorsHeli_RSC::RotorControlState::IDLE);
|
|
||||||
break;
|
|
||||||
case SpoolState::SPOOLING_UP:
|
|
||||||
case SpoolState::THROTTLE_UNLIMITED:
|
|
||||||
// set motor output based on thrust requests
|
|
||||||
update_motor_control(AP_MotorsHeli_RSC::RotorControlState::ACTIVE);
|
|
||||||
break;
|
|
||||||
case SpoolState::SPOOLING_DOWN:
|
|
||||||
// sends idle output to motors and wait for rotor to stop
|
|
||||||
update_motor_control(AP_MotorsHeli_RSC::RotorControlState::IDLE);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Output tail rotor
|
// Output tail rotor
|
||||||
switch (get_tail_type()) {
|
switch (get_tail_type()) {
|
||||||
|
|
Loading…
Reference in New Issue