void AP_MotorsTri::output_to_motors()
{
switch (_spool_mode) {
case SHUT_DOWN:
// sends minimum values out to the motors
rc_write(AP_MOTORS_MOT_1, get_pwm_output_min());
rc_write(AP_MOTORS_MOT_2, get_pwm_output_min());
rc_write(AP_MOTORS_MOT_4, get_pwm_output_min());
rc_write(AP_MOTORS_CH_TRI_YAW, _yaw_servo->get_trim());
break;
case SPIN_WHEN_ARMED:
// sends output to motors when armed but not flying
rc_write(AP_MOTORS_MOT_1, calc_spin_up_to_pwm());
rc_write(AP_MOTORS_MOT_2, calc_spin_up_to_pwm());
rc_write(AP_MOTORS_MOT_4, calc_spin_up_to_pwm());
rc_write(AP_MOTORS_CH_TRI_YAW, _yaw_servo->get_trim());
break;
case SPOOL_UP:
case THROTTLE_UNLIMITED:
case SPOOL_DOWN:
// set motor output based on thrust requests
rc_write(AP_MOTORS_MOT_1, calc_thrust_to_pwm(_thrust_right));
rc_write(AP_MOTORS_MOT_2, calc_thrust_to_pwm(_thrust_left));
rc_write(AP_MOTORS_MOT_4, calc_thrust_to_pwm(_thrust_rear));
rc_write(AP_MOTORS_CH_TRI_YAW, calc_yaw_radio_output(_pivot_angle, radians(_yaw_servo_angle_max_deg)));
break;
}
}
// 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 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
limit.roll_pitch = false;
limit.yaw = false;
limit.throttle_lower = false;
limit.throttle_upper = false;
// Throttle is 0 to 1000 only
int16_t thr_in_min = rel_pwm_to_thr_range(_min_throttle);
if (_throttle_control_input <= thr_in_min) {
_throttle_control_input = thr_in_min;
limit.throttle_lower = true;
}
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 (_throttle_control_input > 800) {
_throttle_control_input = 800;
limit.throttle_upper = true;
}
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( is_zero(_throttle_control_input) ) {
// 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;
}
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)(roll_pwm * 0.866f);
int16_t pitch_out = pitch_pwm / 2;
// check if throttle is below limit
if (_throttle_control_input <= _min_throttle) {
limit.throttle_lower = true;
_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] = throttle_radio_output + roll_out + pitch_out;
//right front
motor_out[AP_MOTORS_MOT_1] = throttle_radio_output - roll_out + pitch_out;
// rear
motor_out[AP_MOTORS_MOT_4] = throttle_radio_output - pitch_pwm;
// Tridge's stability patch
if(motor_out[AP_MOTORS_MOT_1] > out_max) {
motor_out[AP_MOTORS_MOT_2] -= (motor_out[AP_MOTORS_MOT_1] - out_max);
motor_out[AP_MOTORS_MOT_4] -= (motor_out[AP_MOTORS_MOT_1] - out_max);
motor_out[AP_MOTORS_MOT_1] = out_max;
}
if(motor_out[AP_MOTORS_MOT_2] > out_max) {
motor_out[AP_MOTORS_MOT_1] -= (motor_out[AP_MOTORS_MOT_2] - out_max);
motor_out[AP_MOTORS_MOT_4] -= (motor_out[AP_MOTORS_MOT_2] - out_max);
motor_out[AP_MOTORS_MOT_2] = out_max;
}
if(motor_out[AP_MOTORS_MOT_4] > out_max) {
motor_out[AP_MOTORS_MOT_1] -= (motor_out[AP_MOTORS_MOT_4] - out_max);
motor_out[AP_MOTORS_MOT_2] -= (motor_out[AP_MOTORS_MOT_4] - out_max);
motor_out[AP_MOTORS_MOT_4] = out_max;
}
// 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);
// ensure motors don't drop below a minimum value and stop
motor_out[AP_MOTORS_MOT_1] = max(motor_out[AP_MOTORS_MOT_1], out_min);
motor_out[AP_MOTORS_MOT_2] = max(motor_out[AP_MOTORS_MOT_2], out_min);
motor_out[AP_MOTORS_MOT_4] = max(motor_out[AP_MOTORS_MOT_4], out_min);
}
hal.rcout->cork();
// send output to each motor
hal.rcout->write(AP_MOTORS_MOT_1, motor_out[AP_MOTORS_MOT_1]);
hal.rcout->write(AP_MOTORS_MOT_2, motor_out[AP_MOTORS_MOT_2]);
hal.rcout->write(AP_MOTORS_MOT_4, motor_out[AP_MOTORS_MOT_4]);
// send out to yaw command to tail servo
hal.rcout->write(AP_MOTORS_CH_TRI_YAW, yaw_radio_output);
hal.rcout->push();
}
