Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: tailsitter pull in copter ouputs from AP_motors

Browse Source
This commit is contained in:
IamPete1 2019-01-31 20:40:19 +00:00 committed by Andrew Tridgell
parent d075965fce
commit 872c3df45c
1 changed files with 9 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
ArduPlane/tailsitter.cpp
View File

@ -108,6 +108,12 @@ void QuadPlane::tailsitter_output(void)
plane.pitchController.reset_I();
plane.rollController.reset_I();
// pull in copter control outputs
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, (motors->get_yaw())*-SERVO_MAX);
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, (motors->get_pitch())*SERVO_MAX);
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, (motors->get_roll())*SERVO_MAX);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, (motors->get_throttle()) * 100);
if (hal.util->get_soft_armed()) {
// scale surfaces for throttle
tailsitter_speed_scaling();
@ -123,12 +129,12 @@ void QuadPlane::tailsitter_output(void)
takeoff without integrator windup
*/
int32_t pitch_error_cd = (plane.nav_pitch_cd - ahrs_view->pitch_sensor) * 0.5;
float extra_pitch = constrain_float(pitch_error_cd, -4500, 4500) / 4500.0;
float extra_pitch = constrain_float(pitch_error_cd, -SERVO_MAX, SERVO_MAX) / SERVO_MAX;
float extra_sign = extra_pitch > 0?1:-1;
float extra_elevator = extra_sign * powf(fabsf(extra_pitch), tailsitter.vectored_hover_power) * 4500;
float extra_elevator = extra_sign * powf(fabsf(extra_pitch), tailsitter.vectored_hover_power) * SERVO_MAX;
tilt_left = extra_elevator + tilt_left * tailsitter.vectored_hover_gain;
tilt_right = extra_elevator + tilt_right * tailsitter.vectored_hover_gain;
if (fabsf(tilt_left) >= 4500 || fabsf(tilt_right) >= 4500) {
if (fabsf(tilt_left) >= SERVO_MAX || fabsf(tilt_right) >= SERVO_MAX) {
// prevent integrator windup
motors->limit.roll_pitch = 1;
motors->limit.yaw = 1;