Browse Source

AP_MotorsHeli: Simplify servo init/reset

master
Robert Lefebvre 10 years ago committed by Randy Mackay
parent
commit
345663f705
  1. 10
      libraries/AP_Motors/AP_MotorsHeli.cpp
  2. 3
      libraries/AP_Motors/AP_MotorsHeli.h
  3. 5
      libraries/AP_Motors/AP_MotorsHeli_Single.cpp

10
libraries/AP_Motors/AP_MotorsHeli.cpp

@ -243,6 +243,7 @@ void AP_MotorsHeli::reset_swash() @@ -243,6 +243,7 @@ void AP_MotorsHeli::reset_swash()
// reset_swash_servo
void AP_MotorsHeli::reset_swash_servo(RC_Channel& servo)
{
servo.set_range(0, 1000);
servo.radio_min = 1000;
servo.radio_max = 2000;
}
@ -279,15 +280,6 @@ void AP_MotorsHeli::init_swash() @@ -279,15 +280,6 @@ void AP_MotorsHeli::init_swash()
_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
void AP_MotorsHeli::update_throttle_filter()
{

3
libraries/AP_Motors/AP_MotorsHeli.h

@ -199,9 +199,6 @@ protected: @@ -199,9 +199,6 @@ protected:
// init_servos - initialize the servos
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
virtual void calculate_roll_pitch_collective_factors() = 0;

5
libraries/AP_Motors/AP_MotorsHeli_Single.cpp

@ -378,9 +378,8 @@ void AP_MotorsHeli_Single::reset_servos() @@ -378,9 +378,8 @@ void AP_MotorsHeli_Single::reset_servos()
// init_servos
void AP_MotorsHeli_Single::init_servos()
{
init_swash_servo (_swash_servo_1);
init_swash_servo (_swash_servo_2);
init_swash_servo (_swash_servo_3);
// reset swash servo range and endpoints
reset_servos();
_yaw_servo.set_angle(4500);

Loading…
Cancel
Save