aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Motors: switched tricopter to use rc_write_angle()

Browse Source 
simplies tricopter handling
This commit is contained in:
Andrew Tridgell 2019-10-12 21:38:51 +11:00 committed by Randy Mackay
parent 1fe146c409
commit ecd9e45568
2 changed files with 8 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
libraries/AP_Motors/AP_MotorsTri.cpp
View File

@ -51,6 +51,8 @@ void AP_MotorsTri::init(motor_frame_class frame_class, motor_frame_type frame_ty
// allow mapping of motor7
add_motor_num(AP_MOTORS_CH_TRI_YAW);
SRV_Channels::set_angle(SRV_Channels::get_motor_function(AP_MOTORS_CH_TRI_YAW), _yaw_servo_angle_max_deg*100);
// check for reverse tricopter
if (frame_type == MOTOR_FRAME_TYPE_PLUSREV) {
_pitch_reversed = true;
@ -95,7 +97,7 @@ void AP_MotorsTri::output_to_motors()
rc_write(AP_MOTORS_MOT_1, output_to_pwm(0));
rc_write(AP_MOTORS_MOT_2, output_to_pwm(0));
rc_write(AP_MOTORS_MOT_4, output_to_pwm(0));
rc_write(AP_MOTORS_CH_TRI_YAW, _yaw_servo->get_trim());
rc_write_angle(AP_MOTORS_CH_TRI_YAW, 0);
break;
case SpoolState::GROUND_IDLE:
// sends output to motors when armed but not flying
@ -105,7 +107,7 @@ void AP_MotorsTri::output_to_motors()
rc_write(AP_MOTORS_MOT_1, output_to_pwm(_actuator[1]));
rc_write(AP_MOTORS_MOT_2, output_to_pwm(_actuator[2]));
rc_write(AP_MOTORS_MOT_4, output_to_pwm(_actuator[4]));
rc_write(AP_MOTORS_CH_TRI_YAW, _yaw_servo->get_trim());
rc_write_angle(AP_MOTORS_CH_TRI_YAW, 0);
break;
case SpoolState::SPOOLING_UP:
case SpoolState::THROTTLE_UNLIMITED:
@ -117,7 +119,7 @@ void AP_MotorsTri::output_to_motors()
rc_write(AP_MOTORS_MOT_1, output_to_pwm(_actuator[1]));
rc_write(AP_MOTORS_MOT_2, output_to_pwm(_actuator[2]));
rc_write(AP_MOTORS_MOT_4, output_to_pwm(_actuator[4]));
rc_write(AP_MOTORS_CH_TRI_YAW, calc_yaw_radio_output(_pivot_angle, radians(_yaw_servo_angle_max_deg)));
rc_write_angle(AP_MOTORS_CH_TRI_YAW, degrees(_pivot_angle)*100);
break;
}
}
@ -154,6 +156,8 @@ void AP_MotorsTri::output_armed_stabilizing()
float rpy_high = 0.0f; // highest motor value
float thr_adj; // the difference between the pilot's desired throttle and throttle_thrust_best_rpy
SRV_Channels::set_angle(SRV_Channels::get_motor_function(AP_MOTORS_CH_TRI_YAW), _yaw_servo_angle_max_deg*100);
// sanity check YAW_SV_ANGLE parameter value to avoid divide by zero
_yaw_servo_angle_max_deg = constrain_float(_yaw_servo_angle_max_deg, AP_MOTORS_TRI_SERVO_RANGE_DEG_MIN, AP_MOTORS_TRI_SERVO_RANGE_DEG_MAX);
@ -310,24 +314,6 @@ void AP_MotorsTri::output_test_seq(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(float yaw_input, float yaw_input_max)
{
int16_t ret;
if (_yaw_servo->get_reversed()) {
yaw_input = -yaw_input;
}
if (yaw_input >= 0) {
ret = (_yaw_servo->get_trim() + (yaw_input / yaw_input_max * (_yaw_servo->get_output_max() - _yaw_servo->get_trim())));
} else {
ret = (_yaw_servo->get_trim() + (yaw_input / yaw_input_max * (_yaw_servo->get_trim() - _yaw_servo->get_output_min())));
}
return ret;
}
/*
call vehicle supplied thrust compensation if set. This allows for
vehicle specific thrust compensation for motor arrangements such as
@ -358,7 +344,7 @@ void AP_MotorsTri::output_motor_mask(float thrust, uint8_t mask, float rudder_dt
AP_MotorsMulticopter::output_motor_mask(thrust, mask, rudder_dt);
// and override yaw servo
rc_write(AP_MOTORS_CH_TRI_YAW, _yaw_servo->get_trim());
rc_write_angle(AP_MOTORS_CH_TRI_YAW, 0);
}
float AP_MotorsTri::get_roll_factor(uint8_t i)

3
libraries/AP_Motors/AP_MotorsTri.h
View File

@ -61,9 +61,6 @@ protected:
// call vehicle supplied thrust compensation if set
void thrust_compensation(void) override;
// calc_yaw_radio_output - calculate final radio output for yaw channel
int16_t calc_yaw_radio_output(float yaw_input, float yaw_input_max); // calculate radio output for yaw servo, typically in range of 1100-1900
// parameters
SRV_Channel *_yaw_servo; // yaw output channel