zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_Motors: convert heli code to use SRV_Channels 

this converts the heli code to use the SRV_Channels output
functions. It does not change behaviour, but removes the last vehicle
type that did its own servo output calculations.  This change also
fixed servo initialization conflicts.

Note that this also allows helis to be setup with more than one
channel for a particular output (eg. two separate channels for tail
servo if they are wanted). This isn't likely to be used much, but does
make heli consistent with other vehicle types
mission-4.1.18
bnsgeyer 7 years ago committed by Andrew Tridgell
parent
c3d4b792ad
commit
dec8c5de77
11 changed files with 92 additions and 171 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 32
      libraries/AP_Motors/AP_MotorsHeli.cpp
  2. 11
      libraries/AP_Motors/AP_MotorsHeli.h
  3. 61
      libraries/AP_Motors/AP_MotorsHeli_Dual.cpp
  4. 7
      libraries/AP_Motors/AP_MotorsHeli_Dual.h
  5. 10
      libraries/AP_Motors/AP_MotorsHeli_Quad.cpp
  6. 3
      libraries/AP_Motors/AP_MotorsHeli_Quad.h
  7. 4
      libraries/AP_Motors/AP_MotorsHeli_RSC.cpp
  8. 77
      libraries/AP_Motors/AP_MotorsHeli_Single.cpp
  9. 16
      libraries/AP_Motors/AP_MotorsHeli_Single.h
  10. 36
      libraries/AP_Motors/AP_Motors_Class.cpp
  11. 6
      libraries/AP_Motors/AP_Motors_Class.h

32
libraries/AP_Motors/AP_MotorsHeli.cpp
Unescape View File

@ -395,13 +395,13 @@ bool AP_MotorsHeli::parameter_check(bool display_msg) const @@ -395,13 +395,13 @@ bool AP_MotorsHeli::parameter_check(bool display_msg) const
}
// reset_swash_servo
void AP_MotorsHeli::reset_swash_servo(SRV_Channel *servo)
void AP_MotorsHeli::reset_swash_servo(SRV_Channel::Aux_servo_function_t function)
{
servo->set_range(1000);
// 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
servo->set_output_min(1000);
servo->set_output_max(2000);
SRV_Channels::set_output_min_max(function, 1000, 2000);
}
// update the throttle input filter
@ -426,22 +426,16 @@ void AP_MotorsHeli::reset_flight_controls() @@ -426,22 +426,16 @@ void AP_MotorsHeli::reset_flight_controls()
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. Swashplate servo travel range is fixed to 1000 pwm
// and therefore the input is multiplied by 500 to get PWM output.
int16_t AP_MotorsHeli::calc_pwm_output_1to1_swash_servo(float input, const SRV_Channel *servo)
// convert input in -1 to +1 range to pwm output for swashplate servo.
// The value 0 corresponds to the trim value of the servo. Swashplate
// servo travel range is fixed to 1000 pwm and therefore the input is
// multiplied by 500 to get PWM output.
void AP_MotorsHeli::rc_write_swash(uint8_t chan, float swash_in)
{
int16_t ret;
input = constrain_float(input, -1.0f, 1.0f);
if (servo->get_reversed()) {
input = -input;
}
ret = (int16_t (input * 500.0f) + servo->get_trim());
return constrain_int16(ret, servo->get_output_min(), servo->get_output_max());
uint16_t pwm = (uint16_t)(1500 + 500 * swash_in);
SRV_Channel::Aux_servo_function_t function = SRV_Channels::get_motor_function(chan);
SRV_Channels::set_output_pwm_trimmed(function, pwm);
}

11
libraries/AP_Motors/AP_MotorsHeli.h
Unescape View File

@ -99,7 +99,7 @@ public: @@ -99,7 +99,7 @@ public:
uint8_t get_rsc_mode() const { return _rsc_mode; }
// get_rsc_setpoint - gets contents of _rsc_setpoint parameter (0~1)
float get_rsc_setpoint() const { return _rsc_setpoint / 1000.0f; }
float get_rsc_setpoint() const { return _rsc_setpoint * 0.001f; }
// set_desired_rotor_speed - sets target rotor speed as a number from 0 ~ 1
virtual void set_desired_rotor_speed(float desired_speed) = 0;
@ -166,7 +166,7 @@ protected: @@ -166,7 +166,7 @@ protected:
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 *servo);
void reset_swash_servo(SRV_Channel::Aux_servo_function_t function);
// init_outputs - initialise Servo/PWM ranges and endpoints
virtual bool init_outputs() = 0;
@ -184,10 +184,9 @@ protected: @@ -184,10 +184,9 @@ 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);
// write to a swash servo. output value is pwm
void rc_write_swash(uint8_t chan, float swash_in);
// flags bitmask
struct heliflags_type {
uint8_t landing_collective : 1; // true if collective is setup for landing which has much higher minimum

61
libraries/AP_Motors/AP_MotorsHeli_Dual.cpp
Unescape View File

@ -184,28 +184,20 @@ void AP_MotorsHeli_Dual::set_update_rate( uint16_t speed_hz ) @@ -184,28 +184,20 @@ void AP_MotorsHeli_Dual::set_update_rate( uint16_t speed_hz )
bool AP_MotorsHeli_Dual::init_outputs()
{
if (!_flags.initialised_ok) {
_swash_servo_1 = SRV_Channels::get_channel_for(SRV_Channel::k_motor1, CH_1);
_swash_servo_2 = SRV_Channels::get_channel_for(SRV_Channel::k_motor2, CH_2);
_swash_servo_3 = SRV_Channels::get_channel_for(SRV_Channel::k_motor3, CH_3);
_swash_servo_4 = SRV_Channels::get_channel_for(SRV_Channel::k_motor4, CH_4);
_swash_servo_5 = SRV_Channels::get_channel_for(SRV_Channel::k_motor5, CH_5);
_swash_servo_6 = SRV_Channels::get_channel_for(SRV_Channel::k_motor6, CH_6);
if (!_swash_servo_1 || !_swash_servo_2 || !_swash_servo_3 ||
!_swash_servo_4 || !_swash_servo_5 || !_swash_servo_6) {
return false;
// 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);
}
// set rotor servo range
_rotor.init_servo();
}
// reset swash servo range and endpoints
reset_swash_servo (_swash_servo_1);
reset_swash_servo (_swash_servo_2);
reset_swash_servo (_swash_servo_3);
reset_swash_servo (_swash_servo_4);
reset_swash_servo (_swash_servo_5);
reset_swash_servo (_swash_servo_6);
// set rotor servo range
_rotor.init_servo();
for (uint8_t i=0; i<AP_MOTORS_HELI_DUAL_NUM_SWASHPLATE_SERVOS; i++) {
reset_swash_servo(SRV_Channels::get_motor_function(i));
}
_flags.initialised_ok = true;
@ -513,28 +505,21 @@ void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float c @@ -513,28 +505,21 @@ void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float c
_rotor.set_collective(fabsf(collective_out));
// swashplate servos
float servo1_out = (_rollFactor[CH_1] * roll_out + _pitchFactor[CH_1] * pitch_out + _yawFactor[CH_1] * yaw_out)*0.45f + _collectiveFactor[CH_1] * collective_out_scaled;
float servo2_out = (_rollFactor[CH_2] * roll_out + _pitchFactor[CH_2] * pitch_out + _yawFactor[CH_2] * yaw_out)*0.45f + _collectiveFactor[CH_2] * collective_out_scaled;
float servo3_out = (_rollFactor[CH_3] * roll_out + _pitchFactor[CH_3] * pitch_out + _yawFactor[CH_3] * yaw_out)*0.45f + _collectiveFactor[CH_3] * collective_out_scaled;
float servo4_out = (_rollFactor[CH_4] * roll_out + _pitchFactor[CH_4] * pitch_out + _yawFactor[CH_4] * yaw_out)*0.45f + _collectiveFactor[CH_4] * collective2_out_scaled;
float servo5_out = (_rollFactor[CH_5] * roll_out + _pitchFactor[CH_5] * pitch_out + _yawFactor[CH_5] * yaw_out)*0.45f + _collectiveFactor[CH_5] * collective2_out_scaled;
float servo6_out = (_rollFactor[CH_6] * roll_out + _pitchFactor[CH_6] * pitch_out + _yawFactor[CH_6] * yaw_out)*0.45f + _collectiveFactor[CH_6] * collective2_out_scaled;
float servo_out[AP_MOTORS_HELI_DUAL_NUM_SWASHPLATE_SERVOS];
for (uint8_t i=0; i<AP_MOTORS_HELI_DUAL_NUM_SWASHPLATE_SERVOS; i++) {
servo_out[i] = (_rollFactor[i] * roll_out + _pitchFactor[i] * pitch_out + _yawFactor[i] * yaw_out)*0.45f + _collectiveFactor[i] * collective_out_scaled;
}
// rescale from -1..1, so we can use the pwm calc that includes trim
servo1_out = 2*servo1_out - 1;
servo2_out = 2*servo2_out - 1;
servo3_out = 2*servo3_out - 1;
servo4_out = 2*servo4_out - 1;
servo5_out = 2*servo5_out - 1;
servo6_out = 2*servo6_out - 1;
// actually move the servos
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));
for (uint8_t i=0; i<AP_MOTORS_HELI_DUAL_NUM_SWASHPLATE_SERVOS; i++) {
servo_out[i] = 2*servo_out[i] - 1;
}
// actually move the servos. PWM is sent based on nominal 1500 center. servo output shifts center based on trim value.
for (uint8_t i=0; i<AP_MOTORS_HELI_DUAL_NUM_SWASHPLATE_SERVOS; i++) {
rc_write_swash(i, servo_out[i]);
}
}

7
libraries/AP_Motors/AP_MotorsHeli_Dual.h
Unescape View File

@ -136,13 +136,6 @@ protected: @@ -136,13 +136,6 @@ protected:
AP_Float _dcp_yaw_effect; // feed-forward compensation to automatically add yaw input when differential collective pitch is applied.
AP_Float _yaw_scaler; // scaling factor applied to the yaw mixing
SRV_Channel *_swash_servo_1;
SRV_Channel *_swash_servo_2;
SRV_Channel *_swash_servo_3;
SRV_Channel *_swash_servo_4;
SRV_Channel *_swash_servo_5;
SRV_Channel *_swash_servo_6;
// internal variables
float _collective2_mid_pct = 0.0f; // collective mid parameter value for rear swashplate converted to 0 ~ 1 range
float _rollFactor[AP_MOTORS_HELI_DUAL_NUM_SWASHPLATE_SERVOS];

10
libraries/AP_Motors/AP_MotorsHeli_Quad.cpp
Unescape View File

@ -28,6 +28,8 @@ const AP_Param::GroupInfo AP_MotorsHeli_Quad::var_info[] = { @@ -28,6 +28,8 @@ const AP_Param::GroupInfo AP_MotorsHeli_Quad::var_info[] = {
AP_GROUPEND
};
#define QUAD_SERVO_MAX_ANGLE 4500
// set update rate to motors - a value in hertz
void AP_MotorsHeli_Quad::set_update_rate( uint16_t speed_hz )
{
@ -51,10 +53,8 @@ bool AP_MotorsHeli_Quad::init_outputs() @@ -51,10 +53,8 @@ bool AP_MotorsHeli_Quad::init_outputs()
}
for (uint8_t i=0; i<AP_MOTORS_HELI_QUAD_NUM_MOTORS; i++) {
_servo[i] = SRV_Channels::get_channel_for(SRV_Channels::get_motor_function(i), CH_1+i);
if (!_servo[i]) {
return false;
}
add_motor_num(CH_1+i);
SRV_Channels::set_angle(SRV_Channels::get_motor_function(i), QUAD_SERVO_MAX_ANGLE);
}
// set rotor servo range
@ -270,7 +270,7 @@ void AP_MotorsHeli_Quad::move_actuators(float roll_out, float pitch_out, float c @@ -270,7 +270,7 @@ void AP_MotorsHeli_Quad::move_actuators(float roll_out, float pitch_out, float c
// move the servos
for (uint8_t i=0; i<AP_MOTORS_HELI_QUAD_NUM_MOTORS; i++) {
rc_write(AP_MOTORS_MOT_1+i, calc_pwm_output_1to1(out[i], _servo[i]));
rc_write_angle(AP_MOTORS_MOT_1+i, out[i] * QUAD_SERVO_MAX_ANGLE);
}
}

3
libraries/AP_Motors/AP_MotorsHeli_Quad.h
Unescape View File

@ -86,9 +86,6 @@ protected: @@ -86,9 +86,6 @@ protected:
// objects we depend upon
AP_MotorsHeli_RSC _rotor; // main rotor controller
// parameters
SRV_Channel *_servo[AP_MOTORS_HELI_QUAD_NUM_MOTORS];
// rate factors
float _rollFactor[AP_MOTORS_HELI_QUAD_NUM_MOTORS];
float _pitchFactor[AP_MOTORS_HELI_QUAD_NUM_MOTORS];

4
libraries/AP_Motors/AP_MotorsHeli_RSC.cpp
Unescape View File

@ -25,6 +25,10 @@ void AP_MotorsHeli_RSC::init_servo() @@ -25,6 +25,10 @@ void AP_MotorsHeli_RSC::init_servo()
{
// setup RSC on specified channel by default
SRV_Channels::set_aux_channel_default(_aux_fn, _default_channel);
// set servo range
SRV_Channels::set_range(SRV_Channels::get_motor_function(_aux_fn), 1000);
}
// set_power_output_range

77
libraries/AP_Motors/AP_MotorsHeli_Single.cpp
Unescape View File

@ -130,6 +130,8 @@ const AP_Param::GroupInfo AP_MotorsHeli_Single::var_info[] = { @@ -130,6 +130,8 @@ const AP_Param::GroupInfo AP_MotorsHeli_Single::var_info[] = {
AP_GROUPEND
};
#define YAW_SERVO_MAX_ANGLE 4500
// set update rate to motors - a value in hertz
void AP_MotorsHeli_Single::set_update_rate( uint16_t speed_hz )
{
@ -149,33 +151,38 @@ void AP_MotorsHeli_Single::set_update_rate( uint16_t speed_hz ) @@ -149,33 +151,38 @@ void AP_MotorsHeli_Single::set_update_rate( uint16_t speed_hz )
bool AP_MotorsHeli_Single::init_outputs()
{
if (!_flags.initialised_ok) {
_swash_servo_1 = SRV_Channels::get_channel_for(SRV_Channel::k_motor1, CH_1);
_swash_servo_2 = SRV_Channels::get_channel_for(SRV_Channel::k_motor2, CH_2);
_swash_servo_3 = SRV_Channels::get_channel_for(SRV_Channel::k_motor3, CH_3);
_yaw_servo = SRV_Channels::get_channel_for(SRV_Channel::k_motor4, CH_4);
// map primary swash servos
for (uint8_t i=0; i<AP_MOTORS_HELI_SINGLE_NUM_SWASHPLATE_SERVOS; i++) {
add_motor_num(CH_1+i);
}
// yaw servo
add_motor_num(CH_4);
// initialize main rotor servo
_main_rotor.init_servo();
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) {
_tail_rotor.init_servo();
if (!_swash_servo_1 || !_swash_servo_2 || !_swash_servo_3 || !_yaw_servo) {
return false;
}
} else {
_servo_aux = SRV_Channels::get_channel_for(SRV_Channel::k_motor7, CH_7);
if (!_swash_servo_1 || !_swash_servo_2 || !_swash_servo_3 || !_yaw_servo || !_servo_aux) {
return false;
}
} else if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) {
// external gyro output
add_motor_num(AP_MOTORS_HELI_SINGLE_EXTGYRO);
}
}
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) {
// External Gyro uses PWM output thus servo endpoints are forced
SRV_Channels::set_output_min_max(SRV_Channels::get_motor_function(AP_MOTORS_HELI_SINGLE_EXTGYRO), 1000, 2000);
}
// reset swash servo range and endpoints
reset_swash_servo (_swash_servo_1);
reset_swash_servo (_swash_servo_2);
reset_swash_servo (_swash_servo_3);
for (uint8_t i=0; i<AP_MOTORS_HELI_SINGLE_NUM_SWASHPLATE_SERVOS; i++) {
reset_swash_servo(SRV_Channels::get_motor_function(i));
}
_yaw_servo->set_angle(4500);
// yaw servo is an angle from -4500 to 4500
SRV_Channels::set_angle(SRV_Channel::k_motor4, YAW_SERVO_MAX_ANGLE);
// set main rotor servo range
// tail rotor servo use range as set in vehicle code for rc7
_main_rotor.init_servo();
_flags.initialised_ok = true;
return true;
}
@ -208,9 +215,9 @@ void AP_MotorsHeli_Single::output_test_seq(uint8_t motor_seq, int16_t pwm) @@ -208,9 +215,9 @@ void AP_MotorsHeli_Single::output_test_seq(uint8_t motor_seq, int16_t pwm)
// external gyro & tail servo
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) {
if (_acro_tail && _ext_gyro_gain_acro > 0) {
write_aux(_ext_gyro_gain_acro*0.001f);
rc_write(AP_MOTORS_HELI_SINGLE_EXTGYRO, _ext_gyro_gain_acro);
} else {
write_aux(_ext_gyro_gain_std*0.001f);
rc_write(AP_MOTORS_HELI_SINGLE_EXTGYRO, _ext_gyro_gain_std);
}
}
rc_write(AP_MOTORS_MOT_4, pwm);
@ -341,7 +348,7 @@ void AP_MotorsHeli_Single::calculate_roll_pitch_collective_factors() @@ -341,7 +348,7 @@ void AP_MotorsHeli_Single::calculate_roll_pitch_collective_factors()
uint16_t AP_MotorsHeli_Single::get_motor_mask()
{
// heli uses channels 1,2,3,4,7 and 8
return rc_map_mask(1U << 0 | 1U << 1 | 1U << 2 | 1U << 3 | 1U << AP_MOTORS_HELI_SINGLE_AUX | 1U << AP_MOTORS_HELI_SINGLE_RSC);
return rc_map_mask(1U << 0 | 1U << 1 | 1U << 2 | 1U << 3 | 1U << AP_MOTORS_HELI_SINGLE_EXTGYRO | 1U << AP_MOTORS_HELI_SINGLE_RSC);
}
// update_motor_controls - sends commands to motor controllers
@ -456,10 +463,10 @@ void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float @@ -456,10 +463,10 @@ void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float
servo2_out = 2*servo2_out - 1;
servo3_out = 2*servo3_out - 1;
// actually move the servos
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));
// actually move the servos. PWM is sent based on nominal 1500 center. servo output shifts center based on trim value.
rc_write_swash(AP_MOTORS_MOT_1, servo1_out);
rc_write_swash(AP_MOTORS_MOT_2, servo2_out);
rc_write_swash(AP_MOTORS_MOT_3, servo3_out);
// update the yaw rate using the tail rotor/servo
move_yaw(yaw_out + yaw_offset);
@ -483,32 +490,24 @@ void AP_MotorsHeli_Single::move_yaw(float yaw_out) @@ -483,32 +490,24 @@ void AP_MotorsHeli_Single::move_yaw(float yaw_out)
// constrain output so that motor never fully stops
yaw_out = constrain_float(yaw_out, -0.9f, 1.0f);
// output yaw servo to tail rsc
rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(yaw_out, _yaw_servo));
rc_write_angle(AP_MOTORS_MOT_4, yaw_out * YAW_SERVO_MAX_ANGLE);
} else {
// output zero speed to tail rsc
rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(-1.0f, _yaw_servo));
rc_write_angle(AP_MOTORS_MOT_4, -YAW_SERVO_MAX_ANGLE);
}
} else {
rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(yaw_out, _yaw_servo));
rc_write_angle(AP_MOTORS_MOT_4, yaw_out * YAW_SERVO_MAX_ANGLE);
}
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) {
// output gain to exernal gyro
if (_acro_tail && _ext_gyro_gain_acro > 0) {
write_aux(_ext_gyro_gain_acro*0.001f);
rc_write(AP_MOTORS_HELI_SINGLE_EXTGYRO, 1000 + _ext_gyro_gain_acro);
} else {
write_aux(_ext_gyro_gain_std*0.001f);
rc_write(AP_MOTORS_HELI_SINGLE_EXTGYRO, 1000 + _ext_gyro_gain_std);
}
}
}
// write_aux - converts servo_out parameter value (0 to 1 range) to pwm and outputs to aux channel (ch7)
void AP_MotorsHeli_Single::write_aux(float servo_out)
{
if (_servo_aux) {
rc_write(AP_MOTORS_HELI_SINGLE_AUX, calc_pwm_output_0to1(servo_out, _servo_aux));
}
}
// servo_test - move servos through full range of movement
void AP_MotorsHeli_Single::servo_test()
{

16
libraries/AP_Motors/AP_MotorsHeli_Single.h
Unescape View File

@ -8,9 +8,10 @@ @@ -8,9 +8,10 @@
#include "AP_MotorsHeli.h"
#include "AP_MotorsHeli_RSC.h"
// rsc and aux function output channels
// rsc and extgyro function output channels.
#define AP_MOTORS_HELI_SINGLE_RSC CH_8
#define AP_MOTORS_HELI_SINGLE_AUX CH_7
#define AP_MOTORS_HELI_SINGLE_EXTGYRO CH_7
#define AP_MOTORS_HELI_SINGLE_TAILRSC CH_7
// servo position defaults
#define AP_MOTORS_HELI_SINGLE_SERVO1_POS -60
@ -52,7 +53,7 @@ public: @@ -52,7 +53,7 @@ public:
uint16_t speed_hz = AP_MOTORS_HELI_SPEED_DEFAULT) :
AP_MotorsHeli(loop_rate, speed_hz),
_main_rotor(SRV_Channel::k_heli_rsc, AP_MOTORS_HELI_SINGLE_RSC),
_tail_rotor(SRV_Channel::k_heli_tail_rsc, AP_MOTORS_HELI_SINGLE_AUX)
_tail_rotor(SRV_Channel::k_heli_tail_rsc, AP_MOTORS_HELI_SINGLE_TAILRSC)
{
AP_Param::setup_object_defaults(this, var_info);
};
@ -121,9 +122,6 @@ protected: @@ -121,9 +122,6 @@ protected:
// move_yaw - moves the yaw servo
void move_yaw(float yaw_out);
// write_aux - converts servo_out parameter value (0 to 1 range) to pwm and outputs to aux channel (ch7)
void write_aux(float servo_out);
// servo_test - move servos through full range of movement
void servo_test() override;
@ -153,12 +151,6 @@ protected: @@ -153,12 +151,6 @@ protected:
AP_Int8 _flybar_mode; // Flybar present or not. Affects attitude controller used during ACRO flight mode
AP_Int16 _direct_drive_tailspeed; // Direct Drive VarPitch Tail ESC speed (0 ~ 1000)
SRV_Channel *_swash_servo_1;
SRV_Channel *_swash_servo_2;
SRV_Channel *_swash_servo_3;
SRV_Channel *_yaw_servo;
SRV_Channel *_servo_aux;
bool _acro_tail = false;
float _rollFactor[AP_MOTORS_HELI_SINGLE_NUM_SWASHPLATE_SERVOS];
float _pitchFactor[AP_MOTORS_HELI_SINGLE_NUM_SWASHPLATE_SERVOS];

36
libraries/AP_Motors/AP_Motors_Class.cpp
Unescape View File

@ -161,42 +161,6 @@ uint32_t AP_Motors::rc_map_mask(uint32_t mask) const @@ -161,42 +161,6 @@ uint32_t AP_Motors::rc_map_mask(uint32_t mask) const
return mask2;
}
// convert input in -1 to +1 range to pwm output
int16_t AP_Motors::calc_pwm_output_1to1(float input, const SRV_Channel *servo)
{
int16_t ret;
input = constrain_float(input, -1.0f, 1.0f);
if (servo->get_reversed()) {
input = -input;
}
if (input >= 0.0f) {
ret = ((input * (servo->get_output_max() - servo->get_trim())) + servo->get_trim());
} else {
ret = ((input * (servo->get_trim() - servo->get_output_min())) + servo->get_trim());
}
return constrain_int16(ret, servo->get_output_min(), servo->get_output_max());
}
// convert input in 0 to +1 range to pwm output
int16_t AP_Motors::calc_pwm_output_0to1(float input, const SRV_Channel *servo)
{
int16_t ret;
input = constrain_float(input, 0.0f, 1.0f);
if (servo->get_reversed()) {
input = 1.0f-input;
}
ret = input * (servo->get_output_max() - servo->get_output_min()) + servo->get_output_min();
return constrain_int16(ret, servo->get_output_min(), servo->get_output_max());
}
/*
add a motor, setting up default output function as needed
*/

6
libraries/AP_Motors/AP_Motors_Class.h
Unescape View File

@ -178,12 +178,6 @@ protected: @@ -178,12 +178,6 @@ protected:
// save parameters as part of disarming
virtual void save_params_on_disarm() {}
// convert input in -1 to +1 range to pwm output
int16_t calc_pwm_output_1to1(float input, const SRV_Channel *servo);
// convert input in 0 to +1 range to pwm output
int16_t calc_pwm_output_0to1(float input, const SRV_Channel *servo);
// flag bitmask
struct AP_Motors_flags {
uint8_t armed : 1; // 0 if disarmed, 1 if armed

Write Preview
Loading…
Cancel
Save