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

AP_Motors: heli: move swash servo defualt and setup into swash lib

This commit is contained in:
Iampete1 2023-05-05 00:52:54 +01:00 committed by Randy Mackay
parent d32d1dfec8
commit b327f00b25
5 changed files with 10 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_Motors/AP_MotorsHeli.cpp
View File

@ -503,16 +503,6 @@ bool AP_MotorsHeli::parameter_check(bool display_msg) const
return true;
}
// reset_swash_servo
void AP_MotorsHeli::reset_swash_servo(SRV_Channel::Aux_servo_function_t function)
{
// outputs are defined on a -500 to 500 range for swash servos
SRV_Channels::set_range(function, 1000);
// swash servos always use full endpoints as restricting them would lead to scaling errors
SRV_Channels::set_output_min_max(function, 1000, 2000);
}
// update the throttle input filter
void AP_MotorsHeli::update_throttle_filter()
{

3
libraries/AP_Motors/AP_MotorsHeli.h
View File

@ -206,9 +206,6 @@ protected:
// move_actuators - moves swash plate and tail rotor
virtual void move_actuators(float roll_out, float pitch_out, float coll_in, float yaw_out) = 0;
// reset_swash_servo - free up swash servo for maximum movement
void reset_swash_servo(SRV_Channel::Aux_servo_function_t function);
// init_outputs - initialise Servo/PWM ranges and endpoints. This
// method also updates the initialised flag.
virtual void init_outputs() = 0;

23
libraries/AP_Motors/AP_MotorsHeli_Dual.cpp
View File

@ -224,31 +224,8 @@ void AP_MotorsHeli_Dual::set_update_rate( uint16_t speed_hz )
void AP_MotorsHeli_Dual::init_outputs()
{
if (!initialised_ok()) {
// make sure 6 output channels are mapped
for (uint8_t i=0; i<AP_MOTORS_HELI_DUAL_NUM_SWASHPLATE_SERVOS; i++) {
add_motor_num(CH_1+i);
}
if (_swashplate1.get_swash_type() == SWASHPLATE_TYPE_H4_90 || _swashplate1.get_swash_type() == SWASHPLATE_TYPE_H4_45) {
add_motor_num(CH_7);
}
if (_swashplate2.get_swash_type() == SWASHPLATE_TYPE_H4_90 || _swashplate2.get_swash_type() == SWASHPLATE_TYPE_H4_45) {
add_motor_num(CH_8);
}
// set rotor servo range
_main_rotor.init_servo();
}
// reset swash servo range and endpoints
for (uint8_t i=0; i<AP_MOTORS_HELI_DUAL_NUM_SWASHPLATE_SERVOS; i++) {
reset_swash_servo(SRV_Channels::get_motor_function(i));
}
if (_swashplate1.get_swash_type() == SWASHPLATE_TYPE_H4_90 || _swashplate1.get_swash_type() == SWASHPLATE_TYPE_H4_45) {
reset_swash_servo(SRV_Channels::get_motor_function(6));
}
if (_swashplate2.get_swash_type() == SWASHPLATE_TYPE_H4_90 || _swashplate2.get_swash_type() == SWASHPLATE_TYPE_H4_45) {
reset_swash_servo(SRV_Channels::get_motor_function(7));
}
set_initialised_ok(_frame_class == MOTOR_FRAME_HELI_DUAL);

16
libraries/AP_Motors/AP_MotorsHeli_Single.cpp
View File

@ -197,14 +197,6 @@ void AP_MotorsHeli_Single::set_update_rate( uint16_t speed_hz )
void AP_MotorsHeli_Single::init_outputs()
{
if (!initialised_ok()) {
// map primary swash servos
for (uint8_t i=0; i<AP_MOTORS_HELI_SINGLE_NUM_SWASHPLATE_SERVOS; i++) {
add_motor_num(CH_1+i);
}
if (_swashplate.get_swash_type() == SWASHPLATE_TYPE_H4_90 || _swashplate.get_swash_type() == SWASHPLATE_TYPE_H4_45) {
add_motor_num(CH_5);
}
// yaw servo
add_motor_num(CH_4);
@ -224,14 +216,6 @@ void AP_MotorsHeli_Single::init_outputs()
SRV_Channels::set_output_min_max(SRV_Channels::get_motor_function(AP_MOTORS_HELI_SINGLE_EXTGYRO), 1000, 2000);
}
// reset swash servo range and endpoints
for (uint8_t i=0; i<AP_MOTORS_HELI_SINGLE_NUM_SWASHPLATE_SERVOS; i++) {
reset_swash_servo(SRV_Channels::get_motor_function(i));
}
if (_swashplate.get_swash_type() == SWASHPLATE_TYPE_H4_90 || _swashplate.get_swash_type() == SWASHPLATE_TYPE_H4_45) {
reset_swash_servo(SRV_Channels::get_motor_function(4));
}
if (_tail_type != AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH && _tail_type != AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPIT_EXT_GOV) {
// yaw servo is an angle from -4500 to 4500
SRV_Channels::set_angle(SRV_Channel::k_motor4, YAW_SERVO_MAX_ANGLE);

10
libraries/AP_Motors/AP_MotorsHeli_Swash.cpp
View File

@ -199,6 +199,16 @@ void AP_MotorsHeli_Swash::add_servo_raw(uint8_t num, float roll, float pitch, fl
_pitchFactor[num] = pitch * 0.45;
_collectiveFactor[num] = collective;
// Setup output function
SRV_Channel::Aux_servo_function_t function = SRV_Channels::get_motor_function(_motor_num[num]);
SRV_Channels::set_aux_channel_default(function, _motor_num[num]);
// outputs are defined on a -500 to 500 range for swash servos
SRV_Channels::set_range(function, 1000);
// swash servos always use full endpoints as restricting them would lead to scaling errors
SRV_Channels::set_output_min_max(function, 1000, 2000);
}
// calculates servo output