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

Plane: pass aircraft parameters to attitude controllers

Browse Source 
also cope with rename of airspeed min/max variables

Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>
This commit is contained in:
Andrew Tridgell 2013-07-18 11:58:23 +10:00
parent fbdf857634
commit 608345415b
6 changed files with 25 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
ArduPlane/ArduPlane.pde
View File

@ -70,6 +70,10 @@
// Local modules
#include "defines.h"
// key aircraft parameters passed to the speed/height controller
static AP_SpdHgtControl::AircraftParameters aparm;
#include "Parameters.h"
#include "GCS.h"
@ -103,9 +107,6 @@ static const AP_InertialSensor::Sample_rate ins_sample_rate = AP_InertialSensor:
//
static Parameters g;
// key aircraft parameters passed to the speed/height controller
static AP_SpdHgtControl::AircraftParameters aparm;
// main loop scheduler
static AP_Scheduler scheduler;

18
ArduPlane/Attitude.pde
View File

@ -75,8 +75,7 @@ static void stabilize_roll(float speed_scaler)
channel_roll->servo_out = g.rollController.get_servo_out(nav_roll_cd - ahrs.roll_sensor,
speed_scaler,
control_mode == STABILIZE,
aparm.flybywire_airspeed_min);
control_mode == STABILIZE);
}
/*
@ -89,9 +88,7 @@ static void stabilize_pitch(float speed_scaler)
int32_t demanded_pitch = nav_pitch_cd + g.pitch_trim_cd + channel_throttle->servo_out * g.kff_throttle_to_pitch;
channel_pitch->servo_out = g.pitchController.get_servo_out(demanded_pitch - ahrs.pitch_sensor,
speed_scaler,
control_mode == STABILIZE,
aparm.flybywire_airspeed_min,
aparm.flybywire_airspeed_max);
control_mode == STABILIZE);
}
/*
@ -262,8 +259,7 @@ static void stabilize_acro(float speed_scaler)
// 'stabilze' to true, which disables the roll integrator
channel_roll->servo_out = g.rollController.get_servo_out(roll_error_cd,
speed_scaler,
true,
aparm.flybywire_airspeed_min);
true);
} else {
/*
aileron stick is non-zero, use pure rate control until the
@ -287,9 +283,7 @@ static void stabilize_acro(float speed_scaler)
nav_pitch_cd = acro_state.locked_pitch_cd;
channel_pitch->servo_out = g.pitchController.get_servo_out(nav_pitch_cd - ahrs.pitch_sensor,
speed_scaler,
false,
aparm.flybywire_airspeed_min,
aparm.flybywire_airspeed_max);
false);
} else {
/*
user has non-zero pitch input, use a pure rate controller
@ -413,9 +407,7 @@ static void calc_nav_yaw(float speed_scaler, float ch4_inf)
}
channel_rudder->servo_out = g.yawController.get_servo_out(speed_scaler,
control_mode == STABILIZE,
aparm.flybywire_airspeed_min,
aparm.flybywire_airspeed_max);
control_mode == STABILIZE);
// add in rudder mixing from roll
channel_rudder->servo_out += channel_roll->servo_out * g.kff_rudder_mix;

10
ArduPlane/Parameters.h
View File

@ -104,8 +104,8 @@ public:
// 120: Fly-by-wire control
//
k_param_flybywire_airspeed_min = 120,
k_param_flybywire_airspeed_max,
k_param_airspeed_min = 120,
k_param_airspeed_max,
k_param_FBWB_min_altitude_cm, // 0=disabled, minimum value for altitude in cm (for first time try 30 meters = 3000 cm)
k_param_flybywire_elev_reverse,
k_param_alt_control_algorithm,
@ -437,6 +437,12 @@ public:
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
rc_12 (CH_12),
#endif
// pass the aircraft parameters structure into the attitude controllers
rollController(aparm),
pitchController(aparm),
yawController(aparm),
// PID controller initial P initial I initial D initial imax
//-----------------------------------------------------------------------------------
pidNavPitchAirspeed (NAV_PITCH_ASP_P, NAV_PITCH_ASP_I, NAV_PITCH_ASP_D, NAV_PITCH_ASP_INT_MAX_CMSEC),

4
ArduPlane/Parameters.pde
View File

@ -255,7 +255,7 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Range: 5 50
// @Increment: 1
// @User: Standard
ASCALAR(flybywire_airspeed_min, "ARSPD_FBW_MIN", AIRSPEED_FBW_MIN),
ASCALAR(airspeed_min, "ARSPD_FBW_MIN", AIRSPEED_FBW_MIN),
// @Param: ARSPD_FBW_MAX
// @DisplayName: Fly By Wire Maximum Airspeed
@ -264,7 +264,7 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Range: 5 50
// @Increment: 1
// @User: Standard
ASCALAR(flybywire_airspeed_max, "ARSPD_FBW_MAX", AIRSPEED_FBW_MAX),
ASCALAR(airspeed_max, "ARSPD_FBW_MAX", AIRSPEED_FBW_MAX),
// @Param: FBWB_ELEV_REV
// @DisplayName: Fly By Wire elevator reverse

10
ArduPlane/navigation.pde
View File

@ -86,10 +86,10 @@ static void calc_airspeed_errors()
// FBW_B airspeed target
if (control_mode == FLY_BY_WIRE_B ||
control_mode == CRUISE) {
target_airspeed_cm = ((int32_t)(aparm.flybywire_airspeed_max -
aparm.flybywire_airspeed_min) *
target_airspeed_cm = ((int32_t)(aparm.airspeed_max -
aparm.airspeed_min) *
channel_throttle->control_in) +
((int32_t)aparm.flybywire_airspeed_min * 100);
((int32_t)aparm.airspeed_min * 100);
}
// Set target to current airspeed + ground speed undershoot,
@ -107,8 +107,8 @@ static void calc_airspeed_errors()
}
// Apply airspeed limit
if (target_airspeed_cm > (aparm.flybywire_airspeed_max * 100))
target_airspeed_cm = (aparm.flybywire_airspeed_max * 100);
if (target_airspeed_cm > (aparm.airspeed_max * 100))
target_airspeed_cm = (aparm.airspeed_max * 100);
airspeed_error_cm = target_airspeed_cm - aspeed_cm;
airspeed_energy_error = ((target_airspeed_cm * target_airspeed_cm) - (aspeed_cm*aspeed_cm))*0.00005;

2
ArduPlane/radio.pde
View File

@ -103,7 +103,7 @@ static void read_radio()
if (g.throttle_nudge && channel_throttle->servo_out > 50) {
float nudge = (channel_throttle->servo_out - 50) * 0.02;
if (alt_control_airspeed()) {
airspeed_nudge_cm = (aparm.flybywire_airspeed_max * 100 - g.airspeed_cruise_cm) * nudge;
airspeed_nudge_cm = (aparm.airspeed_max * 100 - g.airspeed_cruise_cm) * nudge;
} else {
throttle_nudge = (aparm.throttle_max - aparm.throttle_cruise) * nudge;
}