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

Add a feature for a minimum altitude parameter while flying in FBW-B

Browse Source 
This patch written by YureZzZ - Thank you!
This commit is contained in:
Doug Weibel 2011-10-16 23:11:40 -06:00
parent 56f99dc329
commit 86ea3a9558
3 changed files with 20 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ArduPlane/ArduPlane.pde
View File

@ -808,12 +808,20 @@ static void update_current_flight_mode(void)
break; break;
case FLY_BY_WIRE_B: case FLY_BY_WIRE_B:
// fake Navigation output using sticks // Substitute stick inputs for Navigation control output
// We use g.pitch_limit_min because its magnitude is // We use g.pitch_limit_min because its magnitude is
// normally greater than g.pitch_limit_max // normally greater than g.pitch_limit_max
nav_roll = g.channel_roll.norm_input() * g.roll_limit; nav_roll = g.channel_roll.norm_input() * g.roll_limit;
altitude_error = g.channel_pitch.norm_input() * g.pitch_limit_min; altitude_error = g.channel_pitch.norm_input() * g.pitch_limit_min;
if ((current_loc.alt>=home.alt+g.FBWB_min_altitude) || (g.FBWB_min_altitude == -1)) {
altitude_error = g.channel_pitch.norm_input() * g.pitch_limit_min;
} else {
if (g.channel_pitch.norm_input()<0)
altitude_error =( (home.alt + g.FBWB_min_altitude) - current_loc.alt) + g.channel_pitch.norm_input() * g.pitch_limit_min ;
else altitude_error =( (home.alt + g.FBWB_min_altitude) - current_loc.alt) ;
}
if (g.airspeed_enabled == true) if (g.airspeed_enabled == true)
{ {
airspeed_fbwB = ((int)(g.flybywire_airspeed_max - airspeed_fbwB = ((int)(g.flybywire_airspeed_max -

3
ArduPlane/Parameters.h
View File

@ -83,6 +83,7 @@ public:
// //
k_param_flybywire_airspeed_min = 120, k_param_flybywire_airspeed_min = 120,
k_param_flybywire_airspeed_max, k_param_flybywire_airspeed_max,
k_param_FBWB_min_altitude, // -1=disabled, minimum value for altitude in cm (default 30m)
// //
// 130: Sensor parameters // 130: Sensor parameters
@ -262,6 +263,7 @@ public:
// //
AP_Int8 flybywire_airspeed_min; AP_Int8 flybywire_airspeed_min;
AP_Int8 flybywire_airspeed_max; AP_Int8 flybywire_airspeed_max;
AP_Int16 FBWB_min_altitude;
// Throttle // Throttle
// //
@ -411,6 +413,7 @@ public:
airspeed_cruise (AIRSPEED_CRUISE_CM, k_param_airspeed_cruise, PSTR("TRIM_ARSPD_CM")), airspeed_cruise (AIRSPEED_CRUISE_CM, k_param_airspeed_cruise, PSTR("TRIM_ARSPD_CM")),
pitch_trim (0, k_param_pitch_trim, PSTR("TRIM_PITCH_CD")), pitch_trim (0, k_param_pitch_trim, PSTR("TRIM_PITCH_CD")),
RTL_altitude (ALT_HOLD_HOME_CM, k_param_RTL_altitude, PSTR("ALT_HOLD_RTL")), RTL_altitude (ALT_HOLD_HOME_CM, k_param_RTL_altitude, PSTR("ALT_HOLD_RTL")),
FBWB_min_altitude (ALT_HOLD_FBW_CM, k_param_FBWB_min_altitude, PSTR("ALT_HOLD_FBWCM")),
ground_temperature (0, k_param_ground_temperature, PSTR("GND_TEMP")), ground_temperature (0, k_param_ground_temperature, PSTR("GND_TEMP")),
ground_pressure (0, k_param_ground_pressure, PSTR("GND_ABS_PRESS")), ground_pressure (0, k_param_ground_pressure, PSTR("GND_ABS_PRESS")),
compass_enabled (MAGNETOMETER, k_param_compass_enabled, PSTR("MAG_ENABLE")), compass_enabled (MAGNETOMETER, k_param_compass_enabled, PSTR("MAG_ENABLE")),

8
ArduPlane/config.h
View File

@ -366,6 +366,14 @@
#ifndef AIRSPEED_FBW_MAX #ifndef AIRSPEED_FBW_MAX
# define AIRSPEED_FBW_MAX 22 # define AIRSPEED_FBW_MAX 22
#endif #endif
#ifndef ALT_HOLD_FBW
# define ALT_HOLD_FBW 0
#endif
#define ALT_HOLD_FBW_CM ALT_HOLD_FBW*100
/* The following parmaeters have no corresponding control implementation /* The following parmaeters have no corresponding control implementation
#ifndef THROTTLE_ALT_P #ifndef THROTTLE_ALT_P
# define THROTTLE_ALT_P 0.32 # define THROTTLE_ALT_P 0.32