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(); }