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

AP_MotorsTri: implement output_to_motors for spool logic

This commit is contained in:
Leonard Hall 2016-01-20 12:15:10 +09:00 committed by Randy Mackay
parent d0a7579fa0
commit 45a16d6dad
2 changed files with 42 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
libraries/AP_Motors/AP_MotorsTri.cpp
View File

@ -122,6 +122,41 @@ void AP_MotorsTri::output_min()
hal.rcout->push();
}
void AP_MotorsTri::output_to_motors()
{
switch (_multicopter_flags.spool_mode) {
case SHUT_DOWN:
// sends minimum values out to the motors
hal.rcout->cork();
rc_write(AP_MOTORS_MOT_1, _throttle_radio_min);
rc_write(AP_MOTORS_MOT_2, _throttle_radio_min);
rc_write(AP_MOTORS_MOT_4, _throttle_radio_min);
rc_write(AP_MOTORS_CH_TRI_YAW, _yaw_servo_trim);
hal.rcout->push();
break;
case SPIN_WHEN_ARMED:
// sends output to motors when armed but not flying
hal.rcout->cork();
rc_write(AP_MOTORS_MOT_1, constrain_int16(_throttle_radio_min + _throttle_low_end_pct * _min_throttle, _throttle_radio_min, _throttle_radio_min + _min_throttle));
rc_write(AP_MOTORS_MOT_2, constrain_int16(_throttle_radio_min + _throttle_low_end_pct * _min_throttle, _throttle_radio_min, _throttle_radio_min + _min_throttle));
rc_write(AP_MOTORS_MOT_4, constrain_int16(_throttle_radio_min + _throttle_low_end_pct * _min_throttle, _throttle_radio_min, _throttle_radio_min + _min_throttle));
rc_write(AP_MOTORS_CH_TRI_YAW, _yaw_servo_trim);
hal.rcout->push();
break;
case SPOOL_UP:
case THROTTLE_UNLIMITED:
case SPOOL_DOWN:
// set motor output based on thrust requests
hal.rcout->cork();
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, _pivot_angle_max));
hal.rcout->push();
break;
}
}
// 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()
@ -305,22 +340,13 @@ void AP_MotorsTri::output_test(uint8_t motor_seq, int16_t pwm)
}
// calc_yaw_radio_output - calculate final radio output for yaw channel
int16_t AP_MotorsTri::calc_yaw_radio_output()
int16_t AP_MotorsTri::calc_yaw_radio_output(float yaw_input, float yaw_input_max)
{
int16_t ret;
if (_yaw_servo_reverse < 0) {
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)));
}
if (yaw_input >= 0){
ret = (_yaw_servo_trim + (yaw_input/yaw_input_max * (_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);
}
ret = (_yaw_servo_trim + (yaw_input/yaw_input_max * (_yaw_servo_trim - _yaw_servo_min)));
}
return ret;

3
libraries/AP_Motors/AP_MotorsTri.h
View File

@ -40,6 +40,9 @@ public:
// output_min - sends minimum values out to the motors
virtual void output_min();
// output_to_motors - sends minimum values out to the motors
virtual void output_to_motors();
// 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
virtual uint16_t get_motor_mask();