Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-07 00:53:41 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

several flights in the heat !

Browse Source
This commit is contained in:
Antoine Drouin
2006-07-20 15:58:06 +00:00
parent f903ac1477
commit d03cfa1f7f
11 changed files with 106 additions and 70 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/microjet5.xml
+10 -10
View File
@@ -62,7 +62,7 @@
<define name="RAD_OF_IR_MIN_VALUE" value="0.00075"/>
<define name="ADC_ROLL_NEUTRAL" value="0"/>
<define name="ADC_PITCH_NEUTRAL" value="-716"/>
<define name="ADC_TOP_NEUTRAL" value="716"/>
<define name="ADC_TOP_NEUTRAL" value="512"/>
</section>
<section name="GYRO" prefix="GYRO_">
@@ -90,7 +90,7 @@
<section name="PID">
<define name="ROLL_PGAIN" value="5000."/>
<define name="PITCH_OF_ROLL" value="0.25"/>
<define name="PITCH_PGAIN" value="8000."/>
<define name="PITCH_PGAIN" value="10000."/>
<define name="MAX_ROLL" value="0.35"/>
<define name="MAX_PITCH" value="0.35"/>
<define name="MIN_PITCH" value="-0.35"/>
@@ -98,19 +98,19 @@
</section>
<section name="ALT" prefix="CLIMB_">
<define name="PITCH_PGAIN" value="-0.1"/>
<define name="PITCH_IGAIN" value="0.025"/>
<define name="PITCH_PGAIN" value="-0.05"/>
<define name="PITCH_IGAIN" value="0.075"/>
<define name="PGAIN" value="-0.02"/>
<define name="IGAIN" value="0.1"/>
<define name="MAX" value="1."/>
<define name="LEVEL_GAZ" value="0.50"/>
<define name="PITCH_OF_VZ_PGAIN" value="0.05"/>
<define name="GAZ_OF_CLIMB" value="0.2" unit="%/(m/s)"/>
<define name="GAZ_OF_CLIMB" value="0.15" unit="%/(m/s)"/>
</section>
<section name="NAV">
<define name="COURSE_PGAIN" value="-0.5"/>
<define name="ALTITUDE_PGAIN" value="-0.025"/>
<define name="ALTITUDE_PGAIN" value="-0.05"/>
<define name="NAV_PITCH" value="0."/>
</section>
@@ -121,12 +121,12 @@
</section>
<section name="AGGRESSIVE" prefix="AGR_">
<define name="CLIMB_GAZ" value="0.75"/><!-- Gaz for Aggressive Climb -->
<define name="CLIMB_GAZ" value="0.95"/><!-- Gaz for Aggressive Climb -->
<define name="DESCENT_GAZ" value="0.1"/><!-- Gaz for Aggressive Decent -->
<define name="CLIMB_PITCH" value="0.3"/><!-- Pitch for Aggressive Climb -->
<define name="DESCENT_PITCH" value="-0.35"/><!-- Pitch for Aggressive Decent -->
<define name="BLEND_START" value="30"/><!-- Altitude Error to Initiate Aggressive Climb CANNOT BE ZERO!!-->
<define name="BLEND_END" value="15"/><!-- Altitude Error to Blend Aggressive to Regular Climb Modes CANNOT BE ZERO!!-->
<define name="DESCENT_PITCH" value="-0.25"/><!-- Pitch for Aggressive Decent -->
<define name="BLEND_START" value="20"/><!-- Altitude Error to Initiate Aggressive Climb CANNOT BE ZERO!!-->
<define name="BLEND_END" value="10"/><!-- Altitude Error to Blend Aggressive to Regular Climb Modes CANNOT BE ZERO!!-->
<define name="CLIMB_NAV_RATIO" value="0.8"/><!-- Percent Navigation for Altitude Error Equal to Start Altitude -->
<define name="DESCENT_NAV_RATIO" value="1.0"/>
</section>
conf/airframes/twinjet1.xml
+2 -2
View File
@@ -51,7 +51,7 @@
<define name="ROLL_NEUTRAL_DEFAULT" value="0" unit="deg"/>
<define name="PITCH_NEUTRAL_DEFAULT" value="-1" unit="deg"/>
<define name="DEFAULT_CONTRAST" value="200"/>
<define name="RAD_OF_IR_CONTRAST" value="0.75"/>
<define name="RAD_OF_IR_CONTRAST" value="0.5"/>
<linear name="RollOfIrs" arity="2" coeff1="0" coeff2="-1"/>
<linear name="PitchOfIrs" arity="2" coeff1="-1" coeff2="0"/>
<linear name="TopOfIr" arity="1" coeff1="1"/>
@@ -103,7 +103,7 @@
<section name="NAV">
<define name="COURSE_PGAIN" value="-0.9"/>
<define name="ALTITUDE_PGAIN" value="-0.025"/>
<define name="ALTITUDE_PGAIN" value="-0.05" unit="m/s/m" />
<define name="NAV_PITCH" value="0."/>
</section>
conf/flight_plans/EMAV2006.xml
+13 -5
View File
@@ -8,6 +8,7 @@
<dl_setting MAX="1." MIN="0." STEP="1" VAR="z_constrast_mode"/>
<dl_setting MAX="1." MIN="0." STEP="1" VAR="rate_mode"/>
<dl_setting MAX="2" MIN="0" STEP="1" VAR="pprz_mode"/>
<dl_setting MAX="1" MIN="0" STEP="1" VAR="launch"/>
</dl_settings>
<dl_settings NAME="control">
<dl_settings NAME="attitude">
@@ -15,11 +16,17 @@
<dl_setting MAX="0.5" MIN="-0.3" STEP="0.01" VAR="ir_pitch_neutral"/>
<dl_setting MAX="15000" MIN="1000" STEP="250" VAR="roll_pgain"/>
<dl_setting MAX="15000" MIN="1000" STEP="250" VAR="pitch_pgain"/>
<dl_setting MAX="1" MIN="0" STEP="0.01" VAR="pitch_of_roll"/>
<dl_setting MAX="1" MIN="0" STEP="0.01" VAR="pitch_of_vz_pgain"/>
</dl_settings>
<dl_settings NAME="nav">
<dl_settings name="nav">
<dl_setting MAX="0.7" MIN="0.3" STEP="0.05" VAR="climb_level_gaz"/>
<dl_setting MAX="-0.01" MIN="-0.5" STEP="0.01" VAR="climb_pgain"/>
<dl_setting MAX="-0.01" MIN="-0.1" STEP="0.01" VAR="climb_pgain"/>
<dl_setting MAX="0" MIN="-0.1" STEP="0.01" VAR="altitude_pgain"/>
<dl_setting MAX="1" MIN="0.05" STEP="0.05" VAR="climb_igain"/>
<dl_setting MAX="-0.1" MIN="-2" STEP="0.05" VAR="course_pgain"/>
<dl_setting MAX="-0.01" MIN="-0.1" STEP="0.01" VAR="climb_pitch_pgain"/>
<dl_setting MAX="0.1" MIN="0" STEP="0.01" VAR="climb_pitch_igain"/>
</dl_settings>
</dl_settings>
</dl_settings>
@@ -47,7 +54,7 @@
<deroute BLOCK="circlehome"/>
</block>
<block NAME="circlehome">
<circle RADIUS="-50" WP="HOME" wp_dist="50" wp_qdr="0"/>
<circle RADIUS="50" WP="HOME" wp_dist="50" wp_qdr="0"/>
</block>
<block NAME="ABCDEFAFEDCBA">
<go approaching_time="1" wp="A"/>
@@ -64,10 +71,11 @@
<go approaching_time="1" from="D" hmode="route" wp="C"/>
<go approaching_time="1" from="C" hmode="route" wp="B"/>
<go approaching_time="1" from="B" hmode="route" wp="A"/>
<deroute block="circlehome"/>
</block>
<block name="Detection">
<go approaching_time="1" from="SW" wp="SE"/>
<go approaching_time="1" from="NE" wp="NW"/>
<go approaching_time="1" from="SW" hmode="route" wp="SE"/>
<go approaching_time="1" from="NE" hmode="route" wp="NW"/>
</block>
</blocks>
</flight_plan>
conf/flight_plans/generic.xml
+11 -2
View File
@@ -1,4 +1,4 @@
<flight_plan ALT="450" GROUND_ALT="400" LAT0="43.4624" LON0="1.2727" MAX_DIST_FROM_HOME="500" NAME="Generic" QFU="270" SECURITY_HEIGHT="25">
<flight_plan ALT="450" GROUND_ALT="400" LAT0="43.4624" LON0="1.2727" MAX_DIST_FROM_HOME="1000" NAME="Generic" QFU="270" SECURITY_HEIGHT="25">
<dl_settings>
<dl_settings NAME="flight params">
<dl_setting MAX="2" MIN="0" STEP="1" VAR="light_mode"/>
@@ -17,18 +17,24 @@
<dl_setting MAX="15000" MIN="1000" STEP="250" VAR="roll_pgain"/>
<dl_setting MAX="15000" MIN="1000" STEP="250" VAR="pitch_pgain"/>
<dl_setting MAX="1" MIN="0" STEP="0.01" VAR="pitch_of_roll"/>
<dl_setting MAX="1" MIN="0" STEP="0.01" VAR="pitch_of_vz"/>
<dl_setting MAX="1" MIN="0" STEP="0.01" VAR="pitch_of_vz_pgain"/>
</dl_settings>
<!--
<dl_settings name="rate_loop">
<dl_setting MAX="2000." MIN="0." STEP="1.0" VAR="roll_rate_pgain"/>
<dl_setting MAX="100" MIN="10" STEP="1" VAR="attitude_pgain"/>
</dl_settings>
-->
<dl_settings name="nav">
<dl_setting MAX="0.7" MIN="0.3" STEP="0.05" VAR="climb_level_gaz"/>
<dl_setting MAX="-0.01" MIN="-0.1" STEP="0.01" VAR="climb_pgain"/>
<dl_setting MAX="0" MIN="-0.1" STEP="0.01" VAR="altitude_pgain"/>
<dl_setting MAX="1" MIN="0.05" STEP="0.05" VAR="climb_igain"/>
<dl_setting MAX="-0.1" MIN="-2" STEP="0.05" VAR="course_pgain"/>
<dl_setting MAX="-0.01" MIN="-0.1" STEP="0.01" VAR="climb_pitch_pgain"/>
<dl_setting MAX="0.1" MIN="0" STEP="0.01" VAR="climb_pitch_igain"/>
</dl_settings>
</dl_settings>
</dl_settings>
@@ -52,6 +58,9 @@
<block NAME="circlehome">
<circle RADIUS="75" WP="HOME"/>
</block>
<block NAME="circle left home">
<circle RADIUS="-75" WP="HOME"/>
</block>
<block NAME="circlehome pitch">
<circle RADIUS="75" WP="HOME" gaz="0.6" pitch="auto"/>
</block>
conf/messages.xml
+1
View File
@@ -96,6 +96,7 @@
<field name="climb_sum_err" type="float" format="%.1f"></field>
<field name="climb_pgain" type="float" format="%.3f"></field>
<field name="course_pgain" type="float" format="%.3f"></field>
<field name="climb_gaz_submode" type="uint8"></field>
</message>
<message name="SETTINGS" ID="15">
sw/airborne/ap_downlink.h
+1 -1
View File
@@ -126,7 +126,7 @@
#define PERIODIC_SEND_GYRO_RATES() {}
#endif
#define PERIODIC_SEND_CALIBRATION() DOWNLINK_SEND_CALIBRATION(&climb_sum_err, &climb_pgain, &course_pgain)
#define PERIODIC_SEND_CALIBRATION() DOWNLINK_SEND_CALIBRATION(&climb_sum_err, &climb_pgain, &course_pgain, &climb_gaz_submode)
#define PERIODIC_SEND_CIRCLE() if (in_circle) { DOWNLINK_SEND_CIRCLE(&circle_x, &circle_y, &circle_radius); }
sw/airborne/autopilot.h
+2
View File
@@ -100,6 +100,8 @@ extern bool_t launch;
extern uint8_t light_mode;
extern bool_t gps_lost;
extern bool_t sum_err_reset;
/** Assignment, returning _old_value != _value
* Using GCC expression statements */
#define ModeUpdate(_mode, _value) ({ \
sw/airborne/gps_ubx.c
+1 -3
View File
@@ -108,9 +108,7 @@ void gps_init( void ) {
#define NAV_DYN_AIRBORNE_4G 7
void gps_configure ( void ) {
// UbxSend_CFG_PRT(0x01, 0x00, 0x0000, 0x000080C0, 0x00009600, UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0000, 0x0000);
/* remember to change host baudrate on ack */
UbxSend_CFG_PRT(0x01, 0x00, 0x0000, 0x000080C0, 19200, UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0000, 0x0000);
UbxSend_CFG_NAV(NAV_DYN_AIRBORNE_2G, 3, 16, 24, 20, 5, 0, 0x3C, 0x3C, 0x14, 0x03E8 ,0x0000, 0x0, 0x17, 0x00FA, 0x00FA, 0x0064, 0x012C, 0x000F, 0x00, 0x00);
UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_POSUTM_ID, 0, 1, 0, 0);
UbxSend_CFG_MSG(UBX_NAV_ID, UBX_NAV_VELNED_ID, 0, 1, 0, 0);
sw/airborne/nav.c
+60 -47
View File
@@ -314,10 +314,13 @@ extern float nav_altitude;
#define CLIMB_MODE_GAZ 0
#define CLIMB_MODE_PITCH 1
#define CLIMB_MODE_AGRESSIVE 2
#define CLIMB_MODE_BLENDED 3
#define CLIMB_MODE_GAZ_STANDARD 0
#define CLIMB_MODE_GAZ_AGRESSIVE 1
#define CLIMB_MODE_GAZ_BLENDED 2
uint8_t climb_mode = CLIMB_MODE_GAZ;
uint8_t climb_gaz_submode = CLIMB_MODE_GAZ_STANDARD;
#include "flight_plan.h"
@@ -537,7 +540,7 @@ void course_pid_run( void ) {
speed_depend_nav = Min(speed_depend_nav,1.5);
float roll_from_err = course_pgain * speed_depend_nav * err;
#if defined AGR_CLIMB_GAZ
if (climb_mode == CLIMB_MODE_AGRESSIVE) {
if (climb_gaz_submode == CLIMB_MODE_GAZ_AGRESSIVE) {
if (altitude_error < 0)
roll_from_err *= AGR_CLIMB_NAV_RATIO;
else
@@ -557,7 +560,7 @@ void course_pid_run( void ) {
float climb_pgain = CLIMB_PGAIN;
float climb_igain = CLIMB_IGAIN;
float desired_climb = 0., pre_climb = 0.;
float climb_sum_err = 0;
float climb_sum_err = 0.;
float climb_pitch_pgain = CLIMB_PITCH_PGAIN;
float climb_pitch_igain = CLIMB_PITCH_IGAIN;
@@ -574,45 +577,53 @@ climb_pid_run ( void ) {
pitch_of_vz = desired_climb * pitch_of_vz_pgain;
switch (climb_mode) {
case CLIMB_MODE_GAZ:
fgaz = controlled_gaz;
climb_sum_err += err;
Bound(climb_sum_err, -MAX_CLIMB_SUM_ERR, MAX_CLIMB_SUM_ERR);
nav_pitch += pitch_of_vz;
break;
#if defined AGR_CLIMB_GAZ
switch (climb_gaz_submode) {
case CLIMB_MODE_GAZ_AGRESSIVE:
if (altitude_error < 0) { /* Climbing */
fgaz = AGR_CLIMB_GAZ;
nav_pitch = AGR_CLIMB_PITCH;
} else { /* Going down */
fgaz = AGR_DESCENT_GAZ;
nav_pitch = AGR_DESCENT_PITCH;
}
break;
case CLIMB_MODE_GAZ_BLENDED: {
float ratio = (fabs(altitude_error) - AGR_BLEND_END) / (AGR_BLEND_START-AGR_BLEND_END);
if (altitude_error < 0) {
fgaz = ratio*AGR_CLIMB_GAZ + (1-ratio)*controlled_gaz;
nav_pitch = ratio*AGR_CLIMB_PITCH + (1-ratio)*pitch_of_vz;
} else {
fgaz = ratio*AGR_DESCENT_GAZ + (1-ratio)*controlled_gaz;
nav_pitch = ratio*AGR_DESCENT_PITCH + (1-ratio)*pitch_of_vz;
}
break;
}
case CLIMB_MODE_GAZ_STANDARD:
#endif
fgaz = controlled_gaz;
climb_sum_err += err;
Bound(climb_sum_err, -MAX_CLIMB_SUM_ERR, MAX_CLIMB_SUM_ERR);
nav_pitch += pitch_of_vz;
break;
#if defined AGR_CLIMB_GAZ
} /* switch submode */
#endif
desired_gaz = TRIM_UPPRZ(fgaz * MAX_PPRZ);
return;
case CLIMB_MODE_PITCH:
fgaz = nav_desired_gaz;
desired_gaz = nav_desired_gaz;
nav_pitch = climb_pitch_pgain * (err + climb_pitch_igain * climb_pitch_sum_err);
Bound(nav_pitch, MIN_PITCH, MAX_PITCH);
climb_pitch_sum_err += err;
BoundAbs(climb_pitch_sum_err, MAX_PITCH_CLIMB_SUM_ERR);
break;
#if defined AGR_CLIMB_GAZ
case CLIMB_MODE_AGRESSIVE:
if (altitude_error < 0) {
fgaz = AGR_CLIMB_GAZ;
nav_pitch = AGR_CLIMB_PITCH;
} else {
fgaz = AGR_DESCENT_GAZ;
nav_pitch = AGR_DESCENT_PITCH;
}
break;
case CLIMB_MODE_BLENDED: {
float ratio = (fabs(altitude_error) - AGR_BLEND_END) / (AGR_BLEND_START-AGR_BLEND_END);
if (altitude_error < 0) {
fgaz = ratio*AGR_CLIMB_GAZ + (1-ratio)*controlled_gaz;
nav_pitch = ratio*AGR_CLIMB_PITCH + (1-ratio)*pitch_of_vz;
} else {
fgaz = ratio*AGR_DESCENT_GAZ + (1-ratio)*controlled_gaz;
nav_pitch = ratio*AGR_DESCENT_PITCH + (1-ratio)*pitch_of_vz;
}
break;
return;
}
#endif
}
desired_gaz = TRIM_UPPRZ(fgaz * MAX_PPRZ);
}
float altitude_pgain = ALTITUDE_PGAIN;
@@ -621,17 +632,19 @@ float altitude_pgain = ALTITUDE_PGAIN;
void altitude_pid_run(void) {
desired_altitude = nav_altitude + altitude_shift;
altitude_error = estimator_z - desired_altitude;
desired_climb = pre_climb + altitude_pgain * altitude_error;
Bound(desired_climb, -CLIMB_MAX, CLIMB_MAX);
#ifdef AGR_CLIMB_GAZ
float dist = fabs(altitude_error);
if (climb_mode == CLIMB_MODE_PITCH || dist < AGR_BLEND_END) {
#endif
desired_climb = pre_climb + altitude_pgain * altitude_error;
Bound(desired_climb, -CLIMB_MAX, CLIMB_MAX);
#ifdef AGR_CLIMB_GAZ
} else if (dist > AGR_BLEND_START)
climb_mode = CLIMB_MODE_AGRESSIVE;
else
climb_mode = CLIMB_MODE_BLENDED;
if (climb_mode == CLIMB_MODE_GAZ) {
float dist = fabs(altitude_error);
if (dist < AGR_BLEND_END) {
climb_gaz_submode = CLIMB_MODE_GAZ_STANDARD;
} else if (dist > AGR_BLEND_START)
climb_gaz_submode = CLIMB_MODE_GAZ_AGRESSIVE;
else
climb_gaz_submode = CLIMB_MODE_GAZ_BLENDED;
}
#endif
}
sw/airborne/nav.h
+4
View File
@@ -79,6 +79,7 @@ extern int16_t circle_x, circle_y, circle_radius;
extern int16_t segment_x_1, segment_y_1, segment_x_2, segment_y_2;
extern uint8_t horizontal_mode;
extern uint8_t climb_gaz_submode;
#define HORIZONTAL_MODE_WAYPOINT 0
#define HORIZONTAL_MODE_ROUTE 1
@@ -101,6 +102,9 @@ extern float climb_pgain;
extern float climb_igain;
extern float climb_sum_err;
extern float desired_climb, pre_climb;
extern float altitude_pgain;
extern float climb_pitch_pgain;
extern float climb_pitch_igain;
extern float pitch_of_vz_pgain;
extern float pitch_of_vz;
sw/tools/gen_flight_plan.ml
+1
View File
@@ -165,6 +165,7 @@ let output_vmode x wp last_wp =
lprintf "nav_desired_gaz = %f;\n" (pprz_gaz (parsed_attrib x "gaz"))
end else begin
lprintf "nav_pitch = %s;\n" (parse pitch);
lprintf "climb_mode = CLIMB_MODE_GAZ;\n";
end;
let vmode = try ExtXml.attrib x "vmode" with _ -> "alt" in
begin