Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: added tailsitter surface speed scaling

Browse Source 
added scaling of control surfaces by throttle when in hover. This helps
the controller remain stable over a wide range of throttle levels, such
as when descending or rapidly climbing
This commit is contained in:
Andrew Tridgell 2017-11-05 17:05:08 +11:00
parent 98c2f23371
commit 0a70ed2516
2 changed files with 37 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduPlane/quadplane.h
View File

@ -101,6 +101,9 @@ public:
// check if we have completed transition to vtol // check if we have completed transition to vtol
bool tailsitter_transition_vtol_complete(void) const; bool tailsitter_transition_vtol_complete(void) const;
// account for surface speed scaling in hover
void tailsitter_speed_scaling(void);
// user initiated takeoff for guided mode // user initiated takeoff for guided mode
bool do_user_takeoff(float takeoff_altitude); bool do_user_takeoff(float takeoff_altitude);

35
ArduPlane/tailsitter.cpp
View File

@ -80,11 +80,17 @@ void QuadPlane::tailsitter_output(void)
} }
return; return;
} }
motors_output(); motors_output();
plane.pitchController.reset_I(); plane.pitchController.reset_I();
plane.rollController.reset_I(); plane.rollController.reset_I();
if (hal.util->get_soft_armed()) {
// scale surfaces for throttle
tailsitter_speed_scaling();
}
if (tailsitter.vectored_hover_gain > 0) { if (tailsitter.vectored_hover_gain > 0) {
// thrust vectoring VTOL modes // thrust vectoring VTOL modes
float aileron = SRV_Channels::get_output_scaled(SRV_Channel::k_aileron); float aileron = SRV_Channels::get_output_scaled(SRV_Channel::k_aileron);
@ -194,3 +200,30 @@ bool QuadPlane::in_tailsitter_vtol_transition(void) const
{ {
return is_tailsitter() && in_vtol_mode() && transition_state == TRANSITION_ANGLE_WAIT_VTOL; return is_tailsitter() && in_vtol_mode() && transition_state == TRANSITION_ANGLE_WAIT_VTOL;
} }
/*
account for speed scaling of control surfaces in hover
*/
void QuadPlane::tailsitter_speed_scaling(void)
{
const float hover_throttle = motors->get_throttle_hover();
const float throttle = motors->get_throttle();
const float scaling_max = 5;
float scaling = 1;
if (is_zero(throttle)) {
scaling = scaling_max;
} else {
scaling = constrain_float(hover_throttle / throttle, 1/scaling_max, scaling_max);
}
const SRV_Channel::Aux_servo_function_t functions[3] = {
SRV_Channel::Aux_servo_function_t::k_aileron,
SRV_Channel::Aux_servo_function_t::k_elevator,
SRV_Channel::Aux_servo_function_t::k_rudder};
for (uint8_t i=0; i<ARRAY_SIZE(functions); i++) {
int32_t v = SRV_Channels::get_output_scaled(functions[i]);
v *= scaling;
v = constrain_int32(v, -SERVO_MAX, SERVO_MAX);
SRV_Channels::set_output_scaled(functions[i], v);
}
}