diff --git a/Makefile b/Makefile index 1f50689c5d..faf553d497 100644 --- a/Makefile +++ b/Makefile @@ -189,13 +189,14 @@ help: test_all_example_airframes: - make AIRCRAFT=Tux clean_ac ac + make AIRCRAFT=TJ1 clean_ac ap + make AIRCRAFT=MJ4 clean_ac ap + make AIRCRAFT=Slayer clean_ac ac make AIRCRAFT=Plaster clean_ac sim ac + make AIRCRAFT=Twin4 clean_ac ac + make AIRCRAFT=Tux clean_ac ac make AIRCRAFT=Twin1 clean_ac sim ac make AIRCRAFT=Twin2 clean_ac sim make AIRCRAFT=MJ1 clean_ac ac - make AIRCRAFT=TJ1 clean_ac ap - make AIRCRAFT=MJ4 clean_ac ap make AIRCRAFT=GRZE3 clean_ac ap - make AIRCRAFT=Twin4 clean_ac ac make AIRCRAFT=TS5 clean_ac ac diff --git a/conf/airframes/microjet4.xml b/conf/airframes/microjet4.xml index 7fb0a551c6..25a88e1818 100644 --- a/conf/airframes/microjet4.xml +++ b/conf/airframes/microjet4.xml @@ -39,11 +39,11 @@
- - - + + + - +
@@ -53,6 +53,7 @@ + diff --git a/conf/airframes/plaster1.xml b/conf/airframes/plaster1.xml index 5cc5a127c8..d920dd8bbe 100755 --- a/conf/airframes/plaster1.xml +++ b/conf/airframes/plaster1.xml @@ -118,11 +118,25 @@
+
+ + + + + + + + +
+ + include $(PAPARAZZI_SRC)/conf/autopilot/v1_2_1.makefile include $(PAPARAZZI_SRC)/conf/autopilot/twin_mcu_avr.makefile +fbw.CFLAGS += -DTELEMETRY_MODE_FBW=1 + ap.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=XBeeTransport ap.srcs += downlink.c diff --git a/conf/airframes/slayer1.xml b/conf/airframes/slayer1.xml index 86c48e52f8..123c006142 100644 --- a/conf/airframes/slayer1.xml +++ b/conf/airframes/slayer1.xml @@ -39,13 +39,13 @@
- - - + + + - - - + + +
@@ -54,24 +54,25 @@ - - + + + - - + +
- - + +
- - + +
@@ -113,6 +114,28 @@ + +
+ + + + + + + + +
+ +
+ + + + + + + +
+ + +
@@ -112,6 +132,19 @@
+ +
+ + + + + + + + +
+ + include $(PAPARAZZI_SRC)/conf/autopilot/classix.makefile @@ -148,12 +181,19 @@ ap.srcs += inter_mcu.c link_mcu.c spi.c $(SRC_ARCH)/spi_hw.c ap.CFLAGS += -DGPS -DUBX -DUSE_UART1 -DGPS_LINK=Uart1 -DUART1_BAUD=B38400 ap.srcs += gps_ubx.c gps.c +# ADCs for infrared ap.CFLAGS += -DADC -DUSE_AD0 -DUSE_AD0_1 -DUSE_AD0_2 -DUSE_AD0_3 +# ADCs for gyro +ap.CFLAGS += -DUSE_AD1 -DUSE_AD1_2 -DUSE_AD1_3 ap.srcs += $(SRC_ARCH)/adc_hw.c + ap.CFLAGS += -DINFRARED ap.srcs += infrared.c estimator.c +ap.CFLAGS += -DGYRO -DATTITUDE_RATE_MODE -DSPARK_FUN +ap.srcs += gyro.c + ap.srcs += nav.c pid.c test.CFLAGS += -DFBW -DCONFIG=\"classix.h\" -DLED -DTIME_LED=1 -DACTUATORS=\"servos_4017_hw.h\" -DSERVOS_4017 -DSERVOS_4017_CLOCK_FALLING -DUSE_UART0 -DDATALINK -DPPRZ_INPUT -DPPRZ_UART=Uart0 diff --git a/conf/airframes/twinstar1.xml b/conf/airframes/twinstar1.xml index e3403a0d04..5003ab2fa6 100644 --- a/conf/airframes/twinstar1.xml +++ b/conf/airframes/twinstar1.xml @@ -61,7 +61,7 @@ - + diff --git a/conf/airframes/twinstar2.xml b/conf/airframes/twinstar2.xml index 5966f4cebd..848e6f497c 100644 --- a/conf/airframes/twinstar2.xml +++ b/conf/airframes/twinstar2.xml @@ -64,7 +64,7 @@ - + diff --git a/conf/autopilot/tiny.h b/conf/autopilot/tiny.h index 89c8c5691f..d105dc6dd3 100644 --- a/conf/autopilot/tiny.h +++ b/conf/autopilot/tiny.h @@ -51,4 +51,14 @@ #define PPM_PINSEL_VAL 0x02 #define PPM_PINSEL_BIT 12 +/* ADC */ +#define ADC_0 AdcBank0(3) +#define ADC_1 AdcBank0(2) +#define ADC_2 AdcBank0(1) +#define ADC_3 AdcBank1(7) +#define ADC_4 AdcBank1(3) +#define ADC_5 AdcBank1(4) +#define ADC_6 AdcBank1(5) +#define ADC_7 AdcBank1(2) + #endif /* CONFIG_TINY_H */ diff --git a/conf/flight_plans/mav05_ccw.xml b/conf/flight_plans/mav05_ccw.xml index 145b63b648..129086613e 100644 --- a/conf/flight_plans/mav05_ccw.xml +++ b/conf/flight_plans/mav05_ccw.xml @@ -16,6 +16,8 @@ + + @@ -31,7 +33,7 @@ - + diff --git a/conf/flight_plans/mav05_cw.xml b/conf/flight_plans/mav05_cw.xml index ccd9885de7..e8bb303ced 100644 --- a/conf/flight_plans/mav05_cw.xml +++ b/conf/flight_plans/mav05_cw.xml @@ -11,6 +11,15 @@ + + + + + + + + + diff --git a/conf/flight_plans/sectors.xml b/conf/flight_plans/sectors.xml index 46c85d1246..bb20161300 100644 --- a/conf/flight_plans/sectors.xml +++ b/conf/flight_plans/sectors.xml @@ -21,4 +21,13 @@ + + + + + + + + + diff --git a/conf/messages.xml b/conf/messages.xml index c389db47b3..c607fe6ceb 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -232,6 +232,11 @@ + + + + + diff --git a/conf/telemetry/default.xml b/conf/telemetry/default.xml index ae1312afa9..38f118d49c 100644 --- a/conf/telemetry/default.xml +++ b/conf/telemetry/default.xml @@ -22,6 +22,7 @@ + diff --git a/sw/airborne/adc.h b/sw/airborne/adc.h index 5f72839093..c095c79a8c 100644 --- a/sw/airborne/adc.h +++ b/sw/airborne/adc.h @@ -35,6 +35,7 @@ #define _ADC_H_ #include +#include "adc_hw.h" #define NB_ADC 8 #define MAX_AV_NB_SAMPLE 0x20 diff --git a/sw/airborne/ap_downlink.h b/sw/airborne/ap_downlink.h index d9c2911004..aa1e68f053 100644 --- a/sw/airborne/ap_downlink.h +++ b/sw/airborne/ap_downlink.h @@ -95,18 +95,27 @@ #ifdef INFRARED #define PERIODIC_SEND_IR_SENSORS() DOWNLINK_SEND_IR_SENSORS(&ir_pitch, &ir_roll, &ir_top); -#define PERIODIC_SEND_ADC() {} #define PERIODIC_SEND_RAD_OF_IR() Downlink({ int16_t rad = DeciDegOfRad(estimator_rad); DOWNLINK_SEND_RAD_OF_IR(&ir_roll, &rad, &estimator_rad_of_ir);}) #define PERIODIC_SEND_CALIB_START() if (!estimator_flight_time && calib_status == WAITING_CALIB_CONTRAST) { DOWNLINK_SEND_CALIB_START(); } #define PERIODIC_SEND_CALIB_CONTRAST() if (!estimator_flight_time && calib_status == CALIB_DONE) { DOWNLINK_SEND_CALIB_CONTRAST(&ir_contrast); } #else -#define PERIODIC_SEND_ADC() {} #define SEND_RAD_OF_IR() {} #define PERIODIC_SEND_CALIB_START() {} #define PERIODIC_SEND_CALIB_CONTRAST() {} #endif +#define PERIODIC_SEND_ADC() {} + +#ifdef IDC300 +#include "gyro.h" +#define PERIODIC_SEND_GYRO_RATES() DOWNLINK_SEND_GYRO_RATES(&roll_rate, &pitch_rate) +#elif defined SPARK_FUN +#define PERIODIC_SEND_GYRO_RATES() DOWNLINK_SEND_GYRO_RATES(&roll_rate, &temp_comp) +#else +#define PERIODIC_SEND_GYRO_RATES() {} +#endif + #define PERIODIC_SEND_CALIBRATION() DOWNLINK_SEND_CALIBRATION(&climb_sum_err, &climb_pgain, &course_pgain) #define PERIODIC_SEND_CIRCLE() if (in_circle) { DOWNLINK_SEND_CIRCLE(&circle_x, &circle_y, &circle_radius); } diff --git a/sw/airborne/arm7/adc_hw.c b/sw/airborne/arm7/adc_hw.c index 63f7f59541..33ba51e051 100644 --- a/sw/airborne/arm7/adc_hw.c +++ b/sw/airborne/arm7/adc_hw.c @@ -4,7 +4,8 @@ #include "armVIC.h" #include CONFIG -static struct adc_buf* buffers[NB_ADC]; +/** First NB_ADC for bank 0, others for bank 2 */ +static struct adc_buf* buffers[NB_ADC*2]; volatile uint16_t adc0_val[NB_ADC] = {1, 2, 3, 4, 5, 6, 7, 8}; volatile uint16_t adc1_val[NB_ADC] = {9, 10, 11, 12, 13, 14, 15, 16}; @@ -211,7 +212,7 @@ void adcISR1 ( void ) { uint16_t value = (uint16_t)(tmp >> 6) & 0x03FF; adc1_val[channel] = value; LED_TOGGLE(2); - struct adc_buf* buf = buffers[channel]; + struct adc_buf* buf = buffers[channel+NB_ADC]; if (buf) { uint8_t new_head = buf->head + 1; if (new_head >= buf->av_nb_sample) new_head = 0; diff --git a/sw/airborne/arm7/adc_hw.h b/sw/airborne/arm7/adc_hw.h new file mode 100644 index 0000000000..ede5818dd2 --- /dev/null +++ b/sw/airborne/arm7/adc_hw.h @@ -0,0 +1,2 @@ +#define AdcBank0(x) (x) +#define AdcBank1(x) (x+NB_ADC) diff --git a/sw/airborne/autopilot.h b/sw/airborne/autopilot.h index 95cab96d94..7793c95686 100644 --- a/sw/airborne/autopilot.h +++ b/sw/airborne/autopilot.h @@ -88,7 +88,6 @@ extern uint8_t fatal_error_nb; #define GAZ_THRESHOLD_TAKEOFF (pprz_t)(MAX_PPRZ * 0.9) extern uint8_t vertical_mode; -extern bool_t auto_pitch; extern uint8_t lateral_mode; extern uint8_t vsupply; diff --git a/sw/airborne/avr/adc_hw.h b/sw/airborne/avr/adc_hw.h new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sw/airborne/infrared.c b/sw/airborne/infrared.c index a63e9200cf..6d77b57765 100644 --- a/sw/airborne/infrared.c +++ b/sw/airborne/infrared.c @@ -40,6 +40,8 @@ int16_t ir_roll; int16_t ir_pitch; int16_t ir_top; +float z_constrast_mode; + /** Initialized to \a IR_DEFAULT_CONTRAST. Changed with calibration */ int16_t ir_contrast = IR_DEFAULT_CONTRAST; /** Initialized to \a IR_DEFAULT_CONTRAST. @@ -92,7 +94,7 @@ void ir_update(void) { ir_roll = IR_RollOfIrs(x1_mean, x2_mean); ir_pitch = IR_PitchOfIrs(x1_mean, x2_mean); #ifdef ADC_CHANNEL_IR_TOP - ir_top = buf_ir_top.sum/buf_ir_top.av_nb_sample - IR_ADC_TOP_NEUTRAL; + ir_top = IR_TopOfIr(buf_ir_top.sum/buf_ir_top.av_nb_sample - IR_ADC_TOP_NEUTRAL); #endif /** neutrals are not taken into account in SITL and HITL */ @@ -199,7 +201,11 @@ void estimator_update_state_infrared( void ) { float rad_of_ir = (ir_estim_mode == IR_ESTIM_MODE_ON ? estimator_rad_of_ir : ir_rad_of_ir); - - estimator_phi = rad_of_ir * ir_roll - ir_roll_neutral; - estimator_theta = rad_of_ir * ir_pitch - ir_pitch_neutral; +#ifndef ADC_CHANNEL_IR_TOP + z_constrast_mode = 0; +#endif + ir_top = Max(ir_top, 1); + float c = rad_of_ir*(1-z_constrast_mode)+z_constrast_mode*(IR_RAD_OF_IR_CONTRAST/ir_top); + estimator_phi = c * ir_roll - ir_roll_neutral; + estimator_theta = c * ir_pitch - ir_pitch_neutral; } diff --git a/sw/airborne/infrared.h b/sw/airborne/infrared.h index d2430a8bd2..0f0bc19312 100644 --- a/sw/airborne/infrared.h +++ b/sw/airborne/infrared.h @@ -46,6 +46,7 @@ extern int16_t ir_top; /* averaged vertical ir adc */ extern float ir_rad_of_ir; extern int16_t ir_contrast; +extern float z_constrast_mode; /** Status of the calibration. Can be one of the \a calibration \a states */ extern uint8_t calib_status; diff --git a/sw/airborne/main_ap.c b/sw/airborne/main_ap.c index 577545cf79..cf165fbfeb 100644 --- a/sw/airborne/main_ap.c +++ b/sw/airborne/main_ap.c @@ -39,6 +39,7 @@ #include "pid.h" #include "gps.h" #include "infrared.h" +#include "gyro.h" #include "ap_downlink.h" #include "nav.h" #include "autopilot.h" @@ -82,8 +83,6 @@ uint8_t lateral_mode = LATERAL_MODE_MANUAL; uint8_t ir_estim_mode = IR_ESTIM_MODE_ON; -bool_t auto_pitch = FALSE; - bool_t rc_event_1, rc_event_2; uint8_t vsupply; @@ -119,7 +118,7 @@ static inline uint8_t pprz_mode_update( void ) { /** \fn inline uint8_t ir_estim_mode_update( void ) * \brief update ir estimation if RADIO_LLS is true \n */ -inline uint8_t ir_estim_mode_update( void ) { +static inline uint8_t ir_estim_mode_update( void ) { return ModeUpdate(ir_estim_mode, IR_ESTIM_MODE_OF_PULSE(fbw_state->channels[RADIO_LLS])); } #endif @@ -411,6 +410,9 @@ inline void periodic_task_ap( void ) { } switch (_20Hz) { case 0: +#if defined GYRO + gyro_update(); +#endif break; case 1: { static uint8_t odd; @@ -469,6 +471,9 @@ void init_ap( void ) { #ifdef INFRARED ir_init(); #endif +#ifdef GYRO + gyro_init(); +#endif #ifdef GPS gps_init(); #endif diff --git a/sw/airborne/main_ap_2.c b/sw/airborne/main_ap_2.c index 4eeadca3d3..0a87563b24 100644 --- a/sw/airborne/main_ap_2.c +++ b/sw/airborne/main_ap_2.c @@ -23,7 +23,6 @@ #include "airframe.h" float pitch_of_vz; float pitch_of_vz_pgain; -bool_t auto_pitch = FALSE; uint8_t ac_ident = AC_ID; float desired_roll = 0.; float desired_pitch = 0.; diff --git a/sw/airborne/nav.c b/sw/airborne/nav.c index c297857f0c..5295666e7c 100644 --- a/sw/airborne/nav.c +++ b/sw/airborne/nav.c @@ -40,6 +40,10 @@ #include "cam.h" #include "traffic_info.h" +#define Distance2(p1_x, p1_y, p2_x, p2_y) ((p1_x-p2_x)*(p1_x-p2_x)+(p1_y-p2_y)*(p1_y-p2_y)) +#define Square(_x) ((_x)*(_x)) +#define G 9.81 + uint8_t nav_stage, nav_block; /** To save the current stage when an exception is raised */ uint8_t excpt_stage; @@ -65,6 +69,9 @@ bool_t in_segment = FALSE; int16_t circle_x, circle_y, circle_radius; int16_t segment_x_1, segment_y_1, segment_x_2, segment_y_2; uint8_t horizontal_mode; +float circle_bank = 0; +float nav_prebank, nav_speed_depend; + #define RcRoll(travel) (fbw_state->channels[RADIO_ROLL]* (float)travel /(float)MAX_PPRZ) @@ -115,12 +122,27 @@ static float qdr; sum_alpha += alpha_diff; \ } else \ new_circle = FALSE; \ + float dist2_center = Distance2(estimator_x, estimator_y, x, y); \ + float dist_carrot = CARROT*estimator_hspeed_mod; \ + float abs_radius = fabs(radius); \ + float sign_radius = radius > 0 ? 1 : -1; \ + circle_bank = \ + (dist2_center > Square(abs_radius + dist_carrot) \ + || dist2_center < Square(abs_radius - dist_carrot)) ? \ + 0 : \ + sign_radius * atan2(estimator_hspeed_mod*estimator_hspeed_mod, \ + G*abs_radius); \ + circle_bank *= nav_prebank; \ circle_count = fabs(sum_alpha) / (2*M_PI); \ - float alpha_carrot = alpha + CARROT / -radius * estimator_hspeed_mod; \ + float carrot_angle = CARROT / abs_radius * estimator_hspeed_mod; \ + carrot_angle = Min(carrot_angle, M_PI/4); \ + carrot_angle = Max(carrot_angle, M_PI/16); \ + float alpha_carrot = alpha - sign_radius * carrot_angle; \ horizontal_mode = HORIZONTAL_MODE_CIRCLE; \ - fly_to_xy(x+cos(alpha_carrot)*fabs(radius), \ - y+sin(alpha_carrot)*fabs(radius)); \ - qdr = DegOfRad(M_PI/2 - alpha_carrot); \ + float radius_carrot = abs_radius + nav_prebank * (abs_radius / cos(carrot_angle) - abs_radius); \ + fly_to_xy(x+cos(alpha_carrot)*radius_carrot, \ + y+sin(alpha_carrot)*radius_carrot); \ + qdr = DegOfRad(M_PI/2 - alpha); \ NormCourse(qdr); \ in_circle = TRUE; \ circle_x = x; \ @@ -217,7 +239,7 @@ static inline void survey_init(float y_south, float y_north, float grid) { #define MIN_DIST2_WP (15.*15.) -#define DISTANCE2(p1_x, p1_y, p2) ((p1_x-p2.x)*(p1_x-p2.x)+(p1_y-p2.y)*(p1_y-p2.y)) +// #define DISTANCE2(p1_x, p1_y, p2) ((p1_x-p2.x)*(p1_x-p2.x)+(p1_y-p2.y)*(p1_y-p2.y)) int32_t nav_utm_east0 = NAV_UTM_EAST0; int32_t nav_utm_north0 = NAV_UTM_NORTH0; @@ -380,14 +402,12 @@ void nav_without_gps(void) { #ifdef SECTION_FAILSAFE lateral_mode = LATERAL_MODE_ROLL; nav_desired_roll = FAILSAFE_DEFAULT_ROLL; - auto_pitch = FALSE; nav_pitch = FAILSAFE_DEFAULT_PITCH; vertical_mode = VERTICAL_MODE_AUTO_GAZ; nav_desired_gaz = TRIM_UPPRZ((FAILSAFE_DEFAULT_GAZ)*MAX_PPRZ); #else lateral_mode = LATERAL_MODE_ROLL; nav_desired_roll = 0; - auto_pitch = FALSE; nav_pitch = 0; vertical_mode = VERTICAL_MODE_AUTO_GAZ; nav_desired_gaz = TRIM_UPPRZ((CLIMB_LEVEL_GAZ)*MAX_PPRZ); @@ -397,6 +417,15 @@ void nav_without_gps(void) { float course_pgain = COURSE_PGAIN; float desired_course = 0.; float max_roll = MAX_ROLL; +float altitude_error; + +#define CLIMB_MODE_GAZ 0 +#define CLIMB_MODE_PITCH 1 +#define CLIMB_MODE_AGRESSIVE 2 +#define CLIMB_MODE_BLENDED 3 + +uint8_t climb_mode = CLIMB_MODE_GAZ; + /** \brief Computes ::nav_desired_roll from course estimation and expected course. @@ -404,7 +433,20 @@ float max_roll = MAX_ROLL; void course_pid_run( void ) { float err = estimator_hspeed_dir - desired_course; NormRadAngle(err); - nav_desired_roll = course_pgain * err; + float speed_depend_nav = estimator_hspeed_mod/NOMINAL_AIRSPEED; + speed_depend_nav = Max(speed_depend_nav,0.66); + speed_depend_nav = Min(speed_depend_nav,1.5); + speed_depend_nav = 1. + nav_speed_depend * (speed_depend_nav-1.); + float roll_from_err = course_pgain * speed_depend_nav * err; +#if defined AGR_CLIMB_GAZ + if (climb_mode == CLIMB_MODE_AGRESSIVE) { + if (altitude_error < 0) + roll_from_err *= AGR_CLIMB_NAV_RATIO; + else + roll_from_err *= AGR_DESCENT_NAV_RATIO; + } +#endif + nav_desired_roll = (in_circle ? circle_bank : 0) + roll_from_err; if (nav_desired_roll > max_roll) nav_desired_roll = max_roll; else if (nav_desired_roll < -max_roll) @@ -427,31 +469,73 @@ float climb_level_gaz = CLIMB_LEVEL_GAZ; /** \brief Computes desired_gaz and updates nav_pitch from desired_climb */ void climb_pid_run ( void ) { + float fgaz = 0; float err = estimator_z_dot - desired_climb; - if (auto_pitch) { /* gaz constant */ - desired_gaz = nav_desired_gaz; + float controlled_gaz = climb_pgain * (err + climb_igain * climb_sum_err) + climb_level_gaz + CLIMB_GAZ_OF_CLIMB*desired_climb; + /* pitch offset for climb */ + pitch_of_vz = desired_climb * pitch_of_vz_pgain; + switch (climb_mode) { + case CLIMB_MODE_GAZ: + fgaz = controlled_gaz; + climb_sum_err += err; + if (climb_sum_err > MAX_CLIMB_SUM_ERR) climb_sum_err = MAX_CLIMB_SUM_ERR; + if (climb_sum_err < - MAX_CLIMB_SUM_ERR) climb_sum_err = - MAX_CLIMB_SUM_ERR; + nav_pitch += pitch_of_vz; + break; + + case CLIMB_MODE_PITCH: + fgaz = 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); - } else { /* pitch almost constant */ - /* pitch offset for climb */ - pitch_of_vz = (desired_climb > 0) ? desired_climb * pitch_of_vz_pgain : 0.; - float fgaz = climb_pgain * (err + climb_igain * climb_sum_err) + climb_level_gaz + CLIMB_GAZ_OF_CLIMB*desired_climb; - climb_sum_err += err; - if (climb_sum_err > MAX_CLIMB_SUM_ERR) climb_sum_err = MAX_CLIMB_SUM_ERR; - if (climb_sum_err < - MAX_CLIMB_SUM_ERR) climb_sum_err = - MAX_CLIMB_SUM_ERR; - desired_gaz = TRIM_UPPRZ(fgaz * MAX_PPRZ); - nav_pitch += pitch_of_vz; + 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; } +#endif + } + desired_gaz = TRIM_UPPRZ(fgaz * MAX_PPRZ); } float altitude_pgain = ALTITUDE_PGAIN; void altitude_pid_run(void) { - float err = estimator_z - desired_altitude; - desired_climb = pre_climb + altitude_pgain * err; - if (desired_climb < -CLIMB_MAX) desired_climb = -CLIMB_MAX; - if (desired_climb > CLIMB_MAX) desired_climb = CLIMB_MAX; + altitude_error = estimator_z - desired_altitude; + +#ifdef AGR_CLIMB_GAZ + float dist = fabs(altitude_error); + if (dist < AGR_BLEND_END) { +#endif + desired_climb = pre_climb + altitude_pgain * altitude_error; + if (desired_climb < -CLIMB_MAX) desired_climb = -CLIMB_MAX; + if (desired_climb > CLIMB_MAX) desired_climb = CLIMB_MAX; + climb_mode = CLIMB_MODE_GAZ; +#ifdef AGR_CLIMB_GAZ + } else if (dist > AGR_BLEND_START) + climb_mode = CLIMB_MODE_AGRESSIVE; + else + climb_mode = CLIMB_MODE_BLENDED; +#endif } diff --git a/sw/airborne/nav.h b/sw/airborne/nav.h index 03b193decc..7df18ee63e 100644 --- a/sw/airborne/nav.h +++ b/sw/airborne/nav.h @@ -104,6 +104,7 @@ extern float pitch_of_vz_pgain; extern float pitch_of_vz; extern float aileron_of_gaz; extern float climb_level_gaz; +extern float nav_prebank, nav_speed_depend; void climb_pid_run(void); void altitude_pid_run(void); diff --git a/sw/airborne/pid.c b/sw/airborne/pid.c index 8e6f88ff8a..510e4062c1 100644 --- a/sw/airborne/pid.c +++ b/sw/airborne/pid.c @@ -34,30 +34,95 @@ #include "autopilot.h" #include "infrared.h" +#include "gyro.h" #include "estimator.h" #include "nav.h" + float desired_roll = 0.; float desired_pitch = 0.; int16_t desired_gaz, desired_aileron, desired_elevator; float roll_pgain = ROLL_PGAIN; float pitch_pgain = PITCH_PGAIN; +#ifdef PITCH_IGAIN +float pitch_igain = PITCH_IGAIN; +#endif float pitch_of_roll = PITCH_OF_ROLL; +#ifdef ATTITUDE_RATE_MODE +float attitude_pgain = ATTITUDE_PGAIN; +float roll_rate_pgain = ROLL_RATE_PGAIN; +float roll_rate_dgain = ROLL_RATE_DGAIN; +float roll_rate_igain = ROLL_RATE_IGAIN; + +#ifdef PITCH_RATE_PGAIN +float pitch_rate_pgain = PITCH_RATE_PGAIN; +float pitch_rate_dgain = PITCH_RATE_DGAIN; +float pitch_rate_igain = PITCH_RATE_IGAIN; +#endif +#endif + float pitch_of_vz_pgain = CLIMB_PITCH_OF_VZ_PGAIN; float pitch_of_vz = 0.; float aileron_of_gaz = AILERON_OF_GAZ; +float rate_mode = 0; + /** \brief Computes ::desired_aileron and ::desired_elevator from attitude * estimation and expected attitude. */ void roll_pitch_pid_run( void ) { + /** Attitude PID */ float err = estimator_phi - desired_roll; - desired_aileron = TRIM_PPRZ(roll_pgain * err + desired_gaz * aileron_of_gaz); + Bound(err, -M_PI/2., M_PI/2); + + float std_desired_aileron = TRIM_PPRZ(roll_pgain * err + desired_gaz * aileron_of_gaz); +#ifndef ATTITUDE_RATE_MODE + desired_aileron = std_desired_aileron; +#else + float desired_roll_rate = -attitude_pgain*err; + Bound(desired_roll_rate, -GYRO_MAX_RATE, GYRO_MAX_RATE); + + /** Roll rate pid */ + err = roll_rate - desired_roll_rate; + Bound(err, -GYRO_MAX_RATE, GYRO_MAX_RATE); + + static float rollrate_sum_err = 0.; + static uint8_t rollratesum_idx = 0; + static float rollratesum_values[ROLLRATESUM_NB_SAMPLES]; + + rollrate_sum_err -= rollratesum_values[rollratesum_idx]; + rollratesum_values[rollratesum_idx] = err; + rollrate_sum_err += err; + rollratesum_idx++; + if (rollratesum_idx >= ROLLRATESUM_NB_SAMPLES) rollratesum_idx = 0; + + /* D term calculations */ + static float last_roll_rate; + float d_err = roll_rate - last_roll_rate; + last_roll_rate = roll_rate; + + float rate_desired_aileron = TRIM_PPRZ(roll_rate_pgain*(err + roll_rate_igain*(rollrate_sum_err/ROLLRATESUM_NB_SAMPLES) + roll_rate_dgain*d_err)); + + desired_aileron = rate_mode*rate_desired_aileron+(1-rate_mode)*std_desired_aileron; +#endif + + /** Pitch pi */ if (pitch_of_roll <0.) pitch_of_roll = 0.; - err = -(estimator_theta - desired_pitch - pitch_of_roll * fabs(estimator_phi)); + err = -(estimator_theta - desired_pitch - pitch_of_roll*fabs(estimator_phi)); + Bound(err, -M_PI/2, M_PI/2); + +#ifdef PITCH_IGAIN + pitch_sum_err -= pitchsum_values[pitchsum_idx]; + pitchsum_values[pitchsum_idx] = err; + pitch_sum_err += pitchsum_values[pitchsum_idx]; + pitchsum_idx++; + if (pitchsum_idx >= PITCHSUM_NB_SAMPLES) pitchsum_idx = 0; + desired_elevator = TRIM_PPRZ(pitch_pgain * thr_pitch_multi * (err + (pitch_sum_err/(pitchsum_nb_samples))*pitch_igain)); +#else desired_elevator = TRIM_PPRZ(pitch_pgain * err); +#endif } diff --git a/sw/airborne/pid.h b/sw/airborne/pid.h index 7a3a64e11e..d997928ae7 100644 --- a/sw/airborne/pid.h +++ b/sw/airborne/pid.h @@ -33,8 +33,11 @@ extern float desired_roll; extern float max_roll; extern float desired_pitch; extern float roll_pgain; +extern float roll_rate_pgain; +extern float attitude_pgain; extern float pitch_pgain; extern float pitch_of_roll; +extern float rate_mode; void roll_pitch_pid_run( void ); diff --git a/sw/airborne/sim/adc_hw.h b/sw/airborne/sim/adc_hw.h new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sw/ground_segment/cockpit/map2d.ml b/sw/ground_segment/cockpit/map2d.ml index 8dadc62d44..019faebb5a 100644 --- a/sw/ground_segment/cockpit/map2d.ml +++ b/sw/ground_segment/cockpit/map2d.ml @@ -906,6 +906,8 @@ let button_press = fun (geomap:G.widget) ev -> let label = GMisc.label ~text:name ~packing:eb#add () in eb#coerce#misc#modify_bg [`NORMAL, `NAME color;`ACTIVE, `NAME color]; (fp_notebook:GPack.notebook)#append_page ~tab_label:eb#coerce fp#window#coerce; + fp#connect_selection (fun _ -> prerr_endline "click"); + fp#hide (); ignore (reset_wp_menu#connect#activate (reset_waypoints fp)); Hashtbl.add live_aircrafts ac_id { track = track; color = color; diff --git a/sw/lib/ocaml/mapCanvas.ml b/sw/lib/ocaml/mapCanvas.ml index da30f41093..ae9c04f3a4 100644 --- a/sw/lib/ocaml/mapCanvas.ml +++ b/sw/lib/ocaml/mapCanvas.ml @@ -363,7 +363,7 @@ class basic_widget = fun ?(height=800) ?width ?(projection = Mercator) ?georef ( let scroll_event = GdkEvent.Scroll.cast ev in let (x, y) = canvas#get_scroll_offsets in let xr = GdkEvent.Scroll.x_root scroll_event in - let yr = GdkEvent.Scroll.y_root scroll_event -. 35. in + let yr = GdkEvent.Scroll.y_root scroll_event -. 50. in match GdkEvent.Scroll.direction scroll_event with `UP -> canvas#scroll_to (x+truncate xr) (y+truncate yr); diff --git a/sw/lib/ocaml/mapFP.ml b/sw/lib/ocaml/mapFP.ml index 06c48a4e87..2a9161d4b8 100644 --- a/sw/lib/ocaml/mapFP.ml +++ b/sw/lib/ocaml/mapFP.ml @@ -216,6 +216,8 @@ class flight_plan = fun ?edit geomap color fp_dtd xml -> ) path + method connect_selection = fun cb -> XmlEdit.connect_selection xml_tree_view cb + initializer ( (** Create a graphic waypoint when it is created from the xml editor *) XmlEdit.connect xml_wpts (function XmlEdit.New_child node -> ignore (create_wp node) | _ -> ()) diff --git a/sw/lib/ocaml/mapFP.mli b/sw/lib/ocaml/mapFP.mli index 217b1079e0..0f3ea97dda 100644 --- a/sw/lib/ocaml/mapFP.mli +++ b/sw/lib/ocaml/mapFP.mli @@ -43,6 +43,7 @@ class flight_plan : method xml : Xml.xml method insert_path : (MapWaypoints.waypoint * float) list -> unit method highlight_stage : int -> int -> unit + method connect_selection : (XmlEdit.node->unit) -> unit end (** Extracts [lat0] and [Lon0] attributes *) diff --git a/sw/lib/ocaml/xmlEdit.ml b/sw/lib/ocaml/xmlEdit.ml index 136f110481..462454787e 100644 --- a/sw/lib/ocaml/xmlEdit.ml +++ b/sw/lib/ocaml/xmlEdit.ml @@ -379,6 +379,12 @@ let connect = fun ((model, path):node) cb -> let current_cb = try model#get ~row ~column:event with _ -> fun _ -> () in model#set ~row ~column:event (fun e -> cb e; current_cb e) +let selection_cbs = Hashtbl.create 3 +let connect_selection = fun (model,_) cb -> + let l = try Hashtbl.find selection_cbs model with Not_found -> [] in + Hashtbl.replace selection_cbs model (cb::l) + + let expand_node = fun ?all (_, (tree_view:GTree.view)) ((model, path):node) -> tree_view#expand_row ?all path @@ -477,7 +483,8 @@ let create = fun ?(edit=true) dtd xml -> let attribs = tree_model#get ~row ~column:attributes in attribs_model#clear (); tag_of_last_selection := tree_model#get ~row ~column:tag_col; - set_attributes attribs_model attribs + set_attributes attribs_model attribs; + List.iter (fun cb -> cb (tree_model, path)) (Hashtbl.find selection_cbs tree_model) | _ -> () in let _c = tree_view#selection#connect#after#changed ~callback:selection_changed in diff --git a/sw/lib/ocaml/xmlEdit.mli b/sw/lib/ocaml/xmlEdit.mli index db86f84383..1d60054b16 100644 --- a/sw/lib/ocaml/xmlEdit.mli +++ b/sw/lib/ocaml/xmlEdit.mli @@ -65,6 +65,7 @@ val add_child : node -> tag -> attributes -> node (** Modifications *) val connect : node -> (event -> unit) -> unit +val connect_selection : t -> (node -> unit) -> unit (** To be kept informed about modifications *) val selection : t -> node diff --git a/sw/tools/gen_flight_plan.ml b/sw/tools/gen_flight_plan.ml index a8bb4b849d..e900fd80fc 100644 --- a/sw/tools/gen_flight_plan.ml +++ b/sw/tools/gen_flight_plan.ml @@ -155,10 +155,9 @@ let output_vmode x wp last_wp = let pitch = try Xml.attrib x "pitch" with _ -> "0.0" in if pitch = "auto" then begin - lprintf "auto_pitch = TRUE;\n"; - lprintf "nav_desired_gaz = TRIM_UPPRZ(%s*MAX_PPRZ);\n" (parsed_attrib x "gaz") + lprintf "climb_mode = CLIMB_MODE_PITCH;\n"; + lprintf "nav_desired_gaz = %s;\n" (parsed_attrib x "gaz") end else begin - lprintf "auto_pitch = FALSE;\n"; lprintf "nav_pitch = %s;\n" (parse pitch); end; let vmode = try ExtXml.attrib x "vmode" with _ -> "alt" in @@ -194,7 +193,7 @@ let output_vmode x wp last_wp = if (pitch = "auto") then failwith "auto pich mode not compatible with vmode=gaz"; lprintf "vertical_mode = VERTICAL_MODE_AUTO_GAZ;\n"; - lprintf "nav_desired_gaz = TRIM_UPPRZ(%s*MAX_PPRZ);\n" (parsed_attrib x "gaz") + lprintf "nav_desired_gaz = %s;\n" (parsed_attrib x "gaz") | x -> failwith (sprintf "Unknown vmode '%s'" x) end; vmode