aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Motors: RCMAP fix

Browse Source 
Remove all RC Input channels passed as reference into AP_Motors.  All input handling self-contained inside AP_Motors.
Rework Tricopter to use internal servo calcs.
This commit is contained in:
Robert Lefebvre 2015-05-14 21:00:46 -04:00 committed by Randy Mackay
parent 70a9a5699c
commit b8181b6b90
17 changed files with 334 additions and 231 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
libraries/AP_Motors/AP_MotorsCoax.cpp
View File

@ -119,13 +119,14 @@ void AP_MotorsCoax::output_min()
// send minimum value to each motor
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]), _servo1.radio_trim);
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), _servo2.radio_trim);
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_3]), _rc_throttle.radio_min);
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), _rc_throttle.radio_min);
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_3]), _throttle_radio_min);
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), _throttle_radio_min);
}
void AP_MotorsCoax::output_armed_not_stabilizing()
{
int16_t out_min = _rc_throttle.radio_min + _min_throttle;
int16_t throttle_radio_output; // total throttle pwm value, summed onto throttle channel minimum, typically ~1100-1900
int16_t out_min = _throttle_radio_min + _min_throttle;
int16_t motor_out;
int16_t min_thr = rel_pwm_to_thr_range(_spin_when_armed_ramped);
@ -136,18 +137,18 @@ void AP_MotorsCoax::output_armed_not_stabilizing()
limit.throttle_lower = false;
limit.throttle_upper = false;
if (_rc_throttle.servo_out <= min_thr) {
_rc_throttle.servo_out = min_thr;
if (_throttle_control_input <= min_thr) {
_throttle_control_input = min_thr;
limit.throttle_lower = true;
}
if (_rc_throttle.servo_out >= _max_throttle) {
_rc_throttle.servo_out = _max_throttle;
if (_throttle_control_input >= _max_throttle) {
_throttle_control_input = _max_throttle;
limit.throttle_upper = true;
}
_rc_throttle.calc_pwm();
throttle_radio_output = calc_throttle_radio_output();
motor_out = _rc_throttle.radio_out;
motor_out = throttle_radio_output;
_servo1.servo_out = 0;
_servo1.calc_pwm();
@ -156,7 +157,7 @@ void AP_MotorsCoax::output_armed_not_stabilizing()
_servo2.calc_pwm();
if (motor_out >= out_min) {
motor_out = apply_thrust_curve_and_volt_scaling(motor_out, out_min, _rc_throttle.radio_max);
motor_out = apply_thrust_curve_and_volt_scaling(motor_out, out_min, _throttle_radio_max);
}
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]), _servo1.radio_out);
@ -169,7 +170,9 @@ void AP_MotorsCoax::output_armed_not_stabilizing()
// TODO pull code that is common to output_armed_not_stabilizing into helper functions
void AP_MotorsCoax::output_armed_stabilizing()
{
int16_t out_min = _rc_throttle.radio_min + _min_throttle;
int16_t yaw_pwm; // yaw pwm value, initially calculated by calc_yaw_pwm() but may be modified after, +/- 400
int16_t throttle_radio_output; // total throttle pwm value, summed onto throttle channel minimum, typically ~1100-1900
int16_t out_min = _throttle_radio_min + _min_throttle;
int16_t motor_out[4];
// initialize limits flags
@ -178,36 +181,36 @@ void AP_MotorsCoax::output_armed_stabilizing()
limit.throttle_lower = false;
limit.throttle_upper = false;
if (_rc_throttle.servo_out <= _min_throttle) {
_rc_throttle.servo_out = _min_throttle;
if (_throttle_control_input <= _min_throttle) {
_throttle_control_input = _min_throttle;
limit.throttle_lower = true;
}
if (_rc_throttle.servo_out >= _max_throttle) {
_rc_throttle.servo_out = _max_throttle;
if (_throttle_control_input >= _max_throttle) {
_throttle_control_input = _max_throttle;
limit.throttle_upper = true;
}
// capture desired throttle and yaw from receiver
_rc_throttle.calc_pwm();
_rc_yaw.calc_pwm();
// calculate throttle and yaw PWM
throttle_radio_output = calc_throttle_radio_output();
yaw_pwm = calc_yaw_pwm();
// motors
motor_out[AP_MOTORS_MOT_3] = _rev_yaw*_rc_yaw.pwm_out + _rc_throttle.radio_out;
motor_out[AP_MOTORS_MOT_4] = -_rev_yaw*_rc_yaw.pwm_out +_rc_throttle.radio_out;
motor_out[AP_MOTORS_MOT_3] = _rev_yaw*yaw_pwm + throttle_radio_output;
motor_out[AP_MOTORS_MOT_4] = -_rev_yaw*yaw_pwm + throttle_radio_output;
// TODO: set limits.roll_pitch and limits.yaw
// front
_servo1.servo_out = _rev_roll*_rc_roll.servo_out;
_servo1.servo_out = _rev_roll*_roll_control_input;
// right
_servo2.servo_out = _rev_pitch*_rc_pitch.servo_out;
_servo2.servo_out = _rev_pitch*_pitch_control_input;
_servo1.calc_pwm();
_servo2.calc_pwm();
// adjust for thrust curve and voltage scaling
motor_out[AP_MOTORS_MOT_3] = apply_thrust_curve_and_volt_scaling(motor_out[AP_MOTORS_MOT_3], out_min, _rc_throttle.radio_max);
motor_out[AP_MOTORS_MOT_4] = apply_thrust_curve_and_volt_scaling(motor_out[AP_MOTORS_MOT_4], out_min, _rc_throttle.radio_max);
motor_out[AP_MOTORS_MOT_3] = apply_thrust_curve_and_volt_scaling(motor_out[AP_MOTORS_MOT_3], out_min, _throttle_radio_max);
motor_out[AP_MOTORS_MOT_4] = apply_thrust_curve_and_volt_scaling(motor_out[AP_MOTORS_MOT_4], out_min, _throttle_radio_max);
// ensure motors don't drop below a minimum value and stop
motor_out[AP_MOTORS_MOT_3] = max(motor_out[AP_MOTORS_MOT_3], out_min);

4
libraries/AP_Motors/AP_MotorsCoax.h
View File

@ -25,8 +25,8 @@ class AP_MotorsCoax : public AP_Motors {
public:
/// Constructor
AP_MotorsCoax( RC_Channel& rc_roll, RC_Channel& rc_pitch, RC_Channel& rc_throttle, RC_Channel& rc_yaw, RC_Channel& servo1, RC_Channel& servo2, uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) :
AP_Motors(rc_roll, rc_pitch, rc_throttle, rc_yaw, loop_rate, speed_hz),
AP_MotorsCoax(RC_Channel& servo1, RC_Channel& servo2, uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) :
AP_Motors(loop_rate, speed_hz),
_servo1(servo1),
_servo2(servo2)
{

39
libraries/AP_Motors/AP_MotorsHeli.cpp
View File

@ -403,18 +403,13 @@ void AP_MotorsHeli::output_armed_stabilizing()
{
// if manual override (i.e. when setting up swash), pass pilot commands straight through to swash
if (_servo_manual == 1) {
_rc_roll.servo_out = _rc_roll.control_in;
_rc_pitch.servo_out = _rc_pitch.control_in;
_rc_throttle.servo_out = _rc_throttle.control_in;
_rc_yaw.servo_out = _rc_yaw.control_in;
_roll_control_input = _roll_radio_passthrough;
_pitch_control_input = _pitch_radio_passthrough;
_throttle_control_input = _throttle_radio_passthrough;
_yaw_control_input = _yaw_radio_passthrough;
}
_rc_roll.calc_pwm();
_rc_pitch.calc_pwm();
_rc_throttle.calc_pwm();
_rc_yaw.calc_pwm();
move_swash(_rc_roll.servo_out, _rc_pitch.servo_out, _rc_throttle.servo_out, _rc_yaw.servo_out);
move_swash(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input);
// update rotor and direct drive esc speeds
rsc_control();
@ -456,7 +451,7 @@ void AP_MotorsHeli::reset_swash()
// set roll, pitch and throttle scaling
_roll_scaler = 1.0f;
_pitch_scaler = 1.0f;
_collective_scalar = ((float)(_rc_throttle.radio_max - _rc_throttle.radio_min))/1000.0f;
_collective_scalar = ((float)(_throttle_radio_max - _throttle_radio_min))/1000.0f;
_collective_scalar_manual = 1.0f;
// we must be in set-up mode so mark swash as uninitialised
@ -568,7 +563,7 @@ void AP_MotorsHeli::move_swash(int16_t roll_out, int16_t pitch_out, int16_t coll
}
// To-Do: This equation seems to be wrong. It probably restricts swash movement so that swash setup doesn't work right.
// _collective_scalar should probably not be used or set to 1?
coll_out_scaled = coll_in * _collective_scalar + _rc_throttle.radio_min - 1000;
coll_out_scaled = coll_in * _collective_scalar + _throttle_radio_min - 1000;
}else{ // regular flight mode
// check if we need to reinitialise the swash
@ -846,5 +841,23 @@ void AP_MotorsHeli::update_throttle_filter()
_throttle_filter.apply(_throttle_in, 1.0f/_loop_rate);
// prevent _rc_throttle.servo_out from wrapping at int16 max or min
_rc_throttle.servo_out = constrain_float(_throttle_filter.get(),-32000,32000);
_throttle_control_input = constrain_float(_throttle_filter.get(),-32000,32000);
}
// set_radio_passthrough used to pass radio inputs directly to outputs
void AP_MotorsHeli::set_radio_passthrough(int16_t radio_roll_input, int16_t radio_pitch_input, int16_t radio_throttle_input, int16_t radio_yaw_input)
{
_roll_radio_passthrough = radio_roll_input;
_pitch_radio_passthrough = radio_pitch_input;
_throttle_radio_passthrough = radio_throttle_input;
_yaw_radio_passthrough = radio_yaw_input;
}
// reset_radio_passthrough used to reset all radio inputs to center
void AP_MotorsHeli::reset_radio_passthrough()
{
_roll_radio_passthrough = 0;
_pitch_radio_passthrough = 0;
_throttle_radio_passthrough = 500;
_yaw_radio_passthrough = 0;
}

18
libraries/AP_Motors/AP_MotorsHeli.h
View File

@ -89,11 +89,7 @@ class AP_MotorsHeli : public AP_Motors {
public:
/// Constructor
AP_MotorsHeli( RC_Channel& rc_roll,
RC_Channel& rc_pitch,
RC_Channel& rc_throttle,
RC_Channel& rc_yaw,
RC_Channel& servo_aux,
AP_MotorsHeli( RC_Channel& servo_aux,
RC_Channel& servo_rotor,
RC_Channel& swash_servo_1,
RC_Channel& swash_servo_2,
@ -101,7 +97,7 @@ public:
RC_Channel& yaw_servo,
uint16_t loop_rate,
uint16_t speed_hz = AP_MOTORS_HELI_SPEED_DEFAULT) :
AP_Motors(rc_roll, rc_pitch, rc_throttle, rc_yaw, loop_rate, speed_hz),
AP_Motors(loop_rate, speed_hz),
_servo_aux(servo_aux),
_servo_rsc(servo_rotor),
_servo_1(swash_servo_1),
@ -207,6 +203,12 @@ public:
// this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
virtual uint16_t get_motor_mask();
// set_radio_passthrough used to pass radio inputs directly to outputs
void set_radio_passthrough(int16_t radio_roll_input, int16_t radio_pitch_input, int16_t radio_throttle_input, int16_t radio_yaw_input);
// reset_radio_passthrough used to reset all radio inputs to center
void reset_radio_passthrough();
// output - sends commands to the motors
void output();
@ -310,6 +312,10 @@ private:
int16_t _tail_direct_drive_out; // current ramped speed of output on ch7 when using direct drive variable pitch tail type
float _dt; // main loop time
int16_t _delta_phase_angle; // phase angle dynamic compensation
int16_t _roll_radio_passthrough; // roll control PWM direct from radio, used for manual control
int16_t _pitch_radio_passthrough; // pitch control PWM direct from radio, used for manual control
int16_t _throttle_radio_passthrough;// throttle control PWM direct from radio, used for manual control
int16_t _yaw_radio_passthrough; // yaw control PWM direct from radio, used for manual control
};
#endif // AP_MOTORSHELI

5
libraries/AP_Motors/AP_MotorsHexa.h
View File

@ -16,8 +16,9 @@ class AP_MotorsHexa : public AP_MotorsMatrix {
public:
/// Constructor
AP_MotorsHexa(RC_Channel& rc_roll, RC_Channel& rc_pitch, RC_Channel& rc_throttle, RC_Channel& rc_yaw, uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMatrix(rc_roll, rc_pitch, rc_throttle, rc_yaw, loop_rate, speed_hz) {
};
AP_MotorsHexa(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) :
AP_MotorsMatrix(loop_rate, speed_hz)
{ };
// setup_motors - configures the motors for a hexa
virtual void setup_motors();

75
libraries/AP_Motors/AP_MotorsMatrix.cpp
View File

@ -100,7 +100,7 @@ void AP_MotorsMatrix::output_min()
// fill the motor_out[] array for HIL use and send minimum value to each motor
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( motor_enabled[i] ) {
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[i]), _rc_throttle.radio_min);
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[i]), _throttle_radio_min);
}
}
}
@ -121,9 +121,10 @@ uint16_t AP_MotorsMatrix::get_motor_mask()
void AP_MotorsMatrix::output_armed_not_stabilizing()
{
int8_t i;
int16_t motor_out[AP_MOTORS_MAX_NUM_MOTORS]; // final outputs sent to the motors
int16_t out_min_pwm = _rc_throttle.radio_min + _min_throttle; // minimum pwm value we can send to the motors
int16_t out_max_pwm = _rc_throttle.radio_max; // maximum pwm value we can send to the motors
int16_t throttle_radio_output; // total throttle pwm value, summed onto throttle channel minimum, typically ~1100-1900
int16_t motor_out[AP_MOTORS_MAX_NUM_MOTORS]; // final outputs sent to the motors
int16_t out_min_pwm = _throttle_radio_min + _min_throttle; // minimum pwm value we can send to the motors
int16_t out_max_pwm = _throttle_radio_max; // maximum pwm value we can send to the motors
// initialize limits flags
limit.roll_pitch = true;
@ -133,26 +134,26 @@ void AP_MotorsMatrix::output_armed_not_stabilizing()
int16_t min_thr = rel_pwm_to_thr_range(_spin_when_armed_ramped);
if (_rc_throttle.servo_out <= min_thr) {
_rc_throttle.servo_out = min_thr;
if (_throttle_control_input <= min_thr) {
_throttle_control_input = min_thr;
limit.throttle_lower = true;
}
if (_rc_throttle.servo_out >= _hover_out) {
_rc_throttle.servo_out = _hover_out;
if (_throttle_control_input >= _hover_out) {
_throttle_control_input = _hover_out;
limit.throttle_upper = true;
}
_rc_throttle.calc_pwm();
throttle_radio_output = calc_throttle_radio_output();
// set output throttle
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
motor_out[i] = _rc_throttle.radio_out;
motor_out[i] = throttle_radio_output;
}
}
if(_rc_throttle.radio_out >= out_min_pwm) {
if(throttle_radio_output >= out_min_pwm) {
// apply thrust curve and voltage scaling
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
@ -175,11 +176,15 @@ void AP_MotorsMatrix::output_armed_not_stabilizing()
void AP_MotorsMatrix::output_armed_stabilizing()
{
int8_t i;
int16_t out_min_pwm = _rc_throttle.radio_min + _min_throttle; // minimum pwm value we can send to the motors
int16_t out_max_pwm = _rc_throttle.radio_max; // maximum pwm value we can send to the motors
int16_t out_mid_pwm = (out_min_pwm+out_max_pwm)/2; // mid pwm value we can send to the motors
int16_t out_best_thr_pwm; // the is the best throttle we can come up which provides good control without climbing
float rpy_scale = 1.0; // this is used to scale the roll, pitch and yaw to fit within the motor limits
int16_t roll_pwm; // roll pwm value, initially calculated by calc_roll_pwm() but may be modified after, +/- 400
int16_t pitch_pwm; // pitch pwm value, initially calculated by calc_roll_pwm() but may be modified after, +/- 400
int16_t yaw_pwm; // yaw pwm value, initially calculated by calc_yaw_pwm() but may be modified after, +/- 400
int16_t throttle_radio_output; // total throttle pwm value, summed onto throttle channel minimum, typically ~1100-1900
int16_t out_min_pwm = _throttle_radio_min + _min_throttle; // minimum pwm value we can send to the motors
int16_t out_max_pwm = _throttle_radio_max; // maximum pwm value we can send to the motors
int16_t out_mid_pwm = (out_min_pwm+out_max_pwm)/2; // mid pwm value we can send to the motors
int16_t out_best_thr_pwm; // the is the best throttle we can come up which provides good control without climbing
float rpy_scale = 1.0; // this is used to scale the roll, pitch and yaw to fit within the motor limits
int16_t rpy_out[AP_MOTORS_MAX_NUM_MOTORS]; // buffer so we don't have to multiply coefficients multiple times.
int16_t motor_out[AP_MOTORS_MAX_NUM_MOTORS]; // final outputs sent to the motors
@ -195,29 +200,27 @@ void AP_MotorsMatrix::output_armed_stabilizing()
limit.throttle_lower = false;
limit.throttle_upper = false;
// Throttle is 0 to 1000 only
// To-Do: we should not really be limiting this here because we don't "own" this _rc_throttle object
if (_rc_throttle.servo_out <= _min_throttle) {
_rc_throttle.servo_out = _min_throttle;
// Ensure throttle is within bounds of 0 to 1000
if (_throttle_control_input <= _min_throttle) {
_throttle_control_input = _min_throttle;
limit.throttle_lower = true;
}
if (_rc_throttle.servo_out >= _max_throttle) {
_rc_throttle.servo_out = _max_throttle;
if (_throttle_control_input >= _max_throttle) {
_throttle_control_input = _max_throttle;
limit.throttle_upper = true;
}
// capture desired roll, pitch, yaw and throttle from receiver
_rc_roll.calc_pwm();
_rc_pitch.calc_pwm();
_rc_throttle.calc_pwm();
_rc_yaw.calc_pwm();
roll_pwm = calc_roll_pwm();
pitch_pwm = calc_pitch_pwm();
yaw_pwm = calc_yaw_pwm();
throttle_radio_output = calc_throttle_radio_output();
// calculate roll and pitch for each motor
// set rpy_low and rpy_high to the lowest and highest values of the motors
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
rpy_out[i] = _rc_roll.pwm_out * _roll_factor[i] * get_compensation_gain() +
_rc_pitch.pwm_out * _pitch_factor[i] * get_compensation_gain();
rpy_out[i] = roll_pwm * _roll_factor[i] * get_compensation_gain() +
pitch_pwm * _pitch_factor[i] * get_compensation_gain();
// record lowest roll pitch command
if (rpy_out[i] < rpy_low) {
@ -240,25 +243,25 @@ void AP_MotorsMatrix::output_armed_stabilizing()
// We will choose #1 (the best throttle for yaw control) if that means reducing throttle to the motors (i.e. we favour reducing throttle *because* it provides better yaw control)
// We will choose #2 (a mix of pilot and hover throttle) only when the throttle is quite low. We favour reducing throttle instead of better yaw control because the pilot has commanded it
int16_t motor_mid = (rpy_low+rpy_high)/2;
out_best_thr_pwm = min(out_mid_pwm - motor_mid, max(_rc_throttle.radio_out, _rc_throttle.radio_out*max(0,1.0f-_throttle_thr_mix)+get_hover_throttle_as_pwm()*_throttle_thr_mix));
out_best_thr_pwm = min(out_mid_pwm - motor_mid, max(throttle_radio_output, throttle_radio_output*max(0,1.0f-_throttle_thr_mix)+get_hover_throttle_as_pwm()*_throttle_thr_mix));
// calculate amount of yaw we can fit into the throttle range
// this is always equal to or less than the requested yaw from the pilot or rate controller
yaw_allowed = min(out_max_pwm - out_best_thr_pwm, out_best_thr_pwm - out_min_pwm) - (rpy_high-rpy_low)/2;
yaw_allowed = max(yaw_allowed, _yaw_headroom);
if (_rc_yaw.pwm_out >= 0) {
if (yaw_pwm >= 0) {
// if yawing right
if (yaw_allowed > _rc_yaw.pwm_out * get_compensation_gain()) {
yaw_allowed = _rc_yaw.pwm_out * get_compensation_gain(); // to-do: this is bad form for yaw_allows to change meaning to become the amount that we are going to output
if (yaw_allowed > yaw_pwm * get_compensation_gain()) {
yaw_allowed = yaw_pwm * get_compensation_gain(); // to-do: this is bad form for yaw_allows to change meaning to become the amount that we are going to output
}else{
limit.yaw = true;
}
}else{
// if yawing left
yaw_allowed = -yaw_allowed;
if (yaw_allowed < _rc_yaw.pwm_out * get_compensation_gain()) {
yaw_allowed = _rc_yaw.pwm_out * get_compensation_gain(); // to-do: this is bad form for yaw_allows to change meaning to become the amount that we are going to output
if (yaw_allowed < yaw_pwm * get_compensation_gain()) {
yaw_allowed = yaw_pwm * get_compensation_gain(); // to-do: this is bad form for yaw_allows to change meaning to become the amount that we are going to output
}else{
limit.yaw = true;
}
@ -284,7 +287,7 @@ void AP_MotorsMatrix::output_armed_stabilizing()
}
// check everything fits
thr_adj = _rc_throttle.radio_out - out_best_thr_pwm;
thr_adj = throttle_radio_output - out_best_thr_pwm;
// calculate upper and lower limits of thr_adj
int16_t thr_adj_max = max(out_max_pwm-(out_best_thr_pwm+rpy_high),0);

4
libraries/AP_Motors/AP_MotorsMatrix.h
View File

@ -19,8 +19,8 @@ class AP_MotorsMatrix : public AP_Motors {
public:
/// Constructor
AP_MotorsMatrix(RC_Channel& rc_roll, RC_Channel& rc_pitch, RC_Channel& rc_throttle, RC_Channel& rc_yaw, uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) :
AP_Motors(rc_roll, rc_pitch, rc_throttle, rc_yaw, loop_rate, speed_hz)
AP_MotorsMatrix(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) :
AP_Motors(loop_rate, speed_hz)
{};
// init

5
libraries/AP_Motors/AP_MotorsOcta.h
View File

@ -16,8 +16,9 @@ class AP_MotorsOcta : public AP_MotorsMatrix {
public:
/// Constructor
AP_MotorsOcta(RC_Channel& rc_roll, RC_Channel& rc_pitch, RC_Channel& rc_throttle, RC_Channel& rc_yaw, uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMatrix(rc_roll, rc_pitch, rc_throttle, rc_yaw, loop_rate, speed_hz) {
};
AP_MotorsOcta(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) :
AP_MotorsMatrix(loop_rate, speed_hz)
{ };
// setup_motors - configures the motors for an octa
virtual void setup_motors();

5
libraries/AP_Motors/AP_MotorsOctaQuad.h
View File

@ -16,8 +16,9 @@ class AP_MotorsOctaQuad : public AP_MotorsMatrix {
public:
/// Constructor
AP_MotorsOctaQuad(RC_Channel& rc_roll, RC_Channel& rc_pitch, RC_Channel& rc_throttle, RC_Channel& rc_yaw, uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMatrix(rc_roll, rc_pitch, rc_throttle, rc_yaw, loop_rate, speed_hz) {
};
AP_MotorsOctaQuad(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) :
AP_MotorsMatrix(loop_rate, speed_hz)
{ };
// setup_motors - configures the motors for a quad
virtual void setup_motors();

5
libraries/AP_Motors/AP_MotorsQuad.h
View File

@ -16,8 +16,9 @@ class AP_MotorsQuad : public AP_MotorsMatrix {
public:
/// Constructor
AP_MotorsQuad(RC_Channel& rc_roll, RC_Channel& rc_pitch, RC_Channel& rc_throttle, RC_Channel& rc_yaw, uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMatrix(rc_roll, rc_pitch, rc_throttle, rc_yaw, loop_rate, speed_hz) {
};
AP_MotorsQuad(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) :
AP_MotorsMatrix(loop_rate, speed_hz)
{ };
// setup_motors - configures the motors for a quad
virtual void setup_motors();

58
libraries/AP_Motors/AP_MotorsSingle.cpp
View File

@ -126,7 +126,7 @@ void AP_MotorsSingle::output_min()
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), _servo2.radio_trim);
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_3]), _servo3.radio_trim);
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), _servo4.radio_trim);
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_7]), _rc_throttle.radio_min);
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_7]), _throttle_radio_min);
}
// get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used)
@ -139,9 +139,8 @@ uint16_t AP_MotorsSingle::get_motor_mask()
void AP_MotorsSingle::output_armed_not_stabilizing()
{
int16_t out_min = _rc_throttle.radio_min + _min_throttle;
int16_t motor_out;
int16_t throttle_radio_output; // total throttle pwm value, summed onto throttle channel minimum, typically ~1100-1900
int16_t out_min = _throttle_radio_min + _min_throttle;
int16_t min_thr = rel_pwm_to_thr_range(_spin_when_armed_ramped);
// initialize limits flags
@ -150,18 +149,16 @@ void AP_MotorsSingle::output_armed_not_stabilizing()
limit.throttle_lower = false;
limit.throttle_upper = false;
if (_rc_throttle.servo_out <= min_thr) {
_rc_throttle.servo_out = min_thr;
if (_throttle_control_input <= min_thr) {
_throttle_control_input = min_thr;
limit.throttle_lower = true;
}
if (_rc_throttle.servo_out >= _max_throttle) {
_rc_throttle.servo_out = _max_throttle;
if (_throttle_control_input >= _max_throttle) {
_throttle_control_input = _max_throttle;
limit.throttle_upper = true;
}
_rc_throttle.calc_pwm();
motor_out = _rc_throttle.radio_out;
throttle_radio_output = calc_throttle_radio_output();
// front servo
_servo1.servo_out = 0;
@ -177,23 +174,23 @@ void AP_MotorsSingle::output_armed_not_stabilizing()
_servo3.calc_pwm();
_servo4.calc_pwm();
if (motor_out >= out_min) {
motor_out = apply_thrust_curve_and_volt_scaling(motor_out, out_min, _rc_throttle.radio_max);
if (throttle_radio_output >= out_min) {
throttle_radio_output = apply_thrust_curve_and_volt_scaling(throttle_radio_output, out_min, _throttle_radio_max);
}
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]), _servo1.radio_out);
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), _servo2.radio_out);
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_3]), _servo3.radio_out);
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), _servo4.radio_out);
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_7]), motor_out);
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_7]), throttle_radio_output);
}
// sends commands to the motors
// TODO pull code that is common to output_armed_not_stabilizing into helper functions
void AP_MotorsSingle::output_armed_stabilizing()
{
int16_t out_min = _rc_throttle.radio_min + _min_throttle;
int16_t motor_out; // main motor output
int16_t throttle_radio_output; // total throttle pwm value, summed onto throttle channel minimum, typically ~1100-1900
int16_t out_min = _throttle_radio_min + _min_throttle;
// initialize limits flags
limit.roll_pitch = false;
@ -202,37 +199,34 @@ void AP_MotorsSingle::output_armed_stabilizing()
limit.throttle_upper = false;
// Throttle is 0 to 1000 only
if (_rc_throttle.servo_out <= _min_throttle) {
_rc_throttle.servo_out = _min_throttle;
if (_throttle_control_input <= _min_throttle) {
_throttle_control_input = _min_throttle;
limit.throttle_lower = true;
}
if (_rc_throttle.servo_out >= _max_throttle) {
_rc_throttle.servo_out = _max_throttle;
if (_throttle_control_input >= _max_throttle) {
_throttle_control_input = _max_throttle;
limit.throttle_upper = true;
}
// capture desired throttle from receiver
_rc_throttle.calc_pwm();
//motor
motor_out = _rc_throttle.radio_out;
// calculate throttle PWM
throttle_radio_output = calc_throttle_radio_output();
// adjust for thrust curve and voltage scaling
motor_out = apply_thrust_curve_and_volt_scaling(motor_out, out_min, _rc_throttle.radio_max);
throttle_radio_output = apply_thrust_curve_and_volt_scaling(throttle_radio_output, out_min, _throttle_radio_max);
// ensure motor doesn't drop below a minimum value and stop
motor_out = max(motor_out, out_min);
throttle_radio_output = max(throttle_radio_output, out_min);
// TODO: set limits.roll_pitch and limits.yaw
// front servo
_servo1.servo_out = _rev_roll*_rc_roll.servo_out + _rev_yaw*_rc_yaw.servo_out;
_servo1.servo_out = _rev_roll*_roll_control_input + _rev_yaw*_yaw_control_input;
// right servo
_servo2.servo_out = _rev_pitch*_rc_pitch.servo_out + _rev_yaw*_rc_yaw.servo_out;
_servo2.servo_out = _rev_pitch*_pitch_control_input + _rev_yaw*_yaw_control_input;
// rear servo
_servo3.servo_out = -_rev_roll*_rc_roll.servo_out + _rev_yaw*_rc_yaw.servo_out;
_servo3.servo_out = -_rev_roll*_roll_control_input + _rev_yaw*_yaw_control_input;
// left servo
_servo4.servo_out = -_rev_pitch*_rc_pitch.servo_out + _rev_yaw*_rc_yaw.servo_out;
_servo4.servo_out = -_rev_pitch*_pitch_control_input + _rev_yaw*_yaw_control_input;
_servo1.calc_pwm();
_servo2.calc_pwm();
@ -244,7 +238,7 @@ void AP_MotorsSingle::output_armed_stabilizing()
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), _servo2.radio_out);
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_3]), _servo3.radio_out);
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), _servo4.radio_out);
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_7]), motor_out);
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_7]), throttle_radio_output);
}
// output_disarmed - sends commands to the motors

4
libraries/AP_Motors/AP_MotorsSingle.h
View File

@ -25,8 +25,8 @@ class AP_MotorsSingle : public AP_Motors {
public:
/// Constructor
AP_MotorsSingle(RC_Channel& rc_roll, RC_Channel& rc_pitch, RC_Channel& rc_throttle, RC_Channel& rc_yaw, RC_Channel& servo1, RC_Channel& servo2, RC_Channel& servo3, RC_Channel& servo4, uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) :
AP_Motors(rc_roll, rc_pitch, rc_throttle, rc_yaw, loop_rate, speed_hz),
AP_MotorsSingle(RC_Channel& servo1, RC_Channel& servo2, RC_Channel& servo3, RC_Channel& servo4, uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) :
AP_Motors(loop_rate, speed_hz),
_servo1(servo1),
_servo2(servo2),
_servo3(servo3),

171
libraries/AP_Motors/AP_MotorsTri.cpp
View File

@ -25,6 +25,48 @@
extern const AP_HAL::HAL& hal;
const AP_Param::GroupInfo AP_MotorsTri::var_info[] PROGMEM = {
// variables from parent vehicle
AP_NESTEDGROUPINFO(AP_Motors, 0),
// @Param: YAW_SV_REV
// @DisplayName: Yaw Servo Reverse
// @Description: Yaw servo signal reversing
// @Range: 0, 1
// @User: Standard
AP_GROUPINFO("YAW_SV_REV", 1, AP_MotorsTri, _yaw_servo_reverse, 0),
// @Param: YAW_SV_TRIM
// @DisplayName: Yaw Servo Trim/Center
// @Description: Trim or center position of yaw servo
// @Range: 1250 1750
// @Units: PWM
// @Increment: 1
// @User: Standard
AP_GROUPINFO("YAW_SV_TRIM", 2, AP_MotorsTri, _yaw_servo_trim, 1500),
// @Param: YAW_SV_MIN
// @DisplayName: Yaw Servo Min Position
// @Description: Minimum angle limit of yaw servo
// @Range: 1000 1400
// @Units: PWM
// @Increment: 1
// @User: Standard
AP_GROUPINFO("YAW_SV_MIN", 3, AP_MotorsTri, _yaw_servo_min, 1250),
// @Param: YAW_SV_MAX
// @DisplayName: Yaw Servo Max Position
// @Description: Maximum angle limit of yaw servo
// @Range: 1600 2000
// @Units: PWM
// @Increment: 1
// @User: Standard
AP_GROUPINFO("YAW_SV_MAX", 4, AP_MotorsTri, _yaw_servo_max, 1750),
AP_GROUPEND
};
// init
void AP_MotorsTri::Init()
{
@ -74,17 +116,17 @@ void AP_MotorsTri::output_min()
limit.throttle_lower = true;
// send minimum value to each motor
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]), _rc_throttle.radio_min);
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), _rc_throttle.radio_min);
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), _rc_throttle.radio_min);
hal.rcout->write(AP_MOTORS_CH_TRI_YAW, _rc_yaw.radio_trim);
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]), _throttle_radio_min);
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), _throttle_radio_min);
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), _throttle_radio_min);
hal.rcout->write(AP_MOTORS_CH_TRI_YAW, _yaw_servo_trim);
}
// get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used)
// this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
uint16_t AP_MotorsTri::get_motor_mask()
{
// tri copter uses channels 1,2,4 and 7
// tri copter uses channels 1,2,3 and 4
return (1U << pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1])) |
(1U << pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2])) |
(1U << pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4])) |
@ -93,8 +135,9 @@ uint16_t AP_MotorsTri::get_motor_mask()
void AP_MotorsTri::output_armed_not_stabilizing()
{
int16_t out_min = _rc_throttle.radio_min + _min_throttle;
int16_t out_max = _rc_throttle.radio_max;
int16_t throttle_radio_output; // total throttle pwm value, summed onto throttle channel minimum, typically ~1100-1900
int16_t out_min = _throttle_radio_min + _min_throttle;
int16_t out_max = _throttle_radio_max;
int16_t motor_out[AP_MOTORS_MOT_4+1];
// initialize limits flags
@ -105,26 +148,23 @@ void AP_MotorsTri::output_armed_not_stabilizing()
int16_t min_thr = rel_pwm_to_thr_range(_spin_when_armed_ramped);
if (_rc_throttle.servo_out <= min_thr) {
_rc_throttle.servo_out = min_thr;
if (_throttle_control_input <= min_thr) {
_throttle_control_input = min_thr;
limit.throttle_lower = true;
}
if (_rc_throttle.servo_out >= _hover_out) {
_rc_throttle.servo_out = _hover_out;
if (_throttle_control_input >= _hover_out) {
_throttle_control_input = _hover_out;
limit.throttle_upper = true;
}
_rc_yaw.servo_out=0;
_rc_yaw.calc_pwm();
throttle_radio_output = calc_throttle_radio_output();
_rc_throttle.calc_pwm();
motor_out[AP_MOTORS_MOT_1] = throttle_radio_output;
motor_out[AP_MOTORS_MOT_2] = throttle_radio_output;
motor_out[AP_MOTORS_MOT_4] = throttle_radio_output;
motor_out[AP_MOTORS_MOT_1] = _rc_throttle.radio_out;
motor_out[AP_MOTORS_MOT_2] = _rc_throttle.radio_out;
motor_out[AP_MOTORS_MOT_4] = _rc_throttle.radio_out;
if(_rc_throttle.radio_out >= out_min) {
if(throttle_radio_output >= out_min) {
// adjust for thrust curve and voltage scaling
motor_out[AP_MOTORS_MOT_1] = apply_thrust_curve_and_volt_scaling(motor_out[AP_MOTORS_MOT_1], out_min, out_max);
motor_out[AP_MOTORS_MOT_2] = apply_thrust_curve_and_volt_scaling(motor_out[AP_MOTORS_MOT_2], out_min, out_max);
@ -136,19 +176,20 @@ void AP_MotorsTri::output_armed_not_stabilizing()
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), motor_out[AP_MOTORS_MOT_2]);
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), motor_out[AP_MOTORS_MOT_4]);
if( _rc_tail.get_reverse() == true ) {
hal.rcout->write(AP_MOTORS_CH_TRI_YAW, _rc_yaw.radio_trim - (_rc_yaw.radio_out - _rc_yaw.radio_trim));
}else{
hal.rcout->write(AP_MOTORS_CH_TRI_YAW, _rc_yaw.radio_out);
}
// send centering signal to yaw servo
hal.rcout->write(AP_MOTORS_CH_TRI_YAW, _yaw_servo_trim);
}
// sends commands to the motors
// TODO pull code that is common to output_armed_not_stabilizing into helper functions
void AP_MotorsTri::output_armed_stabilizing()
{
int16_t out_min = _rc_throttle.radio_min + _min_throttle;
int16_t out_max = _rc_throttle.radio_max;
int16_t roll_pwm; // roll pwm value, initially calculated by calc_roll_pwm() but may be modified after, +/- 400
int16_t pitch_pwm; // pitch pwm value, initially calculated by calc_roll_pwm() but may be modified after, +/- 400
int16_t throttle_radio_output; // total throttle pwm value, summed onto throttle channel minimum, typically ~1100-1900
int16_t yaw_radio_output; // final yaw pwm value sent to motors, typically ~1100-1900
int16_t out_min = _throttle_radio_min + _min_throttle;
int16_t out_max = _throttle_radio_max;
int16_t motor_out[AP_MOTORS_MOT_4+1];
// initialize limits flags
@ -158,30 +199,29 @@ void AP_MotorsTri::output_armed_stabilizing()
limit.throttle_upper = false;
// Throttle is 0 to 1000 only
if (_rc_throttle.servo_out <= 0) {
_rc_throttle.servo_out = 0;
if (_throttle_control_input <= 0) {
_throttle_control_input = 0;
limit.throttle_lower = true;
}
if (_rc_throttle.servo_out >= _max_throttle) {
_rc_throttle.servo_out = _max_throttle;
if (_throttle_control_input >= _max_throttle) {
_throttle_control_input = _max_throttle;
limit.throttle_upper = true;
}
// tricopters limit throttle to 80%
// To-Do: implement improved stability patch and remove this limit
if (_rc_throttle.servo_out > 800) {
_rc_throttle.servo_out = 800;
if (_throttle_control_input > 800) {
_throttle_control_input = 800;
limit.throttle_upper = true;
}
// capture desired roll, pitch, yaw and throttle from receiver
_rc_roll.calc_pwm();
_rc_pitch.calc_pwm();
_rc_throttle.calc_pwm();
_rc_yaw.calc_pwm();
roll_pwm = calc_roll_pwm();
pitch_pwm = calc_pitch_pwm();
throttle_radio_output = calc_throttle_radio_output();
yaw_radio_output = calc_yaw_radio_output();
// if we are not sending a throttle output, we cut the motors
if(_rc_throttle.servo_out == 0) {
if( is_zero(_throttle_control_input) ) {
// range check spin_when_armed
if (_spin_when_armed_ramped < 0) {
_spin_when_armed_ramped = 0;
@ -189,29 +229,29 @@ void AP_MotorsTri::output_armed_stabilizing()
if (_spin_when_armed_ramped > _min_throttle) {
_spin_when_armed_ramped = _min_throttle;
}
motor_out[AP_MOTORS_MOT_1] = _rc_throttle.radio_min + _spin_when_armed_ramped;
motor_out[AP_MOTORS_MOT_2] = _rc_throttle.radio_min + _spin_when_armed_ramped;
motor_out[AP_MOTORS_MOT_4] = _rc_throttle.radio_min + _spin_when_armed_ramped;
motor_out[AP_MOTORS_MOT_1] = _throttle_radio_min + _spin_when_armed_ramped;
motor_out[AP_MOTORS_MOT_2] = _throttle_radio_min + _spin_when_armed_ramped;
motor_out[AP_MOTORS_MOT_4] = _throttle_radio_min + _spin_when_armed_ramped;
}else{
int16_t roll_out = (float)_rc_roll.pwm_out * 0.866f;
int16_t pitch_out = _rc_pitch.pwm_out / 2;
int16_t roll_out = (float)(roll_pwm * 0.866f);
int16_t pitch_out = pitch_pwm / 2;
// check if throttle is below limit
if (_rc_throttle.servo_out <= _min_throttle) {
if (_throttle_control_input <= _min_throttle) {
limit.throttle_lower = true;
_rc_throttle.servo_out = _min_throttle;
_rc_throttle.calc_pwm(); // recalculate radio.out
_throttle_control_input = _min_throttle;
throttle_radio_output = calc_throttle_radio_output();
}
// TODO: set limits.roll_pitch and limits.yaw
//left front
motor_out[AP_MOTORS_MOT_2] = _rc_throttle.radio_out + roll_out + pitch_out;
motor_out[AP_MOTORS_MOT_2] = throttle_radio_output + roll_out + pitch_out;
//right front
motor_out[AP_MOTORS_MOT_1] = _rc_throttle.radio_out - roll_out + pitch_out;
motor_out[AP_MOTORS_MOT_1] = throttle_radio_output - roll_out + pitch_out;
// rear
motor_out[AP_MOTORS_MOT_4] = _rc_throttle.radio_out - _rc_pitch.pwm_out;
motor_out[AP_MOTORS_MOT_4] = throttle_radio_output - pitch_pwm;
// Tridge's stability patch
if(motor_out[AP_MOTORS_MOT_1] > out_max) {
@ -248,15 +288,8 @@ void AP_MotorsTri::output_armed_stabilizing()
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), motor_out[AP_MOTORS_MOT_2]);
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), motor_out[AP_MOTORS_MOT_4]);
// also send out to tail command (we rely on any auto pilot to have updated the rc_yaw->radio_out to the correct value)
// note we do not save the radio_out to the motor_out array so it may not appear in the ch7out in the status screen of the mission planner
// note: we use _rc_tail's (aka channel 7's) REV parameter to control whether the servo is reversed or not but this is a bit nonsensical.
// a separate servo object (including min, max settings etc) would be better or at least a separate parameter to specify the direction of the tail servo
if( _rc_tail.get_reverse() == true ) {
hal.rcout->write(AP_MOTORS_CH_TRI_YAW, _rc_yaw.radio_trim - (_rc_yaw.radio_out - _rc_yaw.radio_trim));
}else{
hal.rcout->write(AP_MOTORS_CH_TRI_YAW, _rc_yaw.radio_out);
}
// send out to yaw command to tail servo
hal.rcout->write(AP_MOTORS_CH_TRI_YAW, yaw_radio_output);
}
// output_disarmed - sends commands to the motors
@ -299,3 +332,25 @@ void AP_MotorsTri::output_test(uint8_t motor_seq, int16_t pwm)
break;
}
}
// calc_yaw_radio_output - calculate final radio output for yaw channel
int16_t AP_MotorsTri::calc_yaw_radio_output()
{
int16_t ret;
if (_yaw_servo_reverse){
if (_yaw_control_input >= 0){
ret = (_yaw_servo_trim - (_yaw_control_input/4500 * (_yaw_servo_trim - _yaw_servo_min)));
} else {
ret = (_yaw_servo_trim - (_yaw_control_input/4500 * (_yaw_servo_max - _yaw_servo_trim)));
}
} else {
if (_yaw_control_input >= 0){
ret = ((_yaw_control_input/4500 * (_yaw_servo_max - _yaw_servo_trim)) + _yaw_servo_trim);
} else {
ret = ((_yaw_control_input/4500 * (_yaw_servo_trim - _yaw_servo_min)) + _yaw_servo_trim);
}
}
return ret;
}

21
libraries/AP_Motors/AP_MotorsTri.h
View File

@ -12,16 +12,17 @@
#include "AP_Motors.h"
// tail servo uses channel 7
#define AP_MOTORS_CH_TRI_YAW CH_7
#define AP_MOTORS_CH_TRI_YAW CH_3
/// @class AP_MotorsTri
class AP_MotorsTri : public AP_Motors {
public:
/// Constructor
AP_MotorsTri(RC_Channel& rc_roll, RC_Channel& rc_pitch, RC_Channel& rc_throttle, RC_Channel& rc_yaw, RC_Channel& rc_tail, uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) :
AP_Motors(rc_roll, rc_pitch, rc_throttle, rc_yaw, loop_rate, speed_hz),
_rc_tail(rc_tail) {
AP_MotorsTri(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) :
AP_Motors(loop_rate, speed_hz)
{
AP_Param::setup_object_defaults(this, var_info);
};
// init
@ -45,13 +46,23 @@ public:
// this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
virtual uint16_t get_motor_mask();
// var_info for holding Parameter information
static const struct AP_Param::GroupInfo var_info[];
protected:
// output - sends commands to the motors
void output_armed_stabilizing();
void output_armed_not_stabilizing();
void output_disarmed();
RC_Channel& _rc_tail; // REV parameter used from this channel to determine direction of tail servo movement
// calc_yaw_radio_output - calculate final radio output for yaw channel
int16_t calc_yaw_radio_output(); // calculate radio output for yaw servo, typically in range of 1100-1900
// parameters
AP_Int8 _yaw_servo_reverse; // Yaw servo signal reversing
AP_Int16 _yaw_servo_trim; // Trim or center position of yaw servo
AP_Int16 _yaw_servo_min; // Minimum angle limit of yaw servo
AP_Int16 _yaw_servo_max; // Maximum angle limit of yaw servo
};
#endif // AP_MOTORSTRI

5
libraries/AP_Motors/AP_MotorsY6.h
View File

@ -18,8 +18,9 @@ class AP_MotorsY6 : public AP_MotorsMatrix {
public:
/// Constructor
AP_MotorsY6(RC_Channel& rc_roll, RC_Channel& rc_pitch, RC_Channel& rc_throttle, RC_Channel& rc_yaw, uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMatrix(rc_roll, rc_pitch, rc_throttle, rc_yaw, loop_rate, speed_hz) {
};
AP_MotorsY6(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) :
AP_MotorsMatrix(loop_rate, speed_hz)
{ };
// setup_motors - configures the motors for a Y6
virtual void setup_motors();

36
libraries/AP_Motors/AP_Motors_Class.cpp
View File

@ -104,11 +104,7 @@ const AP_Param::GroupInfo AP_Motors::var_info[] PROGMEM = {
};
// Constructor
AP_Motors::AP_Motors(RC_Channel& rc_roll, RC_Channel& rc_pitch, RC_Channel& rc_throttle, RC_Channel& rc_yaw, uint16_t loop_rate, uint16_t speed_hz) :
_rc_roll(rc_roll),
_rc_pitch(rc_pitch),
_rc_throttle(rc_throttle),
_rc_yaw(rc_yaw),
AP_Motors::AP_Motors(uint16_t loop_rate, uint16_t speed_hz) :
_loop_rate(loop_rate),
_speed_hz(speed_hz),
_min_throttle(AP_MOTORS_DEFAULT_MIN_THROTTLE),
@ -152,16 +148,20 @@ void AP_Motors::armed(bool arm)
AP_Notify::flags.armed = arm;
};
// set_min_throttle - sets the minimum throttle that will be sent to the engines when they're not off (i.e. to prevents issues with some motors spinning and some not at very low throttle)
void AP_Motors::set_min_throttle(uint16_t min_throttle)
// set_throttle_range - sets the minimum throttle that will be sent to the engines when they're not off (i.e. to prevents issues with some motors spinning and some not at very low throttle)
// also sets throttle channel minimum and maximum pwm
void AP_Motors::set_throttle_range(uint16_t min_throttle, int16_t radio_min, int16_t radio_max)
{
_min_throttle = (float)min_throttle * (_rc_throttle.radio_max - _rc_throttle.radio_min) / 1000.0f;
_throttle_radio_min = radio_min;
_throttle_radio_max = radio_max;
_throttle_pwm_scalar = (_throttle_radio_max - _throttle_radio_min) / 1000.0f;
_min_throttle = (float)min_throttle * _throttle_pwm_scalar;
}
// get_hover_throttle_as_pwm - converts hover throttle to pwm (i.e. range 1000 ~ 2000)
int16_t AP_Motors::get_hover_throttle_as_pwm() const
{
return (_rc_throttle.radio_min + (float)(_rc_throttle.radio_max - _rc_throttle.radio_min) * _hover_out / 1000.0f);
return (_throttle_radio_min + (float)(_throttle_radio_max - _throttle_radio_min) * _hover_out / 1000.0f);
}
// throttle_pass_through - passes provided pwm directly to all motors - dangerous but used for initialising ESCs
@ -217,7 +217,7 @@ void AP_Motors::slow_start(bool true_false)
_flags.slow_start = true;
// initialise maximum throttle to current throttle
_max_throttle = constrain_int16(_rc_throttle.servo_out, 0, AP_MOTORS_DEFAULT_MAX_THROTTLE);
_max_throttle = constrain_int16(_throttle_control_input, 0, AP_MOTORS_DEFAULT_MAX_THROTTLE);
}
// update the throttle input filter
@ -229,8 +229,8 @@ void AP_Motors::update_throttle_filter()
_throttle_filter.reset(0.0f);
}
// prevent _rc_throttle.servo_out from wrapping at int16 max or min
_rc_throttle.servo_out = constrain_float(_throttle_filter.get(),-32000,32000);
// prevent _throttle_control_input from wrapping at int16 max or min
_throttle_control_input = constrain_float(_throttle_filter.get(),-32000,32000);
}
// update_max_throttle - updates the limits on _max_throttle if necessary taking into account slow_start_throttle flag
@ -251,7 +251,7 @@ void AP_Motors::update_max_throttle()
_max_throttle += AP_MOTOR_SLOW_START_INCREMENT;
// turn off slow start if we've reached max throttle
if (_max_throttle >= _rc_throttle.servo_out) {
if (_max_throttle >= _throttle_control_input) {
_max_throttle = AP_MOTORS_DEFAULT_MAX_THROTTLE;
_flags.slow_start = false;
}
@ -273,7 +273,7 @@ void AP_Motors::current_limit_max_throttle()
}
// remove throttle limit if throttle is at zero or disarmed
if(_rc_throttle.servo_out <= 0 || !_flags.armed) {
if(_throttle_control_input <= 0 || !_flags.armed) {
_throttle_limit = 1.0f;
}
@ -342,14 +342,14 @@ void AP_Motors::update_lift_max_from_batt_voltage()
void AP_Motors::update_battery_resistance()
{
// if motors are stopped, reset resting voltage and current
if (_rc_throttle.servo_out <= 0 || !_flags.armed) {
if (_throttle_control_input <= 0 || !_flags.armed) {
_batt_voltage_resting = _batt_voltage;
_batt_current_resting = _batt_current;
_batt_timer = 0;
} else {
// update battery resistance when throttle is over hover throttle
if ((_batt_timer < 400) && ((_batt_current_resting*2.0f) < _batt_current)) {
if (_rc_throttle.servo_out >= _hover_out) {
if (_throttle_control_input >= _hover_out) {
// filter reaches 90% in 1/4 the test time
_batt_resistance += 0.05f*(( (_batt_voltage_resting-_batt_voltage)/(_batt_current-_batt_current_resting) ) - _batt_resistance);
_batt_timer += 1;
@ -393,10 +393,10 @@ float AP_Motors::get_compensation_gain() const
float AP_Motors::rel_pwm_to_thr_range(float pwm) const
{
return 1000.0f*pwm/(_rc_throttle.radio_max-_rc_throttle.radio_min);
return pwm/_throttle_pwm_scalar;
}
float AP_Motors::thr_range_to_rel_pwm(float thr) const
{
return (_rc_throttle.radio_max-_rc_throttle.radio_min)*thr/1000.0f;
return _throttle_pwm_scalar*thr;
}

59
libraries/AP_Motors/AP_Motors_Class.h
View File

@ -89,7 +89,7 @@ class AP_Motors {
public:
// Constructor
AP_Motors(RC_Channel& rc_roll, RC_Channel& rc_pitch, RC_Channel& rc_throttle, RC_Channel& rc_yaw, uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT);
AP_Motors(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT);
// init
virtual void Init() {}
@ -113,8 +113,9 @@ public:
// get motor interlock status. true means motors run, false motors don't run
bool get_interlock() const { return _flags.interlock; };
// set_min_throttle - sets the minimum throttle that will be sent to the engines when they're not off (i.e. to prevents issues with some motors spinning and some not at very low throttle)
void set_min_throttle(uint16_t min_throttle);
// set_throttle_range - sets the minimum throttle that will be sent to the engines when they're not off (i.e. to prevents issues with some motors spinning and some not at very low throttle)
// also sets throttle channel minimum and maximum pwm
void set_throttle_range(uint16_t min_throttle, int16_t radio_min, int16_t radio_max);
// set_hover_throttle - sets the mid throttle which is close to the hover throttle of the copter
// this is used to limit the amount that the stability patch will increase the throttle to give more room for roll, pitch and yaw control
@ -127,17 +128,17 @@ public:
int16_t throttle_max() const { return _max_throttle;}
// set_roll, set_pitch, set_yaw, set_throttle
void set_roll(int16_t roll_in) { _rc_roll.servo_out = roll_in; }; // range -4500 ~ 4500
void set_pitch(int16_t pitch_in) { _rc_pitch.servo_out = pitch_in; }; // range -4500 ~ 4500
void set_yaw(int16_t yaw_in) { _rc_yaw.servo_out = yaw_in; }; // range -4500 ~ 4500
void set_throttle(float throttle_in) { _throttle_in = throttle_in; }; // range 0 ~ 1000
void set_roll(int16_t roll_in) { _roll_control_input = roll_in; }; // range -4500 ~ 4500
void set_pitch(int16_t pitch_in) { _pitch_control_input = pitch_in; }; // range -4500 ~ 4500
void set_yaw(int16_t yaw_in) { _yaw_control_input = yaw_in; }; // range -4500 ~ 4500
void set_throttle(float throttle_in) { _throttle_in = throttle_in; }; // range 0 ~ 1000
void set_stabilizing(bool stabilizing) { _flags.stabilizing = stabilizing; }
// accessors for roll, pitch, yaw and throttle inputs to motors
int16_t get_roll() const { return _rc_roll.servo_out; }
int16_t get_pitch() const { return _rc_pitch.servo_out; }
int16_t get_yaw() const { return _rc_yaw.servo_out; }
int16_t get_throttle_out() const { return _rc_throttle.servo_out; }
float get_roll() const { return _roll_control_input; }
float get_pitch() const { return _pitch_control_input; }
float get_yaw() const { return _yaw_control_input; }
float get_throttle() const { return _throttle_control_input; }
void set_throttle_filter_cutoff(float filt_hz) { _throttle_filter.set_cutoff_frequency(filt_hz); }
@ -249,6 +250,15 @@ protected:
float rel_pwm_to_thr_range(float pwm) const;
float thr_range_to_rel_pwm(float thr) const;
// convert RPY and Throttle servo ranges from legacy controller scheme back into PWM values
// RPY channels typically +/-45 degrees servo travel between +/-400 PWM
// Throttle channel typically 0-1000 range converts to 1100-1900 PWM for final output signal to motors
// ToDo: this should all be handled as floats +/- 1.0 instead of PWM and fake angle ranges
int16_t calc_roll_pwm() { return (_roll_control_input / 11.25);}
int16_t calc_pitch_pwm() { return (_pitch_control_input / 11.25);}
int16_t calc_yaw_pwm() { return (_yaw_control_input / 11.25);}
int16_t calc_throttle_radio_output() { return (_throttle_control_input * _throttle_pwm_scalar) + _throttle_radio_min;}
// flag bitmask
struct AP_Motors_flags {
uint8_t armed : 1; // 0 if disarmed, 1 if armed
@ -274,18 +284,21 @@ protected:
AP_Float _thr_mix_min; // current over which maximum throttle is limited
// internal variables
RC_Channel& _rc_roll; // roll input in from users is held in servo_out
RC_Channel& _rc_pitch; // pitch input in from users is held in servo_out
RC_Channel& _rc_throttle; // throttle input in from users is held in servo_out
RC_Channel& _rc_yaw; // yaw input in from users is held in servo_out
uint16_t _loop_rate; // rate at which output() function is called (normally 400hz)
uint16_t _speed_hz; // speed in hz to send updates to motors
int16_t _min_throttle; // the minimum throttle to be sent to the motors when they're on (prevents motors stalling while flying)
int16_t _max_throttle; // the maximum throttle to be sent to the motors (sometimes limited by slow start)
int16_t _hover_out; // the estimated hover throttle as pct * 10 (i.e. 0 ~ 1000)
int16_t _spin_when_armed_ramped;// equal to _spin_when_armed parameter but slowly ramped up from zero
float _throttle_thr_mix; // mix between throttle and hover throttle for 0 to 1 and ratio above hover throttle for >1
float _throttle_thr_mix_desired; // desired throttle_low_comp value, actual throttle_low_comp is slewed towards this value over 1~2 seconds
float _roll_control_input; // desired roll control from attitude controllers, +/- 4500
float _pitch_control_input; // desired pitch control from attitude controller, +/- 4500
float _throttle_control_input; // desired throttle (thrust) control from attitude controller, 0-1000
float _yaw_control_input; // desired yaw control from attitude controller, +/- 4500
float _throttle_pwm_scalar; // scalar used to convert throttle channel pwm range into 0-1000 range, ~0.8 - 1.0
uint16_t _loop_rate; // rate at which output() function is called (normally 400hz)
uint16_t _speed_hz; // speed in hz to send updates to motors
int16_t _min_throttle; // the minimum throttle to be sent to the motors when they're on (prevents motors stalling while flying)
int16_t _max_throttle; // the maximum throttle to be sent to the motors (sometimes limited by slow start)
int16_t _throttle_radio_min; // minimum radio channel pwm
int16_t _throttle_radio_max; // maximum radio channel pwm
int16_t _hover_out; // the estimated hover throttle as pct * 10 (i.e. 0 ~ 1000)
int16_t _spin_when_armed_ramped; // equal to _spin_when_armed parameter but slowly ramped up from zero
float _throttle_thr_mix; // mix between throttle and hover throttle for 0 to 1 and ratio above hover throttle for >1
float _throttle_thr_mix_desired; // desired throttle_low_comp value, actual throttle_low_comp is slewed towards this value over 1~2 seconds
// battery voltage compensation variables
float _batt_voltage; // latest battery voltage reading