Skip to content

Commit

Permalink
AP_Motors: RCMAP fix
Browse files Browse the repository at this point in the history 
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.
  • Loading branch information
@R-Lefebvre @rmackay9
R-Lefebvre authored and rmackay9 committed May 25, 2015
1 parent 70a9a56 commit b8181b6
Show file tree
Hide file tree
Showing 17 changed files with 334 additions and 231 deletions.
51 changes: 27 additions & 24 deletions libraries/AP_Motors/AP_MotorsCoax.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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();
Expand All @@ -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);
Expand All @@ -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
Expand All @@ -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);
Expand Down
4 changes: 2 additions & 2 deletions libraries/AP_Motors/AP_MotorsCoax.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand Down
39 changes: 26 additions & 13 deletions libraries/AP_Motors/AP_MotorsHeli.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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 changes: 12 additions & 6 deletions libraries/AP_Motors/AP_MotorsHeli.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -89,19 +89,15 @@ 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,
RC_Channel& swash_servo_3,
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),
Expand Down Expand Up @@ -207,6 +203,12 @@ class AP_MotorsHeli : public AP_Motors {
// 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();

Expand Down Expand Up @@ -310,6 +312,10 @@ class AP_MotorsHeli : public AP_Motors {
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 changes: 3 additions & 2 deletions libraries/AP_Motors/AP_MotorsHexa.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down
Loading

0 comments on commit b8181b6

Please sign in to comment.