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

Plane: changed to using a "aircraft parameters" structure 

this structure can be passed to libraries to give them easy access to
critical user parameters
This commit is contained in:
Andrew Tridgell 2013-06-26 20:48:45 +10:00
parent 89271b7774
commit a5bda3ffef
8 changed files with 60 additions and 66 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
ArduPlane/ArduPlane.pde
View File

@ -103,6 +103,9 @@ 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;
@ -235,7 +238,7 @@ AP_InertialSensor_Oilpan ins( &apm1_adc );
AP_AHRS_DCM ahrs(&ins, g_gps);
static AP_L1_Control L1_controller(&ahrs);
static AP_TECS TECS_controller(&ahrs, &barometer);
static AP_TECS TECS_controller(&ahrs, &barometer, aparm);
#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
SITL sitl;
@ -349,14 +352,6 @@ static uint32_t last_heartbeat_ms;
// A timer used to track how long we have been in a "short failsafe" condition due to loss of RC signal
static uint32_t ch3_failsafe_timer = 0;
////////////////////////////////////////////////////////////////////////////////
// AUTO takeoff
////////////////////////////////////////////////////////////////////////////////
// States used for delay from launch detect to engine start
static bool launchCountStarted;
static uint32_t last_tkoff_arm_time;
////////////////////////////////////////////////////////////////////////////////
// LED output
////////////////////////////////////////////////////////////////////////////////
@ -1015,7 +1010,7 @@ static void update_current_flight_mode(void)
}
// max throttle for takeoff
channel_throttle->servo_out = g.throttle_max;
channel_throttle->servo_out = aparm.throttle_max;
break;
@ -1086,10 +1081,10 @@ static void update_current_flight_mode(void)
// if the pitch is past the set pitch limits, then
// we set target pitch to the limit
if (ahrs.pitch_sensor >= g.pitch_limit_max_cd) {
nav_pitch_cd = g.pitch_limit_max_cd;
} else if (ahrs.pitch_sensor <= g.pitch_limit_min_cd) {
nav_pitch_cd = g.pitch_limit_min_cd;
if (ahrs.pitch_sensor >= aparm.pitch_limit_max_cd) {
nav_pitch_cd = aparm.pitch_limit_max_cd;
} else if (ahrs.pitch_sensor <= aparm.pitch_limit_min_cd) {
nav_pitch_cd = aparm.pitch_limit_min_cd;
} else {
training_manual_pitch = true;
nav_pitch_cd = 0;
@ -1106,11 +1101,11 @@ static void update_current_flight_mode(void)
nav_roll_cd = constrain_int32(nav_roll_cd, -g.roll_limit_cd, g.roll_limit_cd);
float pitch_input = channel_pitch->norm_input();
if (pitch_input > 0) {
nav_pitch_cd = pitch_input * g.pitch_limit_max_cd;
nav_pitch_cd = pitch_input * aparm.pitch_limit_max_cd;
} else {
nav_pitch_cd = -(pitch_input * g.pitch_limit_min_cd);
nav_pitch_cd = -(pitch_input * aparm.pitch_limit_min_cd);
}
nav_pitch_cd = constrain_int32(nav_pitch_cd, g.pitch_limit_min_cd.get(), g.pitch_limit_max_cd.get());
nav_pitch_cd = constrain_int32(nav_pitch_cd, aparm.pitch_limit_min_cd.get(), aparm.pitch_limit_max_cd.get());
if (inverted_flight) {
nav_pitch_cd = -nav_pitch_cd;
}

50
ArduPlane/Attitude.pde
View File

@ -73,7 +73,9 @@ static void stabilize_roll(float speed_scaler)
if (ahrs.roll_sensor < 0) nav_roll_cd -= 36000;
}
channel_roll->servo_out = g.rollController.get_servo_out(nav_roll_cd, speed_scaler, control_mode == STABILIZE, g.flybywire_airspeed_min);
channel_roll->servo_out = g.rollController.get_servo_out(nav_roll_cd, speed_scaler,
control_mode == STABILIZE,
aparm.flybywire_airspeed_min);
}
/*
@ -85,9 +87,10 @@ 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,
speed_scaler,
control_mode == STABILIZE,
g.flybywire_airspeed_min, g.flybywire_airspeed_max);
speed_scaler,
control_mode == STABILIZE,
aparm.flybywire_airspeed_min,
aparm.flybywire_airspeed_max);
}
/*
@ -161,11 +164,11 @@ static void stabilize_stick_mixing_fbw()
pitch_input = -pitch_input;
}
if (pitch_input > 0) {
nav_pitch_cd += pitch_input * g.pitch_limit_max_cd;
nav_pitch_cd += pitch_input * aparm.pitch_limit_max_cd;
} else {
nav_pitch_cd += -(pitch_input * g.pitch_limit_min_cd);
nav_pitch_cd += -(pitch_input * aparm.pitch_limit_min_cd);
}
nav_pitch_cd = constrain_int32(nav_pitch_cd, g.pitch_limit_min_cd.get(), g.pitch_limit_max_cd.get());
nav_pitch_cd = constrain_int32(nav_pitch_cd, aparm.pitch_limit_min_cd.get(), aparm.pitch_limit_max_cd.get());
}
@ -265,7 +268,7 @@ static void stabilize()
static void calc_throttle()
{
if (g.throttle_cruise <= 1) {
if (aparm.throttle_cruise <= 1) {
// user has asked for zero throttle - this may be done by a
// mission which wants to turn off the engine for a parachute
// landing
@ -276,7 +279,7 @@ static void calc_throttle()
if (g.alt_control_algorithm == ALT_CONTROL_TECS) {
channel_throttle->servo_out = SpdHgt_Controller->get_throttle_demand();
} else if (!alt_control_airspeed()) {
int16_t throttle_target = g.throttle_cruise + throttle_nudge;
int16_t throttle_target = aparm.throttle_cruise + throttle_nudge;
// TODO: think up an elegant way to bump throttle when
// groundspeed_undershoot > 0 in the no airspeed sensor case; PID
@ -286,23 +289,23 @@ static void calc_throttle()
// AUTO, RTL, etc
// ---------------------------------------------------------------------------
if (nav_pitch_cd >= 0) {
channel_throttle->servo_out = throttle_target + (g.throttle_max - throttle_target) * nav_pitch_cd / g.pitch_limit_max_cd;
channel_throttle->servo_out = throttle_target + (aparm.throttle_max - throttle_target) * nav_pitch_cd / aparm.pitch_limit_max_cd;
} else {
channel_throttle->servo_out = throttle_target - (throttle_target - g.throttle_min) * nav_pitch_cd / g.pitch_limit_min_cd;
channel_throttle->servo_out = throttle_target - (throttle_target - aparm.throttle_min) * nav_pitch_cd / aparm.pitch_limit_min_cd;
}
channel_throttle->servo_out = constrain_int16(channel_throttle->servo_out, g.throttle_min.get(), g.throttle_max.get());
channel_throttle->servo_out = constrain_int16(channel_throttle->servo_out, aparm.throttle_min.get(), aparm.throttle_max.get());
} else {
// throttle control with airspeed compensation
// -------------------------------------------
energy_error = airspeed_energy_error + altitude_error_cm * 0.098f;
// positive energy errors make the throttle go higher
channel_throttle->servo_out = g.throttle_cruise + g.pidTeThrottle.get_pid(energy_error);
channel_throttle->servo_out = aparm.throttle_cruise + g.pidTeThrottle.get_pid(energy_error);
channel_throttle->servo_out += (channel_pitch->servo_out * g.kff_pitch_to_throttle);
channel_throttle->servo_out = constrain_int16(channel_throttle->servo_out,
g.throttle_min.get(), g.throttle_max.get());
aparm.throttle_min.get(), aparm.throttle_max.get());
}
@ -326,8 +329,9 @@ 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,
g.flybywire_airspeed_min, g.flybywire_airspeed_max);
control_mode == STABILIZE,
aparm.flybywire_airspeed_min,
aparm.flybywire_airspeed_max);
// add in rudder mixing from roll
channel_rudder->servo_out += channel_roll->servo_out * g.kff_rudder_mix;
@ -346,7 +350,7 @@ static void calc_nav_pitch()
} else {
nav_pitch_cd = g.pidNavPitchAltitude.get_pid(altitude_error_cm);
}
nav_pitch_cd = constrain_int32(nav_pitch_cd, g.pitch_limit_min_cd.get(), g.pitch_limit_max_cd.get());
nav_pitch_cd = constrain_int32(nav_pitch_cd, aparm.pitch_limit_min_cd.get(), aparm.pitch_limit_max_cd.get());
}
@ -375,9 +379,9 @@ static void calc_nav_roll()
static void throttle_slew_limit(int16_t last_throttle)
{
// if slew limit rate is set to zero then do not slew limit
if (g.throttle_slewrate) {
if (aparm.throttle_slewrate) {
// limit throttle change by the given percentage per second
float temp = g.throttle_slewrate * G_Dt * 0.01 * fabsf(channel_throttle->radio_max - channel_throttle->radio_min);
float temp = aparm.throttle_slewrate * G_Dt * 0.01 * fabsf(channel_throttle->radio_max - channel_throttle->radio_min);
// allow a minimum change of 1 PWM per cycle
if (temp < 1) {
temp = 1;
@ -422,6 +426,8 @@ static bool auto_takeoff_check(void)
#else
// this is a more advanced check that relies on TECS
uint32_t now = hal.scheduler->micros();
static bool launchCountStarted;
static uint32_t last_tkoff_arm_time;
if (g_gps == NULL || g_gps->status() != GPS::GPS_OK_FIX_3D)
{
@ -680,8 +686,8 @@ static void set_servos(void)
#else
// convert 0 to 100% into PWM
channel_throttle->servo_out = constrain_int16(channel_throttle->servo_out,
g.throttle_min.get(),
g.throttle_max.get());
aparm.throttle_min.get(),
aparm.throttle_max.get());
if (suppress_throttle()) {
// throttle is suppressed in auto mode
@ -718,7 +724,7 @@ static void set_servos(void)
} else if (airspeed.use()) {
flapSpeedSource = g.airspeed_cruise_cm * 0.01;
} else {
flapSpeedSource = g.throttle_cruise;
flapSpeedSource = aparm.throttle_cruise;
}
if ( g.flap_1_speed != 0 && flapSpeedSource > g.flap_1_speed) {
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_flap_auto, 0);

8
ArduPlane/Parameters.h
View File

@ -297,21 +297,15 @@ public:
// Fly-by-wire
//
AP_Int16 flybywire_airspeed_min;
AP_Int16 flybywire_airspeed_max;
AP_Int8 flybywire_elev_reverse;
AP_Int8 flybywire_climb_rate;
// Throttle
//
AP_Int8 throttle_min;
AP_Int8 throttle_max;
AP_Int8 throttle_slewrate;
AP_Int8 throttle_suppress_manual;
AP_Int8 throttle_passthru_stabilize;
AP_Int8 throttle_fs_enabled;
AP_Int16 throttle_fs_value;
AP_Int8 throttle_cruise;
AP_Int8 throttle_nudge;
// Failsafe
@ -334,8 +328,6 @@ public:
// Navigational maneuvering limits
//
AP_Int16 roll_limit_cd;
AP_Int16 pitch_limit_max_cd;
AP_Int16 pitch_limit_min_cd;
AP_Int16 alt_offset;
// Misc

17
ArduPlane/Parameters.pde
View File

@ -10,6 +10,7 @@
*/
#define GSCALAR(v, name, def) { g.v.vtype, name, Parameters::k_param_ ## v, &g.v, {def_value : def} }
#define ASCALAR(v, name, def) { aparm.v.vtype, name, Parameters::k_param_ ## v, &aparm.v, {def_value : def} }
#define GGROUP(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &g.v, {group_info : class::var_info} }
#define GOBJECT(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &v, {group_info : class::var_info} }
@ -254,7 +255,7 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Range: 5 50
// @Increment: 1
// @User: Standard
GSCALAR(flybywire_airspeed_min, "ARSPD_FBW_MIN", AIRSPEED_FBW_MIN),
ASCALAR(flybywire_airspeed_min, "ARSPD_FBW_MIN", AIRSPEED_FBW_MIN),
// @Param: ARSPD_FBW_MAX
// @DisplayName: Fly By Wire Maximum Airspeed
@ -263,7 +264,7 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Range: 5 50
// @Increment: 1
// @User: Standard
GSCALAR(flybywire_airspeed_max, "ARSPD_FBW_MAX", AIRSPEED_FBW_MAX),
ASCALAR(flybywire_airspeed_max, "ARSPD_FBW_MAX", AIRSPEED_FBW_MAX),
// @Param: FBWB_ELEV_REV
// @DisplayName: Fly By Wire elevator reverse
@ -287,7 +288,7 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Range: 0 100
// @Increment: 1
// @User: Standard
GSCALAR(throttle_min, "THR_MIN", THROTTLE_MIN),
ASCALAR(throttle_min, "THR_MIN", THROTTLE_MIN),
// @Param: THR_MAX
// @DisplayName: Maximum Throttle
@ -296,7 +297,7 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Range: 0 100
// @Increment: 1
// @User: Standard
GSCALAR(throttle_max, "THR_MAX", THROTTLE_MAX),
ASCALAR(throttle_max, "THR_MAX", THROTTLE_MAX),
// @Param: THR_SLEWRATE
// @DisplayName: Throttle slew rate
@ -305,7 +306,7 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Range: 0 100
// @Increment: 1
// @User: Standard
GSCALAR(throttle_slewrate, "THR_SLEWRATE", THROTTLE_SLEW_LIMIT),
ASCALAR(throttle_slewrate, "THR_SLEWRATE", THROTTLE_SLEW_LIMIT),
// @Param: THR_SUPP_MAN
// @DisplayName: Throttle suppress manual passthru
@ -344,7 +345,7 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Range: 0 100
// @Increment: 1
// @User: Standard
GSCALAR(throttle_cruise, "TRIM_THROTTLE", THROTTLE_CRUISE),
ASCALAR(throttle_cruise, "TRIM_THROTTLE", THROTTLE_CRUISE),
// @Param: THROTTLE_NUDGE
// @DisplayName: Throttle nudge enable
@ -453,7 +454,7 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Range: 0 9000
// @Increment: 1
// @User: Standard
GSCALAR(pitch_limit_max_cd, "LIM_PITCH_MAX", PITCH_MAX_CENTIDEGREE),
ASCALAR(pitch_limit_max_cd, "LIM_PITCH_MAX", PITCH_MAX_CENTIDEGREE),
// @Param: LIM_PITCH_MIN
// @DisplayName: Minimum Pitch Angle
@ -462,7 +463,7 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Range: -9000 0
// @Increment: 1
// @User: Standard
GSCALAR(pitch_limit_min_cd, "LIM_PITCH_MIN", PITCH_MIN_CENTIDEGREE),
ASCALAR(pitch_limit_min_cd, "LIM_PITCH_MIN", PITCH_MIN_CENTIDEGREE),
// @Param: AUTO_TRIM
// @DisplayName: Auto trim

6
ArduPlane/commands_logic.pde
View File

@ -323,7 +323,7 @@ static bool verify_takeoff()
}
// see if we have reached takeoff altitude
if (adjusted_altitude_cm() > takeoff_altitude) {
if (adjusted_altitude_cm() > takeoff_altitude_cm) {
hold_course_cd = -1;
takeoff_complete = true;
next_WP = prev_WP = current_loc;
@ -368,7 +368,7 @@ static bool verify_land()
// target speeds too early.
g.airspeed_cruise_cm.load();
g.min_gndspeed_cm.load();
g.throttle_cruise.load();
aparm.throttle_cruise.load();
}
}
@ -589,7 +589,7 @@ static void do_change_speed()
if (next_nonnav_command.lat > 0) {
gcs_send_text_fmt(PSTR("Set throttle %u"), (unsigned)next_nonnav_command.lat);
g.throttle_cruise.set(next_nonnav_command.lat);
aparm.throttle_cruise.set(next_nonnav_command.lat);
}
}

2
ArduPlane/events.pde
View File

@ -88,7 +88,7 @@ void low_battery_event(void)
gcs_send_text_fmt(PSTR("Low Battery %.2fV Used %.0f mAh"),
battery.voltage, battery.current_total_mah);
set_mode(RTL);
g.throttle_cruise = THROTTLE_CRUISE;
aparm.throttle_cruise.load();
battery.low_batttery = true;
}

10
ArduPlane/navigation.pde
View File

@ -85,10 +85,10 @@ static void calc_airspeed_errors()
// FBW_B airspeed target
if (control_mode == FLY_BY_WIRE_B) {
target_airspeed_cm = ((int)(g.flybywire_airspeed_max -
g.flybywire_airspeed_min) *
target_airspeed_cm = ((int32_t)(aparm.flybywire_airspeed_max -
aparm.flybywire_airspeed_min) *
channel_throttle->servo_out) +
((int)g.flybywire_airspeed_min * 100);
((int32_t)aparm.flybywire_airspeed_min * 100);
}
// Set target to current airspeed + ground speed undershoot,
@ -106,8 +106,8 @@ static void calc_airspeed_errors()
}
// Apply airspeed limit
if (target_airspeed_cm > (g.flybywire_airspeed_max * 100))
target_airspeed_cm = (g.flybywire_airspeed_max * 100);
if (target_airspeed_cm > (aparm.flybywire_airspeed_max * 100))
target_airspeed_cm = (aparm.flybywire_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;

4
ArduPlane/radio.pde
View File

@ -115,9 +115,9 @@ 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 = (g.flybywire_airspeed_max * 100 - g.airspeed_cruise_cm) * nudge;
airspeed_nudge_cm = (aparm.flybywire_airspeed_max * 100 - g.airspeed_cruise_cm) * nudge;
} else {
throttle_nudge = (g.throttle_max - g.throttle_cruise) * nudge;
throttle_nudge = (aparm.throttle_max - aparm.throttle_cruise) * nudge;
}
} else {
airspeed_nudge_cm = 0;