Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Motors: Tradheli - swashplate servo trim fix for heli single and dual frames

This commit is contained in:
bnsgeyer 2018-04-20 23:17:50 -04:00 committed by Andrew Tridgell
parent 7830dab0a8
commit c8488e3333
4 changed files with 37 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AP_Motors/AP_MotorsHeli.cpp
View File

@ -425,3 +425,27 @@ void AP_MotorsHeli::reset_flight_controls()
init_outputs();
calculate_scalars();
}
// convert input in -1 to +1 range to pwm output for swashplate servo. Special handling of trim is required
// to keep travel between the swashplate servos consistent.
int16_t AP_MotorsHeli::calc_pwm_output_1to1_swash_servo(float input, const SRV_Channel *servo)
{
int16_t ret;
input = constrain_float(input, -1.0f, 1.0f);
if (servo->get_reversed()) {
input = -input;
}
// With values of trim other than 1500 between swashplate servos
if (input >= 0.0f) {
ret = (int16_t (input * 500.0f) + servo->get_trim());
} else {
ret = (int16_t (input * 500.0f) + servo->get_trim());
}
return constrain_int16(ret, servo->get_output_min(), servo->get_output_max());
}

4
libraries/AP_Motors/AP_MotorsHeli.h
View File

@ -184,6 +184,10 @@ protected:
// to be overloaded by child classes, different vehicle types would have different movement patterns
virtual void servo_test() = 0;
// convert input in -1 to +1 range to pwm output for swashplate servos. . Special handling of trim is required
// to keep travel between the swashplate servos consistent.
int16_t calc_pwm_output_1to1_swash_servo(float input, const SRV_Channel *servo);
// flags bitmask
struct heliflags_type {
uint8_t landing_collective : 1; // true if collective is setup for landing which has much higher minimum

12
libraries/AP_Motors/AP_MotorsHeli_Dual.cpp
View File

@ -529,12 +529,12 @@ void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float c
servo6_out = 2*servo6_out - 1;
// actually move the servos
rc_write(AP_MOTORS_MOT_1, calc_pwm_output_1to1(servo1_out, _swash_servo_1));
rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(servo2_out, _swash_servo_2));
rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(servo3_out, _swash_servo_3));
rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(servo4_out, _swash_servo_4));
rc_write(AP_MOTORS_MOT_5, calc_pwm_output_1to1(servo5_out, _swash_servo_5));
rc_write(AP_MOTORS_MOT_6, calc_pwm_output_1to1(servo6_out, _swash_servo_6));
rc_write(AP_MOTORS_MOT_1, calc_pwm_output_1to1_swash_servo(servo1_out, _swash_servo_1));
rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1_swash_servo(servo2_out, _swash_servo_2));
rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1_swash_servo(servo3_out, _swash_servo_3));
rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1_swash_servo(servo4_out, _swash_servo_4));
rc_write(AP_MOTORS_MOT_5, calc_pwm_output_1to1_swash_servo(servo5_out, _swash_servo_5));
rc_write(AP_MOTORS_MOT_6, calc_pwm_output_1to1_swash_servo(servo6_out, _swash_servo_6));
}

6
libraries/AP_Motors/AP_MotorsHeli_Single.cpp
View File

@ -457,9 +457,9 @@ void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float
servo3_out = 2*servo3_out - 1;
// actually move the servos
rc_write(AP_MOTORS_MOT_1, calc_pwm_output_1to1(servo1_out, _swash_servo_1));
rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(servo2_out, _swash_servo_2));
rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(servo3_out, _swash_servo_3));
rc_write(AP_MOTORS_MOT_1, calc_pwm_output_1to1_swash_servo(servo1_out, _swash_servo_1));
rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1_swash_servo(servo2_out, _swash_servo_2));
rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1_swash_servo(servo3_out, _swash_servo_3));
// update the yaw rate using the tail rotor/servo
move_yaw(yaw_out + yaw_offset);