Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Motors: Break out servo init and reset in AP_MotorsHeli.

Browse Source
This commit is contained in:
Fredrik Hedberg 2015-07-22 10:14:05 +02:00 committed by Randy Mackay
parent ae9a16dc27
commit feb32f40ed
2 changed files with 48 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
libraries/AP_Motors/AP_MotorsHeli.cpp
View File

@ -472,12 +472,7 @@ void AP_MotorsHeli::output_disarmed()
void AP_MotorsHeli::reset_swash()
{
// free up servo ranges
_servo_1.radio_min = 1000;
_servo_1.radio_max = 2000;
_servo_2.radio_min = 1000;
_servo_2.radio_max = 2000;
_servo_3.radio_min = 1000;
_servo_3.radio_max = 2000;
reset_servos();
// calculate factors based on swash type and servo position
calculate_roll_pitch_collective_factors();
@ -492,15 +487,27 @@ void AP_MotorsHeli::reset_swash()
_heliflags.swash_initialised = false;
}
// reset_servos
void AP_MotorsHeli::reset_servos()
{
reset_swash_servo (_servo_1);
reset_swash_servo (_servo_2);
reset_swash_servo (_servo_3);
}
// reset_swash_servo
void AP_MotorsHeli::reset_swash_servo(RC_Channel& servo)
{
servo.radio_min = 1000;
servo.radio_max = 2000;
}
// init_swash - initialise the swash plate
void AP_MotorsHeli::init_swash()
{
// swash servo initialisation
_servo_1.set_range(0,1000);
_servo_2.set_range(0,1000);
_servo_3.set_range(0,1000);
_servo_4.set_angle(4500);
init_servos();
// range check collective min, max and mid
if( _collective_min >= _collective_max ) {
@ -520,18 +527,29 @@ void AP_MotorsHeli::init_swash()
// calculate factors based on swash type and servo position
calculate_roll_pitch_collective_factors();
// servo min/max values
_servo_1.radio_min = 1000;
_servo_1.radio_max = 2000;
_servo_2.radio_min = 1000;
_servo_2.radio_max = 2000;
_servo_3.radio_min = 1000;
_servo_3.radio_max = 2000;
// mark swash as initialised
_heliflags.swash_initialised = true;
}
// init_servos
void AP_MotorsHeli::init_servos()
{
init_swash_servo (_servo_1);
init_swash_servo (_servo_2);
init_swash_servo (_servo_3);
_servo_4.set_angle(4500);
}
// 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;
}
// calculate_roll_pitch_collective_factors - calculate factors based on swash type and servo position
void AP_MotorsHeli::calculate_roll_pitch_collective_factors()
{

12
libraries/AP_Motors/AP_MotorsHeli.h
View File

@ -232,9 +232,21 @@ private:
// reset_swash - free up swash for maximum movements. Used for set-up
void reset_swash();
// reset_servos - free up the swash servos for maximum movement
void reset_servos();
// reset_swash_servo - free up swash servo for maximum movement
static void reset_swash_servo(RC_Channel& servo);
// init_swash - initialise the swash plate
void init_swash();
// init_servos - initialize the servos
void init_servos();
// 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
void calculate_roll_pitch_collective_factors();