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

AP_Motors: introduce set_stabilize(bool), specifies whether torque demands should be output

This commit is contained in:
Jonathan Challinger 2015-04-02 13:54:15 -07:00 committed by Randy Mackay
parent bc2afb31dd
commit 8e442675a7
12 changed files with 437 additions and 257 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

111
libraries/AP_Motors/AP_MotorsCoax.cpp
View File

@ -123,15 +123,63 @@ void AP_MotorsCoax::output_min()
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), _rc_throttle.radio_min);
}
// output_armed - sends commands to the motors
void AP_MotorsCoax::output_armed()
void AP_MotorsCoax::output_armed_not_stabilizing()
{
int16_t out_min = _rc_throttle.radio_min + _min_throttle;
int16_t motor_out;
int16_t min_thr = rel_pwm_to_thr_range(_spin_when_armed_ramped);
// initialize limits flags
limit.roll_pitch = true;
limit.yaw = true;
limit.throttle_lower = false;
limit.throttle_upper = false;
if (_rc_throttle.servo_out <= min_thr) {
_rc_throttle.servo_out = min_thr;
limit.throttle_lower = true;
}
if (_rc_throttle.servo_out >= _max_throttle) {
_rc_throttle.servo_out = _max_throttle;
limit.throttle_upper = true;
}
_rc_throttle.calc_pwm();
motor_out = _rc_throttle.radio_out;
_servo1.servo_out = 0;
_servo1.calc_pwm();
_servo2.servo_out = 0;
_servo2.calc_pwm();
if (motor_out >= out_min) {
motor_out = apply_thrust_curve_and_volt_scaling(motor_out, out_min, _rc_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]), motor_out);
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), motor_out);
}
// sends commands to the motors
// 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 motor_out[4];
// Throttle is 0 to 1000 only
if (_rc_throttle.servo_out <= 0) {
_rc_throttle.servo_out = 0;
// initialize limits flags
limit.roll_pitch = false;
limit.yaw = false;
limit.throttle_lower = false;
limit.throttle_upper = false;
if (_rc_throttle.servo_out <= _min_throttle) {
_rc_throttle.servo_out = _min_throttle;
limit.throttle_lower = true;
}
if (_rc_throttle.servo_out >= _max_throttle) {
@ -143,44 +191,27 @@ void AP_MotorsCoax::output_armed()
_rc_throttle.calc_pwm();
_rc_yaw.calc_pwm();
// if we are not sending a throttle output, we cut the motors
if(_rc_throttle.servo_out == 0) {
// range check spin_when_armed
if (_spin_when_armed < 0) {
_spin_when_armed = 0;
}
if (_spin_when_armed > _min_throttle) {
_spin_when_armed = _min_throttle;
}
motor_out[AP_MOTORS_MOT_3] = _rc_throttle.radio_min + _spin_when_armed;
motor_out[AP_MOTORS_MOT_4] = _rc_throttle.radio_min + _spin_when_armed;
}else{
// 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;
// check if throttle is below limit
if (_rc_throttle.servo_out <= _min_throttle) { // perhaps being at min throttle itself is not a problem, only being under is
limit.throttle_lower = true;
_rc_throttle.servo_out = _min_throttle;
_rc_throttle.calc_pwm(); // recalculate radio.out
}
// TODO: set limits.roll_pitch and limits.yaw
// 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;
// front
_servo1.servo_out = _rev_roll*_rc_roll.servo_out;
// right
_servo2.servo_out = _rev_pitch*_rc_pitch.servo_out;
_servo1.calc_pwm();
_servo2.calc_pwm();
// front
_servo1.servo_out = _rev_roll*_rc_roll.servo_out;
// right
_servo2.servo_out = _rev_pitch*_rc_pitch.servo_out;
// 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);
_servo1.calc_pwm();
_servo2.calc_pwm();
// 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);
motor_out[AP_MOTORS_MOT_4] = max(motor_out[AP_MOTORS_MOT_4], out_min);
}
// 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);
// 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);
motor_out[AP_MOTORS_MOT_4] = max(motor_out[AP_MOTORS_MOT_4], out_min);
// send output to each motor
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]), _servo1.radio_out);
@ -202,7 +233,7 @@ void AP_MotorsCoax::output_disarmed()
void AP_MotorsCoax::output_test(uint8_t motor_seq, int16_t pwm)
{
// exit immediately if not armed
if (!_flags.armed) {
if (!armed()) {
return;
}

5
libraries/AP_Motors/AP_MotorsCoax.h
View File

@ -60,8 +60,9 @@ public:
protected:
// output - sends commands to the motors
virtual void output_armed();
virtual void output_disarmed();
void output_armed_stabilizing();
void output_armed_not_stabilizing();
void output_disarmed();
AP_Int8 _rev_roll; // REV Roll feedback
AP_Int8 _rev_pitch; // REV pitch feedback

14
libraries/AP_Motors/AP_MotorsHeli.cpp
View File

@ -280,7 +280,7 @@ void AP_MotorsHeli::output_min()
void AP_MotorsHeli::output_test(uint8_t motor_seq, int16_t pwm)
{
// exit immediately if not armed
if (!_flags.armed) {
if (!armed()) {
return;
}
@ -375,8 +375,14 @@ uint16_t AP_MotorsHeli::get_motor_mask()
// protected methods
//
// output_armed - sends commands to the motors
void AP_MotorsHeli::output_armed()
void AP_MotorsHeli::output_armed_not_stabilizing()
{
// call output_armed_stabilizing
output_armed_stabilizing();
}
// sends commands to the motors
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) {
@ -401,7 +407,7 @@ void AP_MotorsHeli::output_armed()
void AP_MotorsHeli::output_disarmed()
{
// for helis - armed or disarmed we allow servos to move
output_armed();
output_armed_stabilizing();
}
//

5
libraries/AP_Motors/AP_MotorsHeli.h
View File

@ -208,8 +208,9 @@ public:
protected:
// output - sends commands to the motors
void output_armed();
void output_disarmed();
void output_armed_stabilizing();
void output_armed_not_stabilizing();
void output_disarmed();
private:

338
libraries/AP_Motors/AP_MotorsMatrix.cpp
View File

@ -118,9 +118,61 @@ uint16_t AP_MotorsMatrix::get_motor_mask()
return 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
// initialize limits flags
limit.roll_pitch = true;
limit.yaw = true;
limit.throttle_lower = false;
limit.throttle_upper = false;
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;
limit.throttle_lower = true;
}
if (_rc_throttle.servo_out >= _hover_out) {
_rc_throttle.servo_out = _hover_out;
limit.throttle_upper = true;
}
_rc_throttle.calc_pwm();
// set output throttle
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
motor_out[i] = _rc_throttle.radio_out;
}
}
if(_rc_throttle.radio_out >= out_min_pwm) {
// apply thrust curve and voltage scaling
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
motor_out[i] = apply_thrust_curve_and_volt_scaling(motor_out[i], out_min_pwm, out_max_pwm);
}
}
}
// send output 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]), motor_out[i]);
}
}
}
// output_armed - sends commands to the motors
// includes new scaling stability patch
void AP_MotorsMatrix::output_armed()
// TODO pull code that is common to output_armed_not_stabilizing into helper functions
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
@ -137,7 +189,7 @@ void AP_MotorsMatrix::output_armed()
int16_t yaw_allowed; // amount of yaw we can fit in
int16_t thr_adj; // the difference between the pilot's desired throttle and out_best_thr_pwm (the throttle that is actually provided)
// initialize limits flag
// initialize limits flags
limit.roll_pitch = false;
limit.yaw = false;
limit.throttle_lower = false;
@ -145,8 +197,8 @@ void AP_MotorsMatrix::output_armed()
// 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 <= 0) {
_rc_throttle.servo_out = 0;
if (_rc_throttle.servo_out <= _min_throttle) {
_rc_throttle.servo_out = _min_throttle;
limit.throttle_lower = true;
}
if (_rc_throttle.servo_out >= _max_throttle) {
@ -160,169 +212,139 @@ void AP_MotorsMatrix::output_armed()
_rc_throttle.calc_pwm();
_rc_yaw.calc_pwm();
// if we are not sending a throttle output, we cut the motors
if (_rc_throttle.servo_out == 0) {
// range check spin_when_armed
if (_spin_when_armed_ramped < 0) {
_spin_when_armed_ramped = 0;
}
if (_spin_when_armed_ramped > _min_throttle) {
_spin_when_armed_ramped = _min_throttle;
}
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
// spin motors at minimum
if (motor_enabled[i]) {
motor_out[i] = _rc_throttle.radio_min + _spin_when_armed_ramped;
// 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_voltage_comp_gain() +
_rc_pitch.pwm_out * _pitch_factor[i] * get_voltage_comp_gain();
// record lowest roll pitch command
if (rpy_out[i] < rpy_low) {
rpy_low = rpy_out[i];
}
// record highest roll pich command
if (rpy_out[i] > rpy_high) {
rpy_high = rpy_out[i];
}
}
}
// Every thing is limited
// calculate throttle that gives most possible room for yaw (range 1000 ~ 2000) which is the lower of:
// 1. mid throttle - average of highest and lowest motor (this would give the maximum possible room margin above the highest motor and below the lowest)
// 2. the higher of:
// a) the pilot's throttle input
// b) the mid point between the pilot's input throttle and hover-throttle
// Situation #2 ensure we never increase the throttle above hover throttle unless the pilot has commanded this.
// Situation #2b allows us to raise the throttle above what the pilot commanded but not so far that it would actually cause the copter to rise.
// 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_low_comp)+get_hover_throttle_as_pwm()*_throttle_low_comp));
// 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 yawing right
if (yaw_allowed > _rc_yaw.pwm_out * get_voltage_comp_gain()) {
yaw_allowed = _rc_yaw.pwm_out * get_voltage_comp_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_voltage_comp_gain()) {
yaw_allowed = _rc_yaw.pwm_out * get_voltage_comp_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;
}
}
// add yaw to intermediate numbers for each motor
rpy_low = 0;
rpy_high = 0;
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
rpy_out[i] = rpy_out[i] +
yaw_allowed * _yaw_factor[i];
// record lowest roll+pitch+yaw command
if( rpy_out[i] < rpy_low ) {
rpy_low = rpy_out[i];
}
// record highest roll+pitch+yaw command
if( rpy_out[i] > rpy_high) {
rpy_high = rpy_out[i];
}
}
}
// check everything fits
thr_adj = _rc_throttle.radio_out - 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);
// if we are increasing the throttle (situation #2 above)..
if (thr_adj > 0) {
// increase throttle as close as possible to requested throttle
// without going over out_max_pwm
if (thr_adj > thr_adj_max){
thr_adj = thr_adj_max;
// we haven't even been able to apply full throttle command
limit.throttle_upper = true;
}
}else if(thr_adj < 0){
// decrease throttle as close as possible to requested throttle
// without going under out_min_pwm or over out_max_pwm
// earlier code ensures we can't break both boundaries
int16_t thr_adj_min = min(out_min_pwm-(out_best_thr_pwm+rpy_low),0);
if (thr_adj > thr_adj_max) {
thr_adj = thr_adj_max;
limit.throttle_upper = true;
}
if (thr_adj < thr_adj_min) {
thr_adj = thr_adj_min;
}
}
// do we need to reduce roll, pitch, yaw command
// earlier code does not allow both limit's to be passed simultaneously with abs(_yaw_factor)<1
if ((rpy_low+out_best_thr_pwm)+thr_adj < out_min_pwm){
rpy_scale = (float)(out_min_pwm-thr_adj-out_best_thr_pwm)/rpy_low;
// we haven't even been able to apply full roll, pitch and minimal yaw without scaling
limit.roll_pitch = true;
limit.yaw = true;
}else if((rpy_high+out_best_thr_pwm)+thr_adj > out_max_pwm){
rpy_scale = (float)(out_max_pwm-thr_adj-out_best_thr_pwm)/rpy_high;
// we haven't even been able to apply full roll, pitch and minimal yaw without scaling
limit.roll_pitch = true;
limit.yaw = true;
}
} else {
// check if throttle is below limit
if (_rc_throttle.servo_out <= _min_throttle) { // perhaps being at min throttle itself is not a problem, only being under is
limit.throttle_lower = true;
_rc_throttle.servo_out = _min_throttle;
_rc_throttle.calc_pwm(); // recalculate radio.out
// add scaled roll, pitch, constrained yaw and throttle for each motor
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
motor_out[i] = out_best_thr_pwm+thr_adj +
rpy_scale*rpy_out[i];
}
}
// 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_voltage_comp_gain() +
_rc_pitch.pwm_out * _pitch_factor[i] * get_voltage_comp_gain();
// record lowest roll pitch command
if (rpy_out[i] < rpy_low) {
rpy_low = rpy_out[i];
}
// record highest roll pich command
if (rpy_out[i] > rpy_high) {
rpy_high = rpy_out[i];
}
}
// apply thrust curve and voltage scaling
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
motor_out[i] = apply_thrust_curve_and_volt_scaling(motor_out[i], out_min_pwm, out_max_pwm);
}
}
// calculate throttle that gives most possible room for yaw (range 1000 ~ 2000) which is the lower of:
// 1. mid throttle - average of highest and lowest motor (this would give the maximum possible room margin above the highest motor and below the lowest)
// 2. the higher of:
// a) the pilot's throttle input
// b) the mid point between the pilot's input throttle and hover-throttle
// Situation #2 ensure we never increase the throttle above hover throttle unless the pilot has commanded this.
// Situation #2b allows us to raise the throttle above what the pilot commanded but not so far that it would actually cause the copter to rise.
// 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_low_comp)+get_hover_throttle_as_pwm()*_throttle_low_comp));
// 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 yawing right
if (yaw_allowed > _rc_yaw.pwm_out * get_voltage_comp_gain()) {
yaw_allowed = _rc_yaw.pwm_out * get_voltage_comp_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_voltage_comp_gain()) {
yaw_allowed = _rc_yaw.pwm_out * get_voltage_comp_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;
}
}
// add yaw to intermediate numbers for each motor
rpy_low = 0;
rpy_high = 0;
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
rpy_out[i] = rpy_out[i] +
yaw_allowed * _yaw_factor[i];
// record lowest roll+pitch+yaw command
if( rpy_out[i] < rpy_low ) {
rpy_low = rpy_out[i];
}
// record highest roll+pitch+yaw command
if( rpy_out[i] > rpy_high) {
rpy_high = rpy_out[i];
}
}
}
// check everything fits
thr_adj = _rc_throttle.radio_out - 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);
// if we are increasing the throttle (situation #2 above)..
if (thr_adj > 0) {
// increase throttle as close as possible to requested throttle
// without going over out_max_pwm
if (thr_adj > thr_adj_max){
thr_adj = thr_adj_max;
// we haven't even been able to apply full throttle command
limit.throttle_upper = true;
}
}else if(thr_adj < 0){
// decrease throttle as close as possible to requested throttle
// without going under out_min_pwm or over out_max_pwm
// earlier code ensures we can't break both boundaries
int16_t thr_adj_min = min(out_min_pwm-(out_best_thr_pwm+rpy_low),0);
if (thr_adj > thr_adj_max) {
thr_adj = thr_adj_max;
limit.throttle_upper = true;
}
if (thr_adj < thr_adj_min) {
thr_adj = thr_adj_min;
}
}
// do we need to reduce roll, pitch, yaw command
// earlier code does not allow both limit's to be passed simultaneously with abs(_yaw_factor)<1
if ((rpy_low+out_best_thr_pwm)+thr_adj < out_min_pwm){
rpy_scale = (float)(out_min_pwm-thr_adj-out_best_thr_pwm)/rpy_low;
// we haven't even been able to apply full roll, pitch and minimal yaw without scaling
limit.roll_pitch = true;
limit.yaw = true;
}else if((rpy_high+out_best_thr_pwm)+thr_adj > out_max_pwm){
rpy_scale = (float)(out_max_pwm-thr_adj-out_best_thr_pwm)/rpy_high;
// we haven't even been able to apply full roll, pitch and minimal yaw without scaling
limit.roll_pitch = true;
limit.yaw = true;
}
// add scaled roll, pitch, constrained yaw and throttle for each motor
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
motor_out[i] = out_best_thr_pwm+thr_adj +
rpy_scale*rpy_out[i];
}
}
// apply thrust curve and voltage scaling
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
motor_out[i] = apply_thrust_curve_and_volt_scaling(motor_out[i], out_min_pwm, out_max_pwm);
}
}
// clip motor output if required (shouldn't be)
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
motor_out[i] = constrain_int16(motor_out[i], out_min_pwm, out_max_pwm);
}
// clip motor output if required (shouldn't be)
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
motor_out[i] = constrain_int16(motor_out[i], out_min_pwm, out_max_pwm);
}
}
@ -347,7 +369,7 @@ void AP_MotorsMatrix::output_disarmed()
void AP_MotorsMatrix::output_test(uint8_t motor_seq, int16_t pwm)
{
// exit immediately if not armed
if (!_flags.armed) {
if (!armed()) {
return;
}

3
libraries/AP_Motors/AP_MotorsMatrix.h
View File

@ -67,7 +67,8 @@ public:
protected:
// output - sends commands to the motors
void output_armed();
void output_armed_stabilizing();
void output_armed_not_stabilizing();
void output_disarmed();
// add_motor using raw roll, pitch, throttle and yaw factors

101
libraries/AP_Motors/AP_MotorsSingle.cpp
View File

@ -137,15 +137,73 @@ uint16_t AP_MotorsSingle::get_motor_mask()
return (1U << 0 | 1U << 1 | 1U << 2 | 1U << 3 | 1U << 6);
}
// output_armed - sends commands to the motors
void AP_MotorsSingle::output_armed()
void AP_MotorsSingle::output_armed_not_stabilizing()
{
int16_t out_min = _rc_throttle.radio_min + _min_throttle;
int16_t motor_out;
int16_t min_thr = rel_pwm_to_thr_range(_spin_when_armed_ramped);
// initialize limits flags
limit.roll_pitch = true;
limit.yaw = true;
limit.throttle_lower = false;
limit.throttle_upper = false;
if (_rc_throttle.servo_out <= min_thr) {
_rc_throttle.servo_out = min_thr;
limit.throttle_lower = true;
}
if (_rc_throttle.servo_out >= _max_throttle) {
_rc_throttle.servo_out = _max_throttle;
limit.throttle_upper = true;
}
_rc_throttle.calc_pwm();
motor_out = _rc_throttle.radio_out;
// front servo
_servo1.servo_out = 0;
// right servo
_servo2.servo_out = 0;
// rear servo
_servo3.servo_out = 0;
// left servo
_servo4.servo_out = 0;
_servo1.calc_pwm();
_servo2.calc_pwm();
_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);
}
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);
}
// 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
// initialize limits flags
limit.roll_pitch = false;
limit.yaw = false;
limit.throttle_lower = false;
limit.throttle_upper = false;
// Throttle is 0 to 1000 only
if (_rc_throttle.servo_out <= 0) {
_rc_throttle.servo_out = 0;
if (_rc_throttle.servo_out <= _min_throttle) {
_rc_throttle.servo_out = _min_throttle;
limit.throttle_lower = true;
}
if (_rc_throttle.servo_out >= _max_throttle) {
@ -156,35 +214,16 @@ void AP_MotorsSingle::output_armed()
// capture desired throttle from receiver
_rc_throttle.calc_pwm();
// if we are not sending a throttle output, we cut the motors
if(_rc_throttle.servo_out == 0) {
// range check spin_when_armed
if (_spin_when_armed < 0) {
_spin_when_armed = 0;
}
if (_spin_when_armed > _min_throttle) {
_spin_when_armed = _min_throttle;
}
//motor
motor_out = _rc_throttle.radio_out;
// throttle is limited
motor_out = _rc_throttle.radio_min + _spin_when_armed;
}else{
// check if throttle is at or below limit
if (_rc_throttle.servo_out <= _min_throttle) {
limit.throttle_lower = true;
_rc_throttle.servo_out = _min_throttle;
_rc_throttle.calc_pwm(); // recalculate radio.out
}
// adjust for thrust curve and voltage scaling
motor_out = apply_thrust_curve_and_volt_scaling(motor_out, out_min, _rc_throttle.radio_max);
//motor
motor_out = _rc_throttle.radio_out;
// ensure motor doesn't drop below a minimum value and stop
motor_out = max(motor_out, out_min);
// adjust for thrust curve and voltage scaling
motor_out = apply_thrust_curve_and_volt_scaling(motor_out, out_min, _rc_throttle.radio_max);
// ensure motor doesn't drop below a minimum value and stop
motor_out = max(motor_out, 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;
@ -221,7 +260,7 @@ void AP_MotorsSingle::output_disarmed()
void AP_MotorsSingle::output_test(uint8_t motor_seq, int16_t pwm)
{
// exit immediately if not armed
if (!_flags.armed) {
if (!armed()) {
return;
}

5
libraries/AP_Motors/AP_MotorsSingle.h
View File

@ -61,8 +61,9 @@ public:
protected:
// output - sends commands to the motors
virtual void output_armed();
virtual void output_disarmed();
void output_armed_stabilizing();
void output_armed_not_stabilizing();
void output_disarmed();
AP_Int8 _rev_roll; // REV Roll feedback
AP_Int8 _rev_pitch; // REV pitch feedback

67
libraries/AP_Motors/AP_MotorsTri.cpp
View File

@ -91,15 +91,71 @@ uint16_t AP_MotorsTri::get_motor_mask()
(1U << AP_MOTORS_CH_TRI_YAW);
}
// output_armed - sends commands to the motors
void AP_MotorsTri::output_armed()
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 motor_out[AP_MOTORS_MOT_4+1];
// initialize lower limit flag
// initialize limits flags
limit.roll_pitch = true;
limit.yaw = true;
limit.throttle_lower = false;
limit.throttle_upper = false;
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;
limit.throttle_lower = true;
}
if (_rc_throttle.servo_out >= _hover_out) {
_rc_throttle.servo_out = _hover_out;
limit.throttle_upper = true;
}
_rc_yaw.servo_out=0;
_rc_yaw.calc_pwm();
_rc_throttle.calc_pwm();
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) {
// 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);
motor_out[AP_MOTORS_MOT_4] = apply_thrust_curve_and_volt_scaling(motor_out[AP_MOTORS_MOT_4], out_min, out_max);
}
// send output to each motor
hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]), motor_out[AP_MOTORS_MOT_1]);
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);
}
}
// 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 motor_out[AP_MOTORS_MOT_4+1];
// initialize limits flags
limit.roll_pitch = false;
limit.yaw = false;
limit.throttle_lower = false;
limit.throttle_upper = false;
// Throttle is 0 to 1000 only
if (_rc_throttle.servo_out <= 0) {
@ -147,6 +203,9 @@ void AP_MotorsTri::output_armed()
_rc_throttle.servo_out = _min_throttle;
_rc_throttle.calc_pwm(); // recalculate radio.out
}
// TODO: set limits.roll_pitch and limits.yaw
//left front
motor_out[AP_MOTORS_MOT_2] = _rc_throttle.radio_out + roll_out + pitch_out;
//right front
@ -213,7 +272,7 @@ void AP_MotorsTri::output_disarmed()
void AP_MotorsTri::output_test(uint8_t motor_seq, int16_t pwm)
{
// exit immediately if not armed
if (!_flags.armed) {
if (!armed()) {
return;
}

5
libraries/AP_Motors/AP_MotorsTri.h
View File

@ -47,8 +47,9 @@ public:
protected:
// output - sends commands to the motors
virtual void output_armed();
virtual void output_disarmed();
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
};

23
libraries/AP_Motors/AP_Motors_Class.cpp
View File

@ -195,10 +195,13 @@ void AP_Motors::output()
// move throttle_low_comp towards desired throttle low comp
update_throttle_low_comp();
// output to motors
if (_flags.armed ) {
output_armed();
}else{
if (_flags.armed) {
if (_flags.stabilizing) {
output_armed_stabilizing();
} else {
output_armed_not_stabilizing();
}
} else {
output_disarmed();
}
};
@ -221,7 +224,7 @@ void AP_Motors::update_throttle_filter()
_throttle_filter.set_cutoff_frequency(1.0f/_loop_rate,_throttle_filt_hz);
}
if (_flags.armed) {
if (armed()) {
_throttle_filter.apply(_throttle_in);
} else {
_throttle_filter.reset(0.0f);
@ -365,3 +368,13 @@ void AP_Motors::update_throttle_low_comp()
}
_throttle_low_comp = constrain_float(_throttle_low_comp, 0.1f, 1.0f);
}
float AP_Motors::rel_pwm_to_thr_range(float pwm) const
{
return 1000.0f*pwm/(_rc_throttle.radio_max-_rc_throttle.radio_min);
}
float AP_Motors::thr_range_to_rel_pwm(float thr) const
{
return (_rc_throttle.radio_max-_rc_throttle.radio_min)*thr/1000.0f;
}

17
libraries/AP_Motors/AP_Motors_Class.h
View File

@ -122,6 +122,7 @@ public:
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(int16_t 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; }
@ -196,9 +197,9 @@ public:
static const struct AP_Param::GroupInfo var_info[];
protected:
// output functions that should be overloaded by child classes
virtual void output_armed()=0;
virtual void output_armed_stabilizing()=0;
virtual void output_armed_not_stabilizing()=0;
virtual void output_disarmed()=0;
// update the throttle input filter
@ -225,12 +226,16 @@ protected:
// get_voltage_comp_gain - return battery voltage compensation gain
float get_voltage_comp_gain() const { return 1.0f/_lift_max; }
float rel_pwm_to_thr_range(float pwm) const;
float thr_range_to_rel_pwm(float thr) const;
// flag bitmask
struct AP_Motors_flags {
uint8_t armed : 1; // 1 if the motors are armed, 0 if disarmed
uint8_t frame_orientation : 4; // PLUS_FRAME 0, X_FRAME 1, V_FRAME 2, H_FRAME 3, NEW_PLUS_FRAME 10, NEW_X_FRAME, NEW_V_FRAME, NEW_H_FRAME
uint8_t slow_start : 1; // 1 if slow start is active
uint8_t slow_start_low_end : 1; // 1 just after arming so we can ramp up the spin_when_armed value
uint8_t armed : 1; // 0 if disarmed, 1 if armed
uint8_t stabilizing : 1; // 0 if not controlling attitude, 1 if controlling attitude
uint8_t frame_orientation : 4; // PLUS_FRAME 0, X_FRAME 1, V_FRAME 2, H_FRAME 3, NEW_PLUS_FRAME 10, NEW_X_FRAME, NEW_V_FRAME, NEW_H_FRAME
uint8_t slow_start : 1; // 1 if slow start is active
uint8_t slow_start_low_end : 1; // 1 just after arming so we can ramp up the spin_when_armed value
} _flags;
// mapping of motor number (as received from upper APM code) to RC channel output - used to account for differences between APM1 and APM2