diff --git a/conf/airframes/ENAC/fixed-wing/firestorm.xml b/conf/airframes/ENAC/fixed-wing/firestorm.xml index 2e128397b7..3199fdc038 100644 --- a/conf/airframes/ENAC/fixed-wing/firestorm.xml +++ b/conf/airframes/ENAC/fixed-wing/firestorm.xml @@ -1,37 +1,69 @@ + --> + - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - + + + - -
@@ -44,7 +76,6 @@ - @@ -54,56 +85,51 @@
-
- - - - +
+ + + - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
-
- - - - - - - - - - - - - - - - - - - - -
- -
- - - - - - - - +
+ +
- -
@@ -113,12 +139,10 @@ - - - +
@@ -135,10 +159,10 @@ - - - - + + + + @@ -151,7 +175,7 @@
- + @@ -162,8 +186,8 @@ - - + + @@ -173,7 +197,6 @@
-
@@ -188,87 +211,21 @@
-
+
- - - - - -
+ + + + + +
- - -CONFIG = \"tiny_1_1.h\" - -include $(PAPARAZZI_SRC)/conf/autopilot/tiny.makefile - -FLASH_MODE=IAP - -ap.CFLAGS += -DFBW -DAP -DBOARD_CONFIG=$(CONFIG) -DUSE_LED -DTIME_LED=1 -ap.srcs = sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c main_fbw.c main_ap.c main.c - -ap.srcs += commands.c - -ap.CFLAGS += -DACTUATORS=\"servos_4015_MAT_hw.h\" -DSERVOS_4015_MAT -ap.srcs += $(SRC_ARCH)/servos_4015_MAT_hw.c actuators.c - -ap.CFLAGS += -DRADIO_CONTROL -ap.srcs += radio_control.c $(SRC_ARCH)/ppm_hw.c - -ap.CFLAGS += -DDOWNLINK -DUSE_UART0 -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_FBW_DEVICE=Uart0 -DDOWNLINK_AP_DEVICE=Uart0 -DPPRZ_UART=Uart0 -DDATALINK=PPRZ -DUART0_BAUD=B9600 -ap.srcs += downlink.c $(SRC_ARCH)/mcu_periph/uart_arch.c datalink.c pprz_transport.c - -ap.CFLAGS += -DINTER_MCU -ap.srcs += inter_mcu.c - -ap.CFLAGS += -DADC -DUSE_ADC_0 -DUSE_ADC_1 -DUSE_ADC_2 -DUSE_ADC_3 -ap.srcs += $(SRC_ARCH)/adc_hw.c - -ap.CFLAGS += -DUSE_GPS -DUBX -DUSE_UART1 -DGPS_LINK=Uart1 -DUART1_BAUD=B38400 -DGPS_LED=2 -#ap.CFLAGS += -DUSE_GPS -DUBX -DUSE_UART1 -DGPS_LINK=Uart1 -DUART1_BAUD=B9600 -DGPS_LED=2 -#ap.CFLAGS += -DGPS_CONFIGURE -DGPS_BAUD=38400 - -ap.srcs += gps_ubx.c gps.c latlong.c - -ap.CFLAGS += -DUSE_INFRARED -DALT_KALMAN -ap.srcs += infrared.c estimator.c - -ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM -ap.srcs += subsystems/nav.c fw_h_ctl.c fw_v_ctl.c - - -ap.CFLAGS += -DUSE_GYRO -DADXRS150 -ap.srcs += gyro.c subsystems/navigation/nav_line.c -ap.srcs += subsystems/navigation/nav_survey_rectangle.c - - -# Current sensor; Neutral=162 -#ap.CFLAGS += -DUSE_ADC_7 -DUSE_ADC_GENERIC -DADC_CHANNEL_GENERIC1=ADC_7 -DADC_CHANNEL_GENERIC_NB_SAMPLES=16 -#ap.srcs += adc_generic.c - -ap.CFLAGS += -DUSE_ADC_7 -DADC_CHANNEL_CURRENT=ADC_7 - -ap.CFLAGS += -DUSE_MODULES - -# Config for SITL simulation -#include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile -sim.ARCHDIR = $(ARCHI) -sim.CFLAGS += -DSITL -DAP -DFBW -DRADIO_CONTROL -DINTER_MCU -DDOWNLINK -DDOWNLINK_TRANSPORT=IvyTransport -DUSE_INFRARED -DNAV -DUSE_LED -DWIND_INFO -sim.srcs = latlong.c radio_control.c downlink.c commands.c gps.c inter_mcu.c infrared.c fw_h_ctl.c fw_v_ctl.c subsystems/nav.c estimator.c sys_time.c main_fbw.c main_ap.c datalink.c $(SRC_ARCH)/ppm_hw.c $(SRC_ARCH)/sim_gps.c $(SRC_ARCH)/sim_ir.c $(SRC_ARCH)/sim_ap.c $(SRC_ARCH)/ivy_transport.c $(SRC_ARCH)/sim_adc_generic.c $(SRC_ARCH)/led_hw.c - -sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN - -sim.srcs += subsystems/navigation/nav_line.c subsystems/navigation/nav_survey_rectangle.c - -sim.CFLAGS += -DUSE_MODULES - diff --git a/conf/airframes/ENAC/fixed-wing/weasel.xml b/conf/airframes/ENAC/fixed-wing/weasel.xml index 2fec8df225..827a08b350 100644 --- a/conf/airframes/ENAC/fixed-wing/weasel.xml +++ b/conf/airframes/ENAC/fixed-wing/weasel.xml @@ -33,11 +33,13 @@ - + + + - + diff --git a/conf/airframes/ENAC/quadrotor/blender.xml b/conf/airframes/ENAC/quadrotor/blender.xml index 73c2e59ddf..4a23ddd7b6 100644 --- a/conf/airframes/ENAC/quadrotor/blender.xml +++ b/conf/airframes/ENAC/quadrotor/blender.xml @@ -18,6 +18,7 @@ + @@ -27,10 +28,15 @@ + - - - + + + + + + + @@ -47,6 +53,12 @@ + @@ -55,6 +67,31 @@ + + +
+ + + +
+ + +
@@ -93,9 +130,10 @@ - - - + + + +
@@ -136,32 +174,25 @@ - - - + + + - - - + + + - - + +
-
- - - - -
-
@@ -178,8 +209,8 @@ - - + + @@ -199,7 +230,7 @@
- +
@@ -222,7 +253,7 @@ - +
@@ -230,7 +261,7 @@ - +
diff --git a/conf/airframes/ENAC/quadrotor/booz2_g1.xml b/conf/airframes/ENAC/quadrotor/booz2_g1.xml index 5c0e701cde..db0e515bc6 100644 --- a/conf/airframes/ENAC/quadrotor/booz2_g1.xml +++ b/conf/airframes/ENAC/quadrotor/booz2_g1.xml @@ -1,9 +1,11 @@ - - - + + + + + @@ -161,67 +163,77 @@
- - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + +
- - - - - - -
+ + + + + + + -
- - - -
+
+ + + +
-
- - - -
+
+ + + +
-
- -
+
+ +
-
+
+ + - - - - + + + + -
+ +
-
- - - -
+
+ + + +
+ +
+ + + + +
@@ -229,10 +241,10 @@
-
- - - -
+
+ + + +
diff --git a/conf/airframes/ENAC/quadrotor/navgo.xml b/conf/airframes/ENAC/quadrotor/navgo.xml index 977d683724..a42bd6d411 100644 --- a/conf/airframes/ENAC/quadrotor/navgo.xml +++ b/conf/airframes/ENAC/quadrotor/navgo.xml @@ -12,6 +12,7 @@ + @@ -31,7 +32,9 @@ - + + + @@ -57,7 +60,7 @@
- + @@ -70,18 +73,52 @@
- - - + + + - + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + - + @@ -153,22 +190,22 @@ - - - + + + - - - + + + - - - + + +
diff --git a/conf/joystick/xbox_booz_cam.xml b/conf/joystick/xbox_booz_cam.xml new file mode 100644 index 0000000000..5eecdc472e --- /dev/null +++ b/conf/joystick/xbox_booz_cam.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/conf/messages.xml b/conf/messages.xml index f8bfcf710e..a5e5e008bf 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -2171,6 +2171,12 @@ + + + + + + diff --git a/conf/modules/booz_cam.xml b/conf/modules/booz_cam.xml index e489b6e290..36fc1b8d35 100644 --- a/conf/modules/booz_cam.xml +++ b/conf/modules/booz_cam.xml @@ -7,6 +7,7 @@ + diff --git a/conf/modules/booz_drop.xml b/conf/modules/booz_drop.xml index 79f805cf74..fc1f370e2a 100644 --- a/conf/modules/booz_drop.xml +++ b/conf/modules/booz_drop.xml @@ -1,4 +1,8 @@ + @@ -9,8 +13,6 @@ - - diff --git a/conf/modules/ins_chimu_spi.xml b/conf/modules/ins_chimu_spi.xml index ea57d2ff2e..db4f90c226 100644 --- a/conf/modules/ins_chimu_spi.xml +++ b/conf/modules/ins_chimu_spi.xml @@ -6,7 +6,7 @@ - + diff --git a/conf/modules/poles.xml b/conf/modules/poles.xml index ed760a56e3..042c3dacbc 100644 --- a/conf/modules/poles.xml +++ b/conf/modules/poles.xml @@ -2,7 +2,7 @@
- +
diff --git a/conf/settings/booz_cam.xml b/conf/settings/booz_cam.xml index c177bc7f3e..4b7c08e05d 100644 --- a/conf/settings/booz_cam.xml +++ b/conf/settings/booz_cam.xml @@ -3,17 +3,17 @@ - - - - + + + + - - + + diff --git a/conf/settings/poles.xml b/conf/settings/poles.xml index c7a1ecca56..c5e934896e 100644 --- a/conf/settings/poles.xml +++ b/conf/settings/poles.xml @@ -3,7 +3,9 @@ - + + + diff --git a/conf/settings/settings_booz_drop.xml b/conf/settings/settings_booz_drop.xml index f3767d94e6..6d6d07fdce 100644 --- a/conf/settings/settings_booz_drop.xml +++ b/conf/settings/settings_booz_drop.xml @@ -5,8 +5,8 @@ - - + + diff --git a/conf/telemetry/booz_minimal.xml b/conf/telemetry/booz_minimal.xml index b178ddbb47..281730aa57 100644 --- a/conf/telemetry/booz_minimal.xml +++ b/conf/telemetry/booz_minimal.xml @@ -24,7 +24,7 @@ - + diff --git a/sw/airborne/ap_downlink.h b/sw/airborne/ap_downlink.h index 8c87dc305d..da8d98444a 100644 --- a/sw/airborne/ap_downlink.h +++ b/sw/airborne/ap_downlink.h @@ -217,7 +217,7 @@ static uint8_t i; \ int16_t climb = -gps.ned_vel.z; \ int16_t course = (DegOfRad(gps.course)/((int32_t)1e6)); \ - DOWNLINK_SEND_GPS(DefaultChannel, &gps.fix, &gps.utm_pos.east, &gps.utm_pos.north, &course, &gps.lla_pos.alt, &gps.gspeed, &climb, &gps.week, &gps.tow, &gps.utm_pos.zone, &i); \ + DOWNLINK_SEND_GPS(DefaultChannel, &gps.fix, &gps.utm_pos.east, &gps.utm_pos.north, &course, &gps.hmsl, &gps.gspeed, &climb, &gps.week, &gps.tow, &gps.utm_pos.zone, &i); \ if ((gps.fix != GPS_FIX_3D) && (i >= gps.nb_channels)) i = 0; \ if (i >= gps.nb_channels * 2) i = 0; \ if (i < gps.nb_channels && gps.svinfos[i].cno > 0) { \ diff --git a/sw/airborne/arch/stm32/subsystems/settings_arch.c b/sw/airborne/arch/stm32/subsystems/settings_arch.c index e232d29471..38e1bcd7a8 100644 --- a/sw/airborne/arch/stm32/subsystems/settings_arch.c +++ b/sw/airborne/arch/stm32/subsystems/settings_arch.c @@ -117,7 +117,7 @@ static int32_t flash_detect(struct FlashInfo* flash) { case 0x00008000: /* 32 kBytes */ /* medium density, e.g. STM32F103RBT6 (Olimex STM32-H103) */ case 0x00010000: /* 64 kBytes */ - case 0x00200000: /* 128 kBytes */ + case 0x00020000: /* 128 kBytes */ { flash->page_size = 0x400; break; diff --git a/sw/airborne/boards/umarim/imu_umarim.c b/sw/airborne/boards/umarim/imu_umarim.c index c1e9a8aee5..91b5047f7b 100644 --- a/sw/airborne/boards/umarim/imu_umarim.c +++ b/sw/airborne/boards/umarim/imu_umarim.c @@ -91,7 +91,7 @@ void imu_umarim_event( void ) // If the itg3200 I2C transaction has succeeded: convert the data itg3200_event(); if (itg3200_data_available) { - RATES_COPY(imu.gyro_unscaled, itg3200_data); + RATES_ASSIGN(imu.gyro_unscaled, itg3200_data.p, itg3200_data.q, itg3200_data.r); itg3200_data_available = FALSE; gyr_valid = TRUE; } @@ -99,6 +99,7 @@ void imu_umarim_event( void ) // If the adxl345 I2C transaction has succeeded: convert the data adxl345_event(); if (adxl345_data_available) { + // Be careful with orientation of the ADXL (ITG axes are taken as default reference) VECT3_ASSIGN(imu.accel_unscaled, adxl345_data.y, -adxl345_data.x, adxl345_data.z); adxl345_data_available = FALSE; acc_valid = TRUE; diff --git a/sw/airborne/boards/umarim_1.0.h b/sw/airborne/boards/umarim_1.0.h index 38a1b904c9..f4c87cbb96 100644 --- a/sw/airborne/boards/umarim_1.0.h +++ b/sw/airborne/boards/umarim_1.0.h @@ -44,6 +44,39 @@ /* ADC */ +#define ADC_0 AdcBank1(5) +#ifdef USE_ADC_0 +#ifndef USE_AD1 +#define USE_AD1 +#endif +#define USE_AD1_5 +#endif + +#define ADC_1 AdcBank1(4) +#ifdef USE_ADC_1 +#ifndef USE_AD1 +#define USE_AD1 +#endif +#define USE_AD1_4 +#endif + +#define ADC_2 AdcBank1(3) +#ifdef USE_ADC_2 +#ifndef USE_AD1 +#define USE_AD1 +#endif +#define USE_AD1_3 +#endif + +#define ADC_3 AdcBank1(2) +#ifdef USE_ADC_3 +#ifndef USE_AD1 +#define USE_AD1 +#endif +#define USE_AD1_2 +#endif + + /* battery */ #define ADC_CHANNEL_VSUPPLY AdcBank0(2) #ifndef USE_AD0 diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.c b/sw/airborne/firmwares/rotorcraft/autopilot.c index ba7c3e1af4..6cc818a313 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot.c +++ b/sw/airborne/firmwares/rotorcraft/autopilot.c @@ -37,6 +37,7 @@ bool_t autopilot_motors_on; bool_t autopilot_in_flight; uint32_t autopilot_motors_on_counter; uint32_t autopilot_in_flight_counter; +uint8_t autopilot_check_motor_status; bool_t kill_throttle; bool_t autopilot_rc; @@ -51,6 +52,13 @@ uint16_t autopilot_flight_time; #define AUTOPILOT_IN_FLIGHT_TIME 40 #define AUTOPILOT_THROTTLE_TRESHOLD (MAX_PPRZ / 20) #define AUTOPILOT_YAW_TRESHOLD (MAX_PPRZ * 19 / 20) +// Motors ON check state machine +#define STATUS_MOTORS_OFF 0 +#define STATUS_M_OFF_STICK_PUSHED 1 +#define STATUS_START_MOTORS 2 +#define STATUS_MOTORS_ON 3 +#define STATUS_M_ON_STICK_PUSHED 4 +#define STATUS_STOP_MOTORS 5 void autopilot_init(void) { autopilot_mode = AP_MODE_KILL; @@ -59,6 +67,7 @@ void autopilot_init(void) { kill_throttle = ! autopilot_motors_on; autopilot_motors_on_counter = 0; autopilot_in_flight_counter = 0; + autopilot_check_motor_status = STATUS_MOTORS_OFF; autopilot_mode_auto2 = MODE_AUTO2; autopilot_detect_ground = FALSE; autopilot_detect_ground_once = FALSE; @@ -180,6 +189,7 @@ void autopilot_set_mode(uint8_t new_autopilot_mode) { (radio_control.values[RADIO_YAW] > AUTOPILOT_YAW_TRESHOLD || \ radio_control.values[RADIO_YAW] < -AUTOPILOT_YAW_TRESHOLD) + static inline void autopilot_check_in_flight( void) { if (autopilot_in_flight) { if (autopilot_in_flight_counter > 0) { @@ -220,33 +230,65 @@ static inline int ahrs_is_aligned(void) { } #endif -static inline void autopilot_check_motors_on( void ) { - if (autopilot_motors_on) { - if (THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED()) { - if ( autopilot_motors_on_counter > 0) { - autopilot_motors_on_counter--; - if (autopilot_motors_on_counter == 0) - autopilot_motors_on = FALSE; - } - } - else { /* sticks not in the corner */ - autopilot_motors_on_counter = AUTOPILOT_MOTOR_ON_TIME; - } - } - else { /* motors off */ - if (THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED() && ahrs_is_aligned()) { - if ( autopilot_motors_on_counter < AUTOPILOT_MOTOR_ON_TIME) { - autopilot_motors_on_counter++; - if (autopilot_motors_on_counter == AUTOPILOT_MOTOR_ON_TIME) - autopilot_motors_on = TRUE; - } - } - else { - autopilot_motors_on_counter = 0; - } - } +/** Set motors ON or OFF and change the status of the check_motors state machine + */ +void autopilot_set_motors_on(bool_t motors_on) { + autopilot_motors_on = motors_on; + kill_throttle = ! autopilot_motors_on; + if (autopilot_motors_on) autopilot_check_motor_status = STATUS_MOTORS_ON; + else autopilot_check_motor_status = STATUS_MOTORS_OFF; } +/** + * State machine to check if motors should be turned ON or OFF + * The motors start/stop when pushing the yaw stick without throttle during a given time + * An intermediate state prevents oscillating between ON and OFF while keeping the stick pushed + * The stick must return to a neutral position before starting/stoping again + */ +static inline void autopilot_check_motors_on( void ) { + switch(autopilot_check_motor_status) { + case STATUS_MOTORS_OFF: + autopilot_motors_on = FALSE; + autopilot_motors_on_counter = 0; + if (THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED()) // stick pushed + autopilot_check_motor_status = STATUS_M_OFF_STICK_PUSHED; + break; + case STATUS_M_OFF_STICK_PUSHED: + autopilot_motors_on = FALSE; + autopilot_motors_on_counter++; + if (autopilot_motors_on_counter >= AUTOPILOT_MOTOR_ON_TIME) + autopilot_check_motor_status = STATUS_START_MOTORS; + else if (!(THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED())) // stick released too soon + autopilot_check_motor_status = STATUS_MOTORS_OFF; + break; + case STATUS_START_MOTORS: + autopilot_motors_on = TRUE; + autopilot_motors_on_counter = AUTOPILOT_MOTOR_ON_TIME; + if (!(THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED())) // wait until stick released + autopilot_check_motor_status = STATUS_MOTORS_ON; + break; + case STATUS_MOTORS_ON: + autopilot_motors_on = TRUE; + autopilot_motors_on_counter = AUTOPILOT_MOTOR_ON_TIME; + if (THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED()) // stick pushed + autopilot_check_motor_status = STATUS_M_ON_STICK_PUSHED; + break; + case STATUS_M_ON_STICK_PUSHED: + autopilot_motors_on = TRUE; + autopilot_motors_on_counter--; + if (autopilot_motors_on_counter == 0) + autopilot_check_motor_status = STATUS_STOP_MOTORS; + else if (!(THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED())) // stick released too soon + autopilot_check_motor_status = STATUS_MOTORS_ON; + break; + case STATUS_STOP_MOTORS: + autopilot_motors_on = FALSE; + autopilot_motors_on_counter = 0; + if (!(THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED())) // wait until stick released + autopilot_check_motor_status = STATUS_MOTORS_OFF; + break; + }; +} void autopilot_on_rc_frame(void) { diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.h b/sw/airborne/firmwares/rotorcraft/autopilot.h index 9c611a7316..b2eda139d3 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot.h +++ b/sw/airborne/firmwares/rotorcraft/autopilot.h @@ -60,6 +60,7 @@ extern void autopilot_init(void); extern void autopilot_periodic(void); extern void autopilot_on_rc_frame(void); extern void autopilot_set_mode(uint8_t new_autopilot_mode); +extern void autopilot_set_motors_on(bool_t motors_on); extern bool_t autopilot_detect_ground; extern bool_t autopilot_detect_ground_once; diff --git a/sw/airborne/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c index 46b8b081f7..5b85ffac51 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.c +++ b/sw/airborne/firmwares/rotorcraft/navigation.c @@ -280,7 +280,7 @@ void nav_periodic_task() { /* run carrot loop */ nav_run(); - ground_alt = POS_BFP_OF_REAL((float)ins_ltp_def.hmsl / 100.); + ground_alt = POS_BFP_OF_REAL((float)ins_ltp_def.hmsl / 1000.); } #include "downlink.h" diff --git a/sw/airborne/firmwares/rotorcraft/navigation.h b/sw/airborne/firmwares/rotorcraft/navigation.h index 742f1b9c10..5ce687f4cf 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.h +++ b/sw/airborne/firmwares/rotorcraft/navigation.h @@ -76,8 +76,8 @@ bool_t nav_detect_ground(void); void nav_home(void); -#define NavKillThrottle() ({ if (autopilot_mode == AP_MODE_NAV) { kill_throttle = 1; autopilot_motors_on = 0; } FALSE; }) -#define NavResurrect() ({ if (autopilot_mode == AP_MODE_NAV) { kill_throttle = 0; autopilot_motors_on = 1; } FALSE; }) +#define NavKillThrottle() ({ if (autopilot_mode == AP_MODE_NAV) { autopilot_set_motors_on(FALSE); } FALSE; }) +#define NavResurrect() ({ if (autopilot_mode == AP_MODE_NAV) { autopilot_set_motors_on(TRUE); } FALSE; }) #define NavSetGroundReferenceHere() ({ nav_reset_reference(); FALSE; }) diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c index d0b538e6f7..879c6cbc13 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c @@ -69,11 +69,13 @@ void stabilization_attitude_ref_init(void) { #define OMEGA_2_R_RES 7 #define OMEGA_2_R BFP_OF_REAL((OMEGA_R*OMEGA_R), OMEGA_2_R_RES) -#define USE_REF 1 +#ifndef USE_ATTITUDE_REF +#define USE_ATTITUDE_REF 1 +#endif void stabilization_attitude_ref_update() { -#ifdef USE_REF +#if USE_ATTITUDE_REF /* dumb integrate reference attitude */ const struct Int32Eulers d_angle = { @@ -120,10 +122,10 @@ void stabilization_attitude_ref_update() { /* saturate speed and trim accel accordingly */ SATURATE_SPEED_TRIM_ACCEL(); -#else /* !USE_REF */ - EULERS_COPY(stab_att_ref_euler, stabilization_att_sp); +#else /* !USE_ATTITUDE_REF */ + EULERS_COPY(stab_att_ref_euler, stab_att_sp_euler); INT_RATES_ZERO(stab_att_ref_rate); INT_RATES_ZERO(stab_att_ref_accel); -#endif /* USE_REF */ +#endif /* USE_ATTITUDE_REF */ } diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c index 951eea11bf..44ff7ac984 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c @@ -135,6 +135,9 @@ void stabilization_rate_read_rc( void ) { stabilization_rate_sp.r = (int32_t)-radio_control.values[RADIO_YAW] * STABILIZATION_RATE_SP_MAX_R / MAX_PPRZ; else stabilization_rate_sp.r = 0; + + // Setpoint at ref resolution + INT_RATES_LSHIFT(stabilization_rate_sp, stabilization_rate_sp, REF_FRAC - INT32_RATE_FRAC); } void stabilization_rate_enter(void) { diff --git a/sw/airborne/math/pprz_algebra.h b/sw/airborne/math/pprz_algebra.h index 81b35c7b54..086479d390 100644 --- a/sw/airborne/math/pprz_algebra.h +++ b/sw/airborne/math/pprz_algebra.h @@ -299,6 +299,13 @@ (_c).r = (_a).r + (_b).r; \ } +/* c = a + _s * b */ +#define RATES_SUM_SCALED(_c, _a, _b, _s) { \ + (_c).p = (_a).p + (_s)*(_b).p; \ + (_c).q = (_a).q + (_s)*(_b).q; \ + (_c).r = (_a).r + (_s)*(_b).r; \ + } + /* c = a - b */ #define RATES_DIFF(_c, _a, _b) { \ (_c).p = (_a).p - (_b).p; \ diff --git a/sw/airborne/modules/cam_control/booz_cam.c b/sw/airborne/modules/cam_control/booz_cam.c index c595b787b1..77d762db46 100644 --- a/sw/airborne/modules/cam_control/booz_cam.c +++ b/sw/airborne/modules/cam_control/booz_cam.c @@ -72,7 +72,7 @@ int16_t booz_cam_pan; #endif void booz_cam_init(void) { - booz_cam_mode = BOOZ_CAM_DEFAULT_MODE; + booz_cam_SetCamMode(BOOZ_CAM_DEFAULT_MODE); #ifdef BOOZ_CAM_USE_TILT booz_cam_tilt_pwm = BOOZ_CAM_TILT_NEUTRAL; BOOZ_CAM_SetPwm(booz_cam_tilt_pwm); @@ -81,10 +81,15 @@ void booz_cam_init(void) { #ifdef BOOZ_CAM_USE_PAN booz_cam_pan = BOOZ_CAM_PAN_NEUTRAL; #endif - if (booz_cam_mode == BOOZ_CAM_MODE_NONE) { LED_ON(CAM_SWITCH_LED); } // CAM OFF - else { LED_OFF(CAM_SWITCH_LED); } // CAM ON } +#define ABS(_x) ((_x) < 0 ? -(_x) : (_x)) +#define D_TILT (BOOZ_CAM_TILT_MAX - BOOZ_CAM_TILT_MIN) +#define CT_MIN Min(BOOZ_CAM_TILT_MIN,BOOZ_CAM_TILT_MAX) +#define CT_MAX Max(BOOZ_CAM_TILT_MIN,BOOZ_CAM_TILT_MAX) +#define CP_MIN Min(BOOZ_CAM_PAN_MIN,BOOZ_CAM_PAN_MAX) +#define CP_MAX Max(BOOZ_CAM_PAN_MIN,BOOZ_CAM_PAN_MAX) + void booz_cam_periodic(void) { switch (booz_cam_mode) { @@ -98,17 +103,17 @@ void booz_cam_periodic(void) { break; case BOOZ_CAM_MODE_MANUAL: #ifdef BOOZ_CAM_USE_TILT - Bound(booz_cam_tilt_pwm,BOOZ_CAM_TILT_MIN,BOOZ_CAM_TILT_MAX); + Bound(booz_cam_tilt_pwm, CT_MIN, CT_MAX); #endif break; case BOOZ_CAM_MODE_HEADING: #ifdef BOOZ_CAM_USE_TILT_ANGLES Bound(booz_cam_tilt,CAM_TA_MIN,CAM_TA_MAX); - booz_cam_tilt_pwm = BOOZ_CAM_TILT_MIN + (BOOZ_CAM_TILT_MAX - BOOZ_CAM_TILT_MIN) * (booz_cam_tilt - CAM_TA_MIN) / (CAM_TA_MAX - CAM_TA_MIN); - Bound(booz_cam_tilt_pwm,BOOZ_CAM_TILT_MIN,BOOZ_CAM_TILT_MAX); + booz_cam_tilt_pwm = BOOZ_CAM_TILT_MIN + D_TILT * (booz_cam_tilt - CAM_TA_MIN) / (CAM_TA_MAX - CAM_TA_MIN); + Bound(booz_cam_tilt_pwm, CT_MIN, CT_MAX); #endif #ifdef BOOZ_CAM_USE_PAN - Bound(booz_cam_pan,BOOZ_CAM_PAN_MIN,BOOZ_CAM_PAN_MAX); + Bound(booz_cam_pan, CP_MIN, CP_MAX); nav_heading = booz_cam_pan; #endif break; @@ -126,8 +131,8 @@ void booz_cam_periodic(void) { height = (waypoints[WP_CAM].z - ins_enu_pos.z) >> INT32_POS_FRAC; INT32_ATAN2(booz_cam_tilt, height, dist); Bound(booz_cam_tilt, CAM_TA_MIN, CAM_TA_MAX); - booz_cam_tilt_pwm = BOOZ_CAM_TILT_MIN + (BOOZ_CAM_TILT_MAX - BOOZ_CAM_TILT_MIN) * (booz_cam_tilt - CAM_TA_MIN) / (CAM_TA_MAX - CAM_TA_MIN); - Bound(booz_cam_tilt_pwm, BOOZ_CAM_TILT_MIN, BOOZ_CAM_TILT_MAX); + booz_cam_tilt_pwm = BOOZ_CAM_TILT_MIN + D_TILT * (booz_cam_tilt - CAM_TA_MIN) / (CAM_TA_MAX - CAM_TA_MIN); + Bound(booz_cam_tilt_pwm, CT_MIN, CT_MAX); #endif } #endif diff --git a/sw/airborne/modules/cam_control/booz_cam.h b/sw/airborne/modules/cam_control/booz_cam.h index 4e97b33bf8..1afa847cb0 100644 --- a/sw/airborne/modules/cam_control/booz_cam.h +++ b/sw/airborne/modules/cam_control/booz_cam.h @@ -26,6 +26,7 @@ #define BOOZ_CAM_H #include "generated/airframe.h" +#include "math/pprz_algebra_int.h" #include "std.h" #include "led.h" @@ -34,6 +35,16 @@ #define BOOZ_CAM_MODE_HEADING 2 #define BOOZ_CAM_MODE_WP 3 +// Warning: +// LED_ON set GPIO low +// LED_OFF set GPIO high +#ifndef BOOZ_CAM_ON +#define BOOZ_CAM_ON LED_OFF(CAM_SWITCH_LED) +#endif +#ifndef BOOZ_CAM_OFF +#define BOOZ_CAM_OFF LED_ON(CAM_SWITCH_LED) +#endif + extern uint8_t booz_cam_mode; #ifdef BOOZ_CAM_TILT_NEUTRAL @@ -49,8 +60,17 @@ extern void booz_cam_periodic(void); #define booz_cam_SetCamMode(_v) { \ booz_cam_mode = _v; \ - if (booz_cam_mode == BOOZ_CAM_MODE_NONE) { LED_ON(CAM_SWITCH_LED); } \ - else { LED_OFF(CAM_SWITCH_LED); } \ + if (booz_cam_mode == BOOZ_CAM_MODE_NONE) { BOOZ_CAM_OFF; } \ + else { BOOZ_CAM_ON; } \ +} + +#define BOOZ_CAM_STICK_TILT_INC (ANGLE_BFP_OF_REAL(RadOfDeg(10.))/127.) +#define BOOZ_CAM_STICK_PAN_INC (ANGLE_BFP_OF_REAL(RadOfDeg(20.))/127.) + +#define BOOZ_CAM_STICK_PARSE(_dl_buffer) { \ + booz_cam_tilt += (int16_t)(BOOZ_CAM_STICK_TILT_INC*(float)DL_BOOZ_CAM_STICK_tilt(_dl_buffer)); \ + booz_cam_pan += (int16_t)(BOOZ_CAM_STICK_PAN_INC*(float)DL_BOOZ_CAM_STICK_pan(dl_buffer)); \ + INT32_COURSE_NORMALIZE(booz_cam_pan); \ } #endif /* BOOZ2_CAM_H */ diff --git a/sw/airborne/modules/drop/booz_drop.h b/sw/airborne/modules/drop/booz_drop.h index e04057ef0d..c1b0182305 100644 --- a/sw/airborne/modules/drop/booz_drop.h +++ b/sw/airborne/modules/drop/booz_drop.h @@ -33,5 +33,6 @@ extern void booz_drop_init(void); extern void booz_drop_periodic(void); #define NavDropNow() ({ booz_drop_ball = TRUE; FALSE; }) +#define NavDropClose() ({ booz_drop_ball = FALSE; FALSE; }) #endif /* BOOZ_DROP_H */ diff --git a/sw/airborne/modules/ins/imu_chimu.c b/sw/airborne/modules/ins/imu_chimu.c index 4b0111bee5..8ee2f11cc7 100644 --- a/sw/airborne/modules/ins/imu_chimu.c +++ b/sw/airborne/modules/ins/imu_chimu.c @@ -106,34 +106,6 @@ void CHIMU_Checksum(unsigned char *data, unsigned char buflen) // Communication Definitions #define CHIMU_COM_ID_HIGH 0x1F //Must set this to the max ID expected above -/*************************************************************************** - * Endianness Swapping Functions - */ - -#ifdef CHIMU_BIG_ENDIAN - -static float FloatSwap( float f ) -{ - union - { - float f; - unsigned char b[4]; - } dat1, dat2; - - dat1.f = f; - dat2.b[0] = dat1.b[3]; - dat2.b[1] = dat1.b[2]; - dat2.b[2] = dat1.b[1]; - dat2.b[3] = dat1.b[0]; - return dat2.f; -} - -#else - -#define FloatSwap(X) (X) - -#endif - /*--------------------------------------------------------------------------- Name: CHIMU_Init diff --git a/sw/airborne/modules/ins/imu_chimu.h b/sw/airborne/modules/ins/imu_chimu.h index d1caf15286..d246bf6a6f 100644 --- a/sw/airborne/modules/ins/imu_chimu.h +++ b/sw/airborne/modules/ins/imu_chimu.h @@ -86,6 +86,35 @@ #define CHIMU_Msg_15_SFCheck 15 +/*************************************************************************** + * Endianness Swapping Functions + */ + +#ifdef CHIMU_BIG_ENDIAN + +static inline float FloatSwap( float f ) +{ + union + { + float f; + unsigned char b[4]; + } dat1, dat2; + + dat1.f = f; + dat2.b[0] = dat1.b[3]; + dat2.b[1] = dat1.b[2]; + dat2.b[2] = dat1.b[1]; + dat2.b[3] = dat1.b[0]; + return dat2.f; +} + +#else + +#define FloatSwap(X) (X) + +#endif + + typedef struct { float phi; float theta; diff --git a/sw/airborne/modules/ins/ins_chimu_spi.c b/sw/airborne/modules/ins/ins_chimu_spi.c index 6ea0deaca2..87dd9f519f 100644 --- a/sw/airborne/modules/ins/ins_chimu_spi.c +++ b/sw/airborne/modules/ins/ins_chimu_spi.c @@ -27,6 +27,9 @@ C code to connect a CHIMU using uart #include "ins_module.h" #include "imu_chimu.h" +#include "subsystems/gps.h" + + CHIMU_PARSER_DATA CHIMU_DATA; INS_FORMAT ins_roll_neutral; @@ -101,7 +104,17 @@ void parse_ins_msg( void ) void ins_periodic_task( void ) { // Send SW Centripetal Corrections - uint8_t centripedal[19] = {0xae, 0xae, 0x0d, 0xaa, 0x0b, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc2 }; + uint8_t centripedal[19] = {0xae, 0xae, 0x0d, 0xaa, 0x0b, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc2 }; + + float gps_speed = 0; + + if (gps.fix == GPS_FIX_3D) + { + gps_speed = gps.speed_3d/100.; + } + gps_speed = FloatSwap(gps_speed); + + memmove (¢ripedal[6], &gps_speed, 4); // Fill X-speed diff --git a/sw/airborne/modules/ins/ins_xsens.c b/sw/airborne/modules/ins/ins_xsens.c index 9b55080239..f16d597a69 100644 --- a/sw/airborne/modules/ins/ins_xsens.c +++ b/sw/airborne/modules/ins/ins_xsens.c @@ -306,6 +306,10 @@ void handle_ins_msg( void) { hmsl /= 1000.0f; EstimatorSetAlt(hmsl); + #ifndef ALT_KALMAN + #warning NO_VZ + #endif + // Horizontal speed float fspeed = sqrt(ins_vx*ins_vx + ins_vy*ins_vy); if (gps.fix != GPS_FIX_3D) diff --git a/sw/airborne/modules/poles/nav_poles.c b/sw/airborne/modules/poles/nav_poles.c index 2a09f63e8a..6d9bb74e88 100644 --- a/sw/airborne/modules/poles/nav_poles.c +++ b/sw/airborne/modules/poles/nav_poles.c @@ -1,9 +1,11 @@ -#include "subsystems/navigation/nav_poles.h" +#include "modules/poles/nav_poles.h" #include "subsystems/navigation/common_nav.h" uint8_t nav_poles_count = 0; +float nav_poles_time = 0.; +int8_t nav_poles_land = 1; -#define SAFETY_MARGIN 0.9 +#define SAFETY_MARGIN 0.7 /** computes position of wp1c and wp2c, reference points for an oval around waypoints wp1 and wp2 */ diff --git a/sw/airborne/modules/poles/nav_poles.h b/sw/airborne/modules/poles/nav_poles.h index 37e81f6167..186e490cba 100644 --- a/sw/airborne/modules/poles/nav_poles.h +++ b/sw/airborne/modules/poles/nav_poles.h @@ -5,9 +5,13 @@ #include "std.h" extern uint8_t nav_poles_count; +extern float nav_poles_time; +extern int8_t nav_poles_land; bool nav_poles_init(uint8_t wp1, uint8_t wp2, uint8_t wp1c, uint8_t wp2c, float radius ); +#define nav_poles_SetLandDir(_d) { if (_d < 0) _d = -1; else _d = 1; } + #endif diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c index 7f88535141..3ca1a0f63d 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c +++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c @@ -91,6 +91,18 @@ int renorm_blowup_count = 0; float imu_health = 0.; #endif +#ifdef USE_HIGH_ACCEL_FLAG +// High Accel Flag +#define HIGH_ACCEL_LOW_SPEED 15.0 +#define HIGH_ACCEL_LOW_SPEED_RESUME 4.0 // Hysteresis +#define HIGH_ACCEL_HIGH_THRUST (0.8*MAX_PPRZ) +#define HIGH_ACCEL_HIGH_THRUST_RESUME (0.1*MAX_PPRZ) // Hysteresis +bool_t high_accel_done; +bool_t high_accel_flag; +// Command vector for thrust (fixed_wing) +#include "inter_mcu.h" +#endif + static inline void set_dcm_matrix_from_rmat(struct FloatRMat *rmat) { @@ -175,6 +187,11 @@ void ahrs_init(void) { /* set inital filter dcm */ set_dcm_matrix_from_rmat(&ahrs_float.ltp_to_imu_rmat); + +#ifdef USE_HIGH_ACCEL_FLAG + high_accel_done = FALSE; + high_accel_flag = FALSE; +#endif } void ahrs_align(void) @@ -411,6 +428,25 @@ void Drift_correction(void) // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0) Accel_weight = Chop(1 - 2*fabs(1 - Accel_magnitude),0,1); // +#ifdef USE_HIGH_ACCEL_FLAG + // Test for high acceleration: + // - low speed + // - high thrust + if (estimator_hspeed_mod < HIGH_ACCEL_LOW_SPEED && ap_state->commands[COMMAND_THROTTLE] > HIGH_ACCEL_HIGH_THRUST && !high_accel_done) { + high_accel_flag = TRUE; + } else { + high_accel_flag = FALSE; + if (estimator_hspeed_mod > HIGH_ACCEL_LOW_SPEED && !high_accel_done) { + high_accel_done = TRUE; // After takeoff, don't use high accel before landing (GS small, Throttle small) + } + if (estimator_hspeed_mod < HIGH_ACCEL_HIGH_THRUST_RESUME && ap_state->commands[COMMAND_THROTTLE] < HIGH_ACCEL_HIGH_THRUST_RESUME) { + high_accel_done = FALSE; // Activate high accel after landing + } + } + if (high_accel_flag) { Accel_weight = 0.; } +#endif + + #if PERFORMANCE_REPORTING == 1 { diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c index 7b95101f01..eeaf3fb72e 100644 --- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c +++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c @@ -95,7 +95,41 @@ void ahrs_align(void) { } +//#define USE_NOISE_CUT 1 +//#define USE_NOISE_FILTER 1 +#define NOISE_FILTER_GAIN 50 +#ifdef USE_NOISE_CUT +#include "led.h" +static inline bool_t cut_rates (struct Int32Rates i1, struct Int32Rates i2, int32_t threshold) { + struct Int32Rates diff; + RATES_DIFF(diff, i1, i2); + if (diff.p < -threshold || diff.p > threshold || + diff.q < -threshold || diff.q > threshold || + diff.r < -threshold || diff.r > threshold) { + return TRUE; + } else { + return FALSE; + } +} +#define RATE_CUT_THRESHOLD RATE_BFP_OF_REAL(1) + +static inline bool_t cut_accel (struct Int32Vect3 i1, struct Int32Vect3 i2, int32_t threshold) { + struct Int32Vect3 diff; + VECT3_DIFF(diff, i1, i2); + if (diff.x < -threshold || diff.x > threshold || + diff.y < -threshold || diff.y > threshold || + diff.z < -threshold || diff.z > threshold) { + LED_ON(4); + return TRUE; + } else { + LED_OFF(4); + return FALSE; + } +} +#define ACCEL_CUT_THRESHOLD ACCEL_BFP_OF_REAL(20) + +#endif /* * @@ -116,9 +150,22 @@ void ahrs_propagate(void) { /* unbias gyro */ struct Int32Rates uf_rate; RATES_DIFF(uf_rate, imu.gyro, ahrs_impl.gyro_bias); - /* low pass rate */ - RATES_ADD(ahrs.imu_rate, uf_rate); - RATES_SDIV(ahrs.imu_rate, ahrs.imu_rate, 2); +#ifdef USE_NOISE_CUT + static struct Int32Rates last_uf_rate = { 0, 0, 0 }; + if (!cut_rates(uf_rate, last_uf_rate, RATE_CUT_THRESHOLD)) { +#endif + /* low pass rate */ +#ifdef USE_NOISE_FILTER + RATES_SUM_SCALED(ahrs.imu_rate, ahrs.imu_rate, uf_rate, NOISE_FILTER_GAIN); + RATES_SDIV(ahrs.imu_rate, ahrs.imu_rate, NOISE_FILTER_GAIN+1); +#else + RATES_ADD(ahrs.imu_rate, uf_rate); + RATES_SDIV(ahrs.imu_rate, ahrs.imu_rate, 2); +#endif +#ifdef USE_NOISE_CUT + } + RATES_COPY(last_uf_rate, uf_rate); +#endif /* integrate eulers */ struct Int32Eulers euler_dot; @@ -152,7 +199,21 @@ void ahrs_propagate(void) { void ahrs_update_accel(void) { - get_phi_theta_measurement_fom_accel(&ahrs_impl.measurement.phi, &ahrs_impl.measurement.theta, imu.accel); +#if defined(USE_NOISE_CUT) || defined(USE_NOISE_FILTER) + static struct Int32Vect3 last_accel = { 0, 0, 0 }; +#endif +#ifdef USE_NOISE_CUT + if (!cut_accel(imu.accel, last_accel, ACCEL_CUT_THRESHOLD)) { +#endif +#ifdef USE_NOISE_FILTER + VECT3_SUM_SCALED(imu.accel, imu.accel, last_accel, NOISE_FILTER_GAIN); + VECT3_SDIV(imu.accel, imu.accel, NOISE_FILTER_GAIN+1); +#endif + get_phi_theta_measurement_fom_accel(&ahrs_impl.measurement.phi, &ahrs_impl.measurement.theta, imu.accel); +#ifdef USE_NOISE_CUT + } + VECT3_COPY(last_accel, imu.accel); +#endif } diff --git a/sw/airborne/subsystems/nav.h b/sw/airborne/subsystems/nav.h index 9688560c8c..fbda094cbb 100644 --- a/sw/airborne/subsystems/nav.h +++ b/sw/airborne/subsystems/nav.h @@ -175,7 +175,8 @@ bool_t nav_approaching_xy(float x, float y, float from_x, float from_y, float ap #define NavAttitude(_roll) { \ lateral_mode = LATERAL_MODE_ROLL; \ - h_ctl_roll_setpoint = _roll; \ + if(pprz_mode != PPRZ_MODE_AUTO1) \ + {h_ctl_roll_setpoint = _roll;} \ } #define nav_IncreaseShift(x) { if (x==0) nav_shift = 0; else nav_shift += x; } diff --git a/sw/ground_segment/tmtc/gpsd2ivy.c b/sw/ground_segment/tmtc/gpsd2ivy.c index 9c2882ba58..5f31381ed2 100644 --- a/sw/ground_segment/tmtc/gpsd2ivy.c +++ b/sw/ground_segment/tmtc/gpsd2ivy.c @@ -70,8 +70,7 @@ static struct gps_data_t *gpsdata; static void update_gps(struct gps_data_t *gpsdata, char *message, - size_t len, - int level) + size_t len) { static double fix_time = 0; double fix_track = 0; @@ -117,17 +116,7 @@ static void update_gps(struct gps_data_t *gpsdata, static gboolean gps_periodic(gpointer data __attribute__ ((unused))) { - struct timeval timeout; - int ret; - fd_set rfds; - - FD_ZERO(&rfds); - FD_SET(gpsdata->gps_fd, &rfds); - - timeout.tv_sec = 0; - timeout.tv_usec = 100000; - - ret = select(gpsdata->gps_fd + 1, &rfds, NULL, NULL, &timeout); + int ret = gps_waiting(gpsdata); if (ret == -1) { @@ -150,8 +139,7 @@ int main(int argc, char *argv[]) gps_set_raw_hook(gpsdata, update_gps); - gps_query(gpsdata, "w+x\n"); - gps_query(gpsdata, "j=1\n"); + gps_stream(gpsdata, WATCH_ENABLE, NULL); IvyInit ("GPSd2Ivy", "GPSd2Ivy READY", NULL, NULL, NULL, NULL); IvyStart("127.255.255.255"); diff --git a/sw/logalizer/export.ml b/sw/logalizer/export.ml index 2b97f0ee53..ff4d16753e 100644 --- a/sw/logalizer/export.ml +++ b/sw/logalizer/export.ml @@ -93,11 +93,11 @@ let get_last_geo_pos = fun lookup -> and utm_north = float_of_string (lookup "GPS" "utm_north") /. 100. and utm_zone = int_of_string (lookup "GPS" "utm_zone") in Latlong.of_utm WGS84 {utm_x=utm_east; utm_y=utm_north; utm_zone=utm_zone} - else if lookup "NAV_REF" "x" <>"" && lookup "ROTORCRAFT_FP" "east" <>"" then + else if lookup "INS_REF" "ecef_x0" <>"" && lookup "ROTORCRAFT_FP" "east" <>"" then let getf = fun m f -> float_of_string (lookup m f) in - let x0 = getf "NAV_REF" "x" /. 100. - and y0 = getf "NAV_REF" "y" /. 100. - and z0 = getf "NAV_REF" "z" /. 100. + let x0 = getf "INS_REF" "ecef_x0" /. 100. + and y0 = getf "INS_REF" "ecef_y0" /. 100. + and z0 = getf "INS_REF" "ecef_z0" /. 100. and e = getf "ROTORCRAFT_FP" "east" /. 256. and n = getf "ROTORCRAFT_FP" "north" /. 256. and u = getf "ROTORCRAFT_FP" "up" /. 256. in diff --git a/sw/logalizer/plot.ml b/sw/logalizer/plot.ml index ebd5008995..79aac83b22 100644 --- a/sw/logalizer/plot.ml +++ b/sw/logalizer/plot.ml @@ -534,7 +534,7 @@ let rec select_gps_values = function let l = ref [] in for i = 0 to Array.length xs - 1 do let z = truncate (snd zs.(i)) - and a = snd alts.(i) /. 100. in + and a = snd alts.(i) /. 1000. in if z <> 0 && a > 0. then let t = fst xs.(i) and x = snd xs.(i) /. 100. @@ -543,18 +543,18 @@ let rec select_gps_values = function l := (t, of_utm WGS84 utm, a) :: !l done; List.rev !l - | (m, values)::_ when m.Pprz.name = "BOOZ2_GPS" -> + | (m, values)::_ when m.Pprz.name = "GPS_INT" -> let lats = List.assoc "lat" values and lons = List.assoc "lon" values - and alts = List.assoc "alt" values in + and alts = List.assoc "hmsl" values in let l = ref [] in for i = 0 to Array.length lats - 1 do - let a = snd alts.(i) /. 100. in + let a = snd alts.(i) /. 1000. in if a > 0. then let t = fst lats.(i) and lat = snd lats.(i) /. 1e7 and lon = snd lons.(i) /. 1e7 in - let wgs84 = make_geo_deg lat lon in + let wgs84 = make_geo lat lon in l := (t, wgs84, a) :: !l done; List.rev !l