mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-03 14:38:44 -04:00
AP_MotorsHeli: Simplify servo init/reset
This commit is contained in:
parent
c00fd86b45
commit
bac559d5af
@ -243,6 +243,7 @@ void AP_MotorsHeli::reset_swash()
|
|||||||
// reset_swash_servo
|
// reset_swash_servo
|
||||||
void AP_MotorsHeli::reset_swash_servo(RC_Channel& servo)
|
void AP_MotorsHeli::reset_swash_servo(RC_Channel& servo)
|
||||||
{
|
{
|
||||||
|
servo.set_range(0, 1000);
|
||||||
servo.radio_min = 1000;
|
servo.radio_min = 1000;
|
||||||
servo.radio_max = 2000;
|
servo.radio_max = 2000;
|
||||||
}
|
}
|
||||||
@ -279,15 +280,6 @@ void AP_MotorsHeli::init_swash()
|
|||||||
_heliflags.swash_initialised = true;
|
_heliflags.swash_initialised = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
// init_swash_servo
|
|
||||||
void AP_MotorsHeli::init_swash_servo(RC_Channel& servo)
|
|
||||||
{
|
|
||||||
servo.set_range(0, 1000);
|
|
||||||
|
|
||||||
servo.radio_min = 1000;
|
|
||||||
servo.radio_max = 2000;
|
|
||||||
}
|
|
||||||
|
|
||||||
// update the throttle input filter
|
// update the throttle input filter
|
||||||
void AP_MotorsHeli::update_throttle_filter()
|
void AP_MotorsHeli::update_throttle_filter()
|
||||||
{
|
{
|
||||||
|
@ -199,9 +199,6 @@ protected:
|
|||||||
// init_servos - initialize the servos
|
// init_servos - initialize the servos
|
||||||
virtual void init_servos() = 0;
|
virtual void init_servos() = 0;
|
||||||
|
|
||||||
// init_swash_servo - initialize a swash servo
|
|
||||||
static void init_swash_servo(RC_Channel& servo);
|
|
||||||
|
|
||||||
// calculate_roll_pitch_collective_factors - calculate factors based on swash type and servo position
|
// calculate_roll_pitch_collective_factors - calculate factors based on swash type and servo position
|
||||||
virtual void calculate_roll_pitch_collective_factors() = 0;
|
virtual void calculate_roll_pitch_collective_factors() = 0;
|
||||||
|
|
||||||
|
@ -378,9 +378,8 @@ void AP_MotorsHeli_Single::reset_servos()
|
|||||||
// init_servos
|
// init_servos
|
||||||
void AP_MotorsHeli_Single::init_servos()
|
void AP_MotorsHeli_Single::init_servos()
|
||||||
{
|
{
|
||||||
init_swash_servo (_swash_servo_1);
|
// reset swash servo range and endpoints
|
||||||
init_swash_servo (_swash_servo_2);
|
reset_servos();
|
||||||
init_swash_servo (_swash_servo_3);
|
|
||||||
|
|
||||||
_yaw_servo.set_angle(4500);
|
_yaw_servo.set_angle(4500);
|
||||||
|
|
||||||
|
Loading…
Reference in New Issue
Block a user