diff --git a/conf/airframes/microjet5.xml b/conf/airframes/microjet5.xml index ecb2148137..88e911f72e 100644 --- a/conf/airframes/microjet5.xml +++ b/conf/airframes/microjet5.xml @@ -62,7 +62,7 @@ - +
@@ -90,7 +90,7 @@
- + @@ -98,19 +98,19 @@
- - + + - +
- +
@@ -121,12 +121,12 @@
- + - - - + + +
diff --git a/conf/airframes/twinjet1.xml b/conf/airframes/twinjet1.xml index c580506d70..dd404642f1 100644 --- a/conf/airframes/twinjet1.xml +++ b/conf/airframes/twinjet1.xml @@ -51,7 +51,7 @@ - + @@ -103,7 +103,7 @@
- +
diff --git a/conf/flight_plans/EMAV2006.xml b/conf/flight_plans/EMAV2006.xml index a18c15f5c9..e014fcc4fa 100644 --- a/conf/flight_plans/EMAV2006.xml +++ b/conf/flight_plans/EMAV2006.xml @@ -8,6 +8,7 @@ + @@ -15,11 +16,17 @@ + + - + - + + + + + @@ -47,7 +54,7 @@ - + @@ -64,10 +71,11 @@ + - - + + diff --git a/conf/flight_plans/generic.xml b/conf/flight_plans/generic.xml index 4f2495947a..6a08fabfa2 100644 --- a/conf/flight_plans/generic.xml +++ b/conf/flight_plans/generic.xml @@ -1,4 +1,4 @@ - + @@ -17,18 +17,24 @@ - + + + + + + @@ -52,6 +58,9 @@ + + + diff --git a/conf/messages.xml b/conf/messages.xml index 0d028fe01d..b3165c88c7 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -96,6 +96,7 @@ + diff --git a/sw/airborne/ap_downlink.h b/sw/airborne/ap_downlink.h index c5e3b3d205..850895143f 100644 --- a/sw/airborne/ap_downlink.h +++ b/sw/airborne/ap_downlink.h @@ -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); } diff --git a/sw/airborne/autopilot.h b/sw/airborne/autopilot.h index dec24926a6..c7ebe04954 100644 --- a/sw/airborne/autopilot.h +++ b/sw/airborne/autopilot.h @@ -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) ({ \ diff --git a/sw/airborne/gps_ubx.c b/sw/airborne/gps_ubx.c index 08398d1303..9ae2691e39 100644 --- a/sw/airborne/gps_ubx.c +++ b/sw/airborne/gps_ubx.c @@ -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); diff --git a/sw/airborne/nav.c b/sw/airborne/nav.c index 5a1dfd1dac..f8b825f256 100644 --- a/sw/airborne/nav.c +++ b/sw/airborne/nav.c @@ -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 } diff --git a/sw/airborne/nav.h b/sw/airborne/nav.h index 8ab59462ce..900305a432 100644 --- a/sw/airborne/nav.h +++ b/sw/airborne/nav.h @@ -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; diff --git a/sw/tools/gen_flight_plan.ml b/sw/tools/gen_flight_plan.ml index 8c4771969c..c4c8ac028d 100644 --- a/sw/tools/gen_flight_plan.ml +++ b/sw/tools/gen_flight_plan.ml @@ -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