diff --git a/conf/TUDELFT/tudelft_KM_control_panel.xml b/conf/TUDELFT/tudelft_KM_control_panel.xml index 4eb19d920c..26e7a43fe5 100644 --- a/conf/TUDELFT/tudelft_KM_control_panel.xml +++ b/conf/TUDELFT/tudelft_KM_control_panel.xml @@ -68,8 +68,8 @@ - - + + diff --git a/conf/abi.xml b/conf/abi.xml index ac16c25163..5d4008727e 100644 --- a/conf/abi.xml +++ b/conf/abi.xml @@ -72,6 +72,12 @@ + + + + + + diff --git a/conf/airframes/BR/ladybird_kit_bart_bluegiga_optitrack.xml b/conf/airframes/BR/ladybird_kit_bart_bluegiga_optitrack.xml index be86c838b9..f9ff144034 100644 --- a/conf/airframes/BR/ladybird_kit_bart_bluegiga_optitrack.xml +++ b/conf/airframes/BR/ladybird_kit_bart_bluegiga_optitrack.xml @@ -159,13 +159,7 @@
-
- -
- - - - +
@@ -183,8 +177,6 @@
- -
diff --git a/conf/airframes/FLIXR/flixr_zmr250_elle0.xml b/conf/airframes/FLIXR/flixr_zmr250_elle0.xml index ec6077d908..49a1499d0b 100644 --- a/conf/airframes/FLIXR/flixr_zmr250_elle0.xml +++ b/conf/airframes/FLIXR/flixr_zmr250_elle0.xml @@ -180,8 +180,8 @@
- - + + diff --git a/conf/airframes/LS/ls_ladybird_lisa_s_bluegiga_small_gps_messages.xml b/conf/airframes/LS/ls_ladybird_lisa_s_bluegiga_small_gps_messages.xml index 99c55f2732..298fd77b8e 100644 --- a/conf/airframes/LS/ls_ladybird_lisa_s_bluegiga_small_gps_messages.xml +++ b/conf/airframes/LS/ls_ladybird_lisa_s_bluegiga_small_gps_messages.xml @@ -173,13 +173,6 @@
-
- - - - -
-
diff --git a/conf/airframes/LS/ls_quadrotor_bebop_small_gps_messages.xml b/conf/airframes/LS/ls_quadrotor_bebop_small_gps_messages.xml index b5d7e19075..0732035bde 100644 --- a/conf/airframes/LS/ls_quadrotor_bebop_small_gps_messages.xml +++ b/conf/airframes/LS/ls_quadrotor_bebop_small_gps_messages.xml @@ -102,13 +102,6 @@
-
- - - - -
-
diff --git a/conf/airframes/TUDELFT/tudelft_ladylisa_bluegiga_stereoboard.xml b/conf/airframes/TUDELFT/tudelft_ladylisa_bluegiga_stereoboard.xml index 0158d4dd58..110d41ccc0 100644 --- a/conf/airframes/TUDELFT/tudelft_ladylisa_bluegiga_stereoboard.xml +++ b/conf/airframes/TUDELFT/tudelft_ladylisa_bluegiga_stereoboard.xml @@ -181,14 +181,14 @@ - +
- - + +
- - + + @@ -252,6 +252,8 @@ - + + + diff --git a/conf/boards/naze32_rev4.makefile b/conf/boards/naze32_rev4.makefile index 1f63190414..326f398f37 100644 --- a/conf/boards/naze32_rev4.makefile +++ b/conf/boards/naze32_rev4.makefile @@ -16,9 +16,9 @@ $(TARGET).LDSCRIPT=$(SRC_ARCH)/naze32.ld # ----------------------------------------------------------------------- -# default flash mode is via SWD +# default flash mode is via SERIAL (USB plug which is connected to cp210x converter) # other possibilities: DFU-UTIL, JTAG, SWD, STLINK, SERIAL -FLASH_MODE ?= SWD +FLASH_MODE ?= SERIAL # # diff --git a/conf/boards/naze32_rev5.makefile b/conf/boards/naze32_rev5.makefile index faf3bd5d4e..1cc077984f 100644 --- a/conf/boards/naze32_rev5.makefile +++ b/conf/boards/naze32_rev5.makefile @@ -16,9 +16,9 @@ $(TARGET).LDSCRIPT=$(SRC_ARCH)/naze32.ld # ----------------------------------------------------------------------- -# default flash mode is via SWD +# default flash mode is via SERIAL (USB plug which is connected to cp210x converter) # other possibilities: DFU-UTIL, JTAG, SWD, STLINK, SERIAL -FLASH_MODE ?= SWD +FLASH_MODE ?= SERIAL # # diff --git a/conf/firmwares/subsystems/fixedwing/gps_ublox_utm.makefile b/conf/firmwares/subsystems/fixedwing/gps_ublox_utm.makefile index 31da09264a..c7fd2abd9e 100644 --- a/conf/firmwares/subsystems/fixedwing/gps_ublox_utm.makefile +++ b/conf/firmwares/subsystems/fixedwing/gps_ublox_utm.makefile @@ -1,27 +1,3 @@ -# Hey Emacs, this is a -*- makefile -*- -# UBlox LEA 4P +include $(CFG_SHARED)/gps_ublox.makefile -GPS_LED ?= none -UBX_GPS_PORT_LOWER=$(shell echo $(GPS_PORT) | tr A-Z a-z) - -ap.CFLAGS += -DUSE_GPS -DUBX -ap.CFLAGS += -DGPS_LINK=$(UBX_GPS_PORT_LOWER) -ap.CFLAGS += -DUSE_$(GPS_PORT) -ap.CFLAGS += -D$(GPS_PORT)_BAUD=$(GPS_BAUD) - -ifneq ($(GPS_LED),none) - ap.CFLAGS += -DGPS_LED=$(GPS_LED) -endif - -ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_ubx.h\" -ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_ubx.c - -$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c - -sim.CFLAGS += -DUSE_GPS -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" -sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c - -nps.CFLAGS += -DUSE_GPS -nps.srcs += $(SRC_SUBSYSTEMS)/gps.c -nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\" -nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c +$(info Please replace with ) diff --git a/conf/firmwares/subsystems/fixedwing/gps_piksi.makefile b/conf/firmwares/subsystems/shared/gps_piksi.makefile similarity index 100% rename from conf/firmwares/subsystems/fixedwing/gps_piksi.makefile rename to conf/firmwares/subsystems/shared/gps_piksi.makefile diff --git a/conf/firmwares/subsystems/shared/gps_ublox.makefile b/conf/firmwares/subsystems/shared/gps_ublox.makefile index b739a9ada7..d18fc98df7 100644 --- a/conf/firmwares/subsystems/shared/gps_ublox.makefile +++ b/conf/firmwares/subsystems/shared/gps_ublox.makefile @@ -1,5 +1,5 @@ # Hey Emacs, this is a -*- makefile -*- -# UBlox LEA 5H +# UBlox LEA GPS_LED ?= none UBX_GPS_PORT_LOWER=$(shell echo $(GPS_PORT) | tr A-Z a-z) diff --git a/conf/firmwares/subsystems/shared/imu_apogee_mpu9150.makefile b/conf/firmwares/subsystems/shared/imu_apogee_mpu9150.makefile new file mode 100644 index 0000000000..1e4dcecb88 --- /dev/null +++ b/conf/firmwares/subsystems/shared/imu_apogee_mpu9150.makefile @@ -0,0 +1,16 @@ +# Hey Emacs, this is a -*- makefile -*- +# +# Apogee IMU +# + +include $(CFG_SHARED)/imu_apogee.makefile + +IMU_APOGEE_MPU9150_CFLAGS = -DAPOGEE_USE_MPU9150 +IMU_APOGEE_MPU9150_SRCS = peripherals/ak8975.c + +# add it for all targets except sim, fbw and nps +ifeq (,$(findstring $(TARGET),sim fbw nps)) +$(TARGET).CFLAGS += $(IMU_APOGEE_MPU9150_CFLAGS) +$(TARGET).srcs += $(IMU_APOGEE_MPU9150_SRCS) +endif + diff --git a/conf/flash_modes.xml b/conf/flash_modes.xml index 5ebf80e763..d3685e3044 100644 --- a/conf/flash_modes.xml +++ b/conf/flash_modes.xml @@ -23,6 +23,7 @@ + diff --git a/conf/messages.xml b/conf/messages.xml index 70e433a16e..4f26e61fee 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -856,7 +856,6 @@ - @@ -2479,8 +2478,9 @@ - bits 31-22 x position in cm : bits 21-12 y position in cm : bits 11-2 z position in cm : bits 1 and 0 are free - bits 31-22 speed x in cm/s : bits 21-12 speed y in cm/s : bits 11-2 heading in rad*1e2 : bits 1 and 0 are free + bits 31-21 x position in cm : bits 20-10 y position in cm : bits 9-0 z position in cm + bits 31-21 speed x in cm/s : bits 20-10 speed y in cm/s : bits 9-0 speed z in cm/s + diff --git a/conf/modules/airspeed_ets.xml b/conf/modules/airspeed_ets.xml index 2f6dfcff9e..51cb506ba4 100644 --- a/conf/modules/airspeed_ets.xml +++ b/conf/modules/airspeed_ets.xml @@ -25,6 +25,7 @@ +
diff --git a/conf/modules/humid_sht.xml b/conf/modules/humid_sht.xml index 3c0824d9ba..0fbe7cc44a 100644 --- a/conf/modules/humid_sht.xml +++ b/conf/modules/humid_sht.xml @@ -8,6 +8,7 @@ +
diff --git a/conf/modules/temp_temod.xml b/conf/modules/temp_temod.xml index f6cfe7f625..f80f626a86 100644 --- a/conf/modules/temp_temod.xml +++ b/conf/modules/temp_temod.xml @@ -5,6 +5,7 @@ Hygrosens TEMOD-I2C-Rx temperature sensor +
@@ -14,12 +15,12 @@ - TEMOD_DEV ?= i2c0 - TEMOD_DEV_LOWER=$(shell echo $(TEMOD_DEV) | tr A-Z a-z) - TEMOD_DEV_UPPER=$(shell echo $(TEMOD_DEV) | tr a-z A-Z) + TEMOD_I2C_DEV ?= i2c0 + TEMOD_I2C_DEV_LOWER=$(shell echo $(TEMOD_I2C_DEV) | tr A-Z a-z) + TEMOD_I2C_DEV_UPPER=$(shell echo $(TEMOD_I2C_DEV) | tr a-z A-Z) - - + + diff --git a/sw/airborne/arch/sim/sim_gps.c b/sw/airborne/arch/sim/sim_gps.c index 39199eb6e6..e99c70fe40 100644 --- a/sw/airborne/arch/sim/sim_gps.c +++ b/sw/airborne/arch/sim/sim_gps.c @@ -39,7 +39,7 @@ value sim_use_gps_pos(value x, value y, value z, value c, value a, value s, valu //TODO set alt above ellipsoid, use hmsl for now lla_f.alt = Double_val(a); LLA_BFP_OF_REAL(gps.lla_pos, lla_f); - SetBit(gps.valid_fields, GPS_VALID_POS_UTM_BIT); + SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT); gps.utm_pos.east = Int_val(x); gps.utm_pos.north = Int_val(y); diff --git a/sw/airborne/boards/apogee/imu_apogee.c b/sw/airborne/boards/apogee/imu_apogee.c index 3752226e84..19b6fdfe1c 100644 --- a/sw/airborne/boards/apogee/imu_apogee.c +++ b/sw/airborne/boards/apogee/imu_apogee.c @@ -66,11 +66,33 @@ PRINT_CONFIG_VAR(APOGEE_GYRO_RANGE) #endif PRINT_CONFIG_VAR(APOGEE_ACCEL_RANGE) +#if APOGEE_USE_MPU9150 +/** Normal frequency with the default settings + * + * the mag read function should be called at around 50 Hz + */ +#ifndef APOGEE_MAG_FREQ +#define APOGEE_MAG_FREQ 50 +#endif +PRINT_CONFIG_VAR(APOGEE_MAG_FREQ) +/** Mag periodic prescaler + */ +#define MAG_PRESCALER Max(1,((PERIODIC_FREQUENCY)/APOGEE_MAG_FREQ)) +PRINT_CONFIG_VAR(MAG_PRESCALER) + +// mag config will be done later in bypass mode +bool_t configure_mag_slave(Mpu60x0ConfigSet mpu_set, void *mpu); +bool_t configure_mag_slave(Mpu60x0ConfigSet mpu_set __attribute__((unused)), void *mpu __attribute__((unused))) +{ + return TRUE; +} + +#endif + struct ImuApogee imu_apogee; // baro config will be done later in bypass mode bool_t configure_baro_slave(Mpu60x0ConfigSet mpu_set, void *mpu); - bool_t configure_baro_slave(Mpu60x0ConfigSet mpu_set __attribute__((unused)), void *mpu __attribute__((unused))) { return TRUE; @@ -90,6 +112,12 @@ void imu_impl_init(void) imu_apogee.mpu.config.nb_slaves = 1; imu_apogee.mpu.config.slaves[0].configure = &configure_baro_slave; imu_apogee.mpu.config.i2c_bypass = TRUE; +#if APOGEE_USE_MPU9150 + // if using MPU9150, internal mag needs to be configured + ak8975_init(&imu_apogee.ak, &(IMU_APOGEE_I2C_DEV), AK8975_I2C_SLV_ADDR); + imu_apogee.mpu.config.nb_slaves = 2; + imu_apogee.mpu.config.slaves[1].configure = &configure_mag_slave; +#endif } void imu_periodic(void) @@ -97,6 +125,11 @@ void imu_periodic(void) // Start reading the latest gyroscope data mpu60x0_i2c_periodic(&imu_apogee.mpu); +#if APOGEE_USE_MPU9150 + // Start reading internal mag if available + RunOnceEvery(MAG_PRESCALER, ak8975_periodic(&imu_apogee.ak)); +#endif + //RunOnceEvery(10,imu_apogee_downlink_raw()); } @@ -134,5 +167,20 @@ void imu_apogee_event(void) AbiSendMsgIMU_GYRO_INT32(IMU_BOARD_ID, now_ts, &imu.gyro); AbiSendMsgIMU_ACCEL_INT32(IMU_BOARD_ID, now_ts, &imu.accel); } + +#if APOGEE_USE_MPU9150 + ak8975_event(&imu_apogee.ak); + if (imu_apogee.ak.data_available) { + struct Int32Vect3 mag = { + (int32_t)( imu_apogee.ak.data.value[IMU_APOGEE_CHAN_Y]), + (int32_t)(-imu_apogee.ak.data.value[IMU_APOGEE_CHAN_X]), + (int32_t)( imu_apogee.ak.data.value[IMU_APOGEE_CHAN_Z]) + }; + VECT3_COPY(imu.mag_unscaled, mag); + imu_apogee.ak.data_available = FALSE; + imu_scale_mag(&imu); + AbiSendMsgIMU_MAG_INT32(IMU_BOARD_ID, now_ts, &imu.mag); + } +#endif } diff --git a/sw/airborne/boards/apogee/imu_apogee.h b/sw/airborne/boards/apogee/imu_apogee.h index 04a1c75e16..c68c1e1ed4 100644 --- a/sw/airborne/boards/apogee/imu_apogee.h +++ b/sw/airborne/boards/apogee/imu_apogee.h @@ -33,6 +33,9 @@ #include "std.h" #include "generated/airframe.h" #include "subsystems/imu.h" +#if APOGEE_USE_MPU9150 +#include "peripherals/ak8975.h" +#endif #include "peripherals/mpu60x0_i2c.h" @@ -96,6 +99,9 @@ struct ImuApogee { struct Mpu60x0_I2c mpu; +#if APOGEE_USE_MPU9150 + struct Ak8975 ak; +#endif }; extern struct ImuApogee imu_apogee; diff --git a/sw/airborne/boards/elle0_common.h b/sw/airborne/boards/elle0_common.h index fd9527ab1a..702d4a6d61 100644 --- a/sw/airborne/boards/elle0_common.h +++ b/sw/airborne/boards/elle0_common.h @@ -262,55 +262,42 @@ * these directly map to the index number of the 4 adc channels defined above * 4th (index 3) is used for bat monitoring by default */ + +#ifndef USE_ADC_1 +#define USE_ADC_1 1 +#endif + #if USE_ADC_1 -#define AD1_1_CHANNEL 13 +#define AD1_1_CHANNEL 14 #define ADC_1 AD1_1 #define ADC_1_GPIO_PORT GPIOC -#define ADC_1_GPIO_PIN GPIO3 +#define ADC_1_GPIO_PIN GPIO4 #endif #if USE_ADC_2 -#define AD1_2_CHANNEL 10 +#define AD1_2_CHANNEL 15 #define ADC_2 AD1_2 #define ADC_2_GPIO_PORT GPIOC -#define ADC_2_GPIO_PIN GPIO0 +#define ADC_2_GPIO_PIN GPIO5 #endif #if USE_ADC_3 -#define AD1_3_CHANNEL 11 +#define AD1_3_CHANNEL 0 #define ADC_3 AD1_3 -#define ADC_3_GPIO_PORT GPIOC -#define ADC_3_GPIO_PIN GPIO1 +#define ADC_3_GPIO_PORT GPIOA +#define ADC_3_GPIO_PIN GPIO0 #endif #if USE_ADC_4 -#define AD2_1_CHANNEL 15 -#define ADC_4 AD2_1 -#define ADC_4_GPIO_PORT GPIOC -#define ADC_4_GPIO_PIN GPIO5 -#endif - -// Internal ADC for battery enabled by default -#ifndef USE_ADC_5 -#define USE_ADC_5 1 -#endif -#if USE_ADC_5 -#define AD1_4_CHANNEL 14 -#define ADC_5 AD1_4 -#define ADC_5_GPIO_PORT GPIOC -#define ADC_5_GPIO_PIN GPIO4 -#endif - -#if USE_ADC_6 -#define AD2_2_CHANNEL 12 -#define ADC_6 AD2_2 -#define ADC_6_GPIO_PORT GPIOC -#define ADC_6_GPIO_PIN GPIO2 +#define AD1_4_CHANNEL 1 +#define ADC_4 AD1_4 +#define ADC_4_GPIO_PORT GPIOA +#define ADC_4_GPIO_PIN GPIO1 #endif /* allow to define ADC_CHANNEL_VSUPPLY in the airframe file*/ #ifndef ADC_CHANNEL_VSUPPLY -#define ADC_CHANNEL_VSUPPLY ADC_5 +#define ADC_CHANNEL_VSUPPLY ADC_1 #endif #define DefaultVoltageOfAdc(adc) (0.0045*adc) diff --git a/sw/airborne/firmwares/rotorcraft/datalink.c b/sw/airborne/firmwares/rotorcraft/datalink.c index b726d08bcc..1783658e55 100644 --- a/sw/airborne/firmwares/rotorcraft/datalink.c +++ b/sw/airborne/firmwares/rotorcraft/datalink.c @@ -90,7 +90,7 @@ void dl_parse_msg(void) } break; -#if defined USE_NAVIGATION +#ifdef USE_NAVIGATION case DL_BLOCK : { if (DL_BLOCK_ac_id(dl_buffer) != AC_ID) { break; } nav_goto_block(DL_BLOCK_block_id(dl_buffer)); @@ -138,21 +138,21 @@ void dl_parse_msg(void) } break; #endif // RADIO_CONTROL_TYPE_DATALINK -#if defined GPS_DATALINK -#ifdef GPS_USE_DATALINK_SMALL - case DL_REMOTE_GPS_SMALL : +#if USE_GPS +#ifdef GPS_DATALINK + case DL_REMOTE_GPS_SMALL : { // Check if the GPS is for this AC - if (DL_REMOTE_GPS_SMALL_ac_id(dl_buffer) != AC_ID) { - break; - } + if (DL_REMOTE_GPS_SMALL_ac_id(dl_buffer) != AC_ID) { break; } parse_gps_datalink_small( DL_REMOTE_GPS_SMALL_numsv(dl_buffer), DL_REMOTE_GPS_SMALL_pos_xyz(dl_buffer), - DL_REMOTE_GPS_SMALL_speed_xy(dl_buffer)); - break; -#endif - case DL_REMOTE_GPS : + DL_REMOTE_GPS_SMALL_speed_xyz(dl_buffer), + DL_REMOTE_GPS_SMALL_heading(dl_buffer)); + } + break; + + case DL_REMOTE_GPS : { // Check if the GPS is for this AC if (DL_REMOTE_GPS_ac_id(dl_buffer) != AC_ID) { break; } @@ -171,10 +171,11 @@ void dl_parse_msg(void) DL_REMOTE_GPS_ecef_zd(dl_buffer), DL_REMOTE_GPS_tow(dl_buffer), DL_REMOTE_GPS_course(dl_buffer)); - break; -#endif -#if USE_GPS - case DL_GPS_INJECT : + } + break; +#endif // GPS_DATALINK + + case DL_GPS_INJECT : { // Check if the GPS is for this AC if (DL_GPS_INJECT_ac_id(dl_buffer) != AC_ID) { break; } @@ -183,9 +184,10 @@ void dl_parse_msg(void) DL_GPS_INJECT_packet_id(dl_buffer), DL_GPS_INJECT_data_length(dl_buffer), DL_GPS_INJECT_data(dl_buffer) - ); - break; -#endif + ); + } + break; +#endif // USE_GPS case DL_GUIDED_SETPOINT_NED: if (DL_GUIDED_SETPOINT_NED_ac_id(dl_buffer) != AC_ID) { break; } diff --git a/sw/airborne/modules/cam_control/point.c b/sw/airborne/modules/cam_control/point.c index 2be9eae0fb..37d6d5662c 100644 --- a/sw/airborne/modules/cam_control/point.c +++ b/sw/airborne/modules/cam_control/point.c @@ -408,7 +408,7 @@ void vPoint(float fPlaneEast, float fPlaneNorth, float fPlaneAltitude, struct UtmCoor_f utm; utm.east = nav_utm_east0 + fObjectEast; utm.north = nav_utm_north0 + fObjectNorth; - utm.zone = gps.utm_pos.zone; + utm.zone = nav_utm_zone0; struct LlaCoor_f lla; lla_of_utm_f(&lla, &utm); cam_point_lon = lla.lon * (180 / M_PI); diff --git a/sw/airborne/modules/ins/ins_xsens.c b/sw/airborne/modules/ins/ins_xsens.c index 39d9d55969..5c3d3bc9d3 100644 --- a/sw/airborne/modules/ins/ins_xsens.c +++ b/sw/airborne/modules/ins/ins_xsens.c @@ -275,10 +275,7 @@ void ins_xsens_register(void) void ins_xsens_update_gps(struct GpsState *gps_s) { - struct UtmCoor_f utm; - utm.east = gps_s->utm_pos.east / 100.; - utm.north = gps_s->utm_pos.north / 100.; - utm.zone = nav_utm_zone0; + struct UtmCoor_f utm = utm_float_from_gps(gps_s, nav_utm_zone0); utm.alt = gps_s->hmsl / 1000.; // set position diff --git a/sw/airborne/modules/ins/ins_xsens700.c b/sw/airborne/modules/ins/ins_xsens700.c index 102f9b39f3..44f10dd576 100644 --- a/sw/airborne/modules/ins/ins_xsens700.c +++ b/sw/airborne/modules/ins/ins_xsens700.c @@ -206,10 +206,7 @@ void ins_xsens_register(void) void ins_xsens_update_gps(struct GpsState *gps_s) { - struct UtmCoor_f utm; - utm.east = gps_s->utm_pos.east / 100.; - utm.north = gps_s->utm_pos.north / 100.; - utm.zone = nav_utm_zone0; + struct UtmCoor_f utm = utm_float_from_gps(gps_s, nav_utm_zone0); utm.alt = gps_s->hmsl / 1000.; // set position diff --git a/sw/airborne/modules/meteo/humid_sht.c b/sw/airborne/modules/meteo/humid_sht.c index ec89245930..d13bfd87b2 100644 --- a/sw/airborne/modules/meteo/humid_sht.c +++ b/sw/airborne/modules/meteo/humid_sht.c @@ -35,6 +35,14 @@ #include "subsystems/datalink/downlink.h" #include "humid_sht.h" +// sd-log +#if SHT_SDLOG +#include "sdLog.h" +#include "subsystems/chibios-libopencm3/chibios_sdlog.h" +#include "subsystems/gps.h" +bool_t log_sht_started; +#endif + //#include "led.h" #define noACK 0 @@ -299,6 +307,11 @@ void humid_sht_init(void) humid_sht_available = FALSE; humid_sht_status = SHT_IDLE; + +#if SHT_SDLOG + log_sht_started = FALSE; +#endif + } void humid_sht_periodic(void) @@ -338,6 +351,21 @@ void humid_sht_periodic(void) humid_sht_status = SHT_MEASURING_HUMID; DOWNLINK_SEND_SHT_STATUS(DefaultChannel, DefaultDevice, &humidsht, &tempsht, &fhumidsht, &ftempsht); humid_sht_available = FALSE; + +#if SHT_SDLOG + if (pprzLogFile != -1) { + if (!log_sht_started) { + sdLogWriteLog(pprzLogFile, "SHT75: Humid(pct) Temp(degC) H(usec) GPS_fix TOW(ms) Week Lat(1e7rad) Lon(1e7rad) HMSL(mm) gpseed(cm/s) course(1e7rad) climb(cm/s)\n"); + log_sht_started = TRUE; + } + sdLogWriteLog(pprzLogFile, "sht75: %9.4f %9.4f %d %d %d %d %d %d %d %d %d\n", + fhumidsht, ftempsht, + gps.fix, gps.tow, gps.week, + gps.lla_pos.lat, gps.lla_pos.lon, gps.hmsl, + gps.gspeed, gps.course, -gps.ned_vel.z); + } +#endif + } } } diff --git a/sw/airborne/modules/meteo/meteo_france_DAQ.c b/sw/airborne/modules/meteo/meteo_france_DAQ.c index 7f000f5897..59db68ba77 100644 --- a/sw/airborne/modules/meteo/meteo_france_DAQ.c +++ b/sw/airborne/modules/meteo/meteo_france_DAQ.c @@ -106,10 +106,12 @@ void mf_daq_send_report(void) uint8_t foo = 0; int16_t climb = -gps.ned_vel.z; int16_t course = (DegOfRad(gps.course) / ((int32_t)1e6)); + struct UtmCoor_f utm = stateGetPositionEnu_f(); + int32_t east = utm.east * 100; + int32_t north = utm.north * 100; DOWNLINK_SEND_GPS(pprzlog_tp, chibios_sdlog, &gps.fix, - &gps.utm_pos.east, &gps.utm_pos.north, - &course, &gps.hmsl, &gps.gspeed, &climb, - &gps.week, &gps.tow, &gps.utm_pos.zone, &foo); + &east, &north, &course, &gps.hmsl, &gps.gspeed, &climb, + &gps.week, &gps.tow, &utm.zone, &foo); } } diff --git a/sw/airborne/modules/meteo/temp_temod.c b/sw/airborne/modules/meteo/temp_temod.c index 3ab9522534..5ff5ab3088 100644 --- a/sw/airborne/modules/meteo/temp_temod.c +++ b/sw/airborne/modules/meteo/temp_temod.c @@ -33,6 +33,14 @@ #include "pprzlink/messages.h" #include "subsystems/datalink/downlink.h" +// sd-log +#if TEMP_TEMOD_SDLOG +#include "sdLog.h" +#include "subsystems/chibios-libopencm3/chibios_sdlog.h" +#include "subsystems/gps.h" +bool_t log_temod_started; +#endif + float ftmd_temperature; struct i2c_transaction tmd_trans; @@ -50,11 +58,16 @@ struct i2c_transaction tmd_trans; void temod_init(void) { tmd_trans.status = I2CTransDone; + +#if TEMP_TEMOD_SDLOG + log_temod_started = FALSE; +#endif } void temod_periodic(void) { i2c_receive(&TEMOD_I2C_DEV, &tmd_trans, TEMOD_SLAVE_ADDR, 2); + } void temod_event(void) @@ -72,6 +85,24 @@ void temod_event(void) DOWNLINK_SEND_TMP_STATUS(DefaultChannel, DefaultDevice, &tmd_temperature, &ftmd_temperature); tmd_trans.status = I2CTransDone; + + +#if TEMP_TEMOD_SDLOG + if (pprzLogFile != -1) { + if (!log_temod_started) { + sdLogWriteLog(pprzLogFile, "TEMOD: Temp(degC) H(usec) GPS_fix TOW(ms) Week Lat(1e7rad) Lon(1e7rad) HMSL(mm) gpseed(cm/s) course(1e7rad) climb(cm/s)\n"); + log_temod_started = TRUE; + } + else { + sdLogWriteLog(pprzLogFile, "temod: %9.4f %d %d %d %d %d %d %d %d %d\n", + ftmd_temperature, + gps.fix, gps.tow, gps.week, + gps.lla_pos.lat, gps.lla_pos.lon, gps.hmsl, + gps.gspeed, gps.course, -gps.ned_vel.z); + } + } +#endif + } } diff --git a/sw/airborne/modules/sensors/airspeed_ets.c b/sw/airborne/modules/sensors/airspeed_ets.c index 0e137a4ddb..ec5c3d0682 100644 --- a/sw/airborne/modules/sensors/airspeed_ets.c +++ b/sw/airborne/modules/sensors/airspeed_ets.c @@ -83,6 +83,13 @@ PRINT_CONFIG_VAR(AIRSPEED_ETS_I2C_DEV) #endif PRINT_CONFIG_VAR(AIRSPEED_ETS_START_DELAY) +#if AIRSPEED_ETS_SDLOG +#include "sdLog.h" +#include "subsystems/chibios-libopencm3/chibios_sdlog.h" +#include "subsystems/gps.h" +bool_t log_airspeed_ets_started; +#endif + // Global variables uint16_t airspeed_ets_raw; @@ -123,6 +130,10 @@ void airspeed_ets_init(void) airspeed_ets_delay_done = FALSE; SysTimeTimerStart(airspeed_ets_delay_time); + +#if AIRSPEED_ETS_SDLOG + log_airspeed_ets_started = FALSE; +#endif } void airspeed_ets_read_periodic(void) @@ -221,6 +232,23 @@ void airspeed_ets_read_event(void) airspeed_ets = 0.0; } + +#if AIRSPEED_ETS_SDLOG + if (pprzLogFile != -1) { + if (!log_airspeed_ets_started) { + sdLogWriteLog(pprzLogFile, "AIRSPEED_ETS: raw offset airspeed(m/s) GPS_fix TOW(ms) Week Lat(1e7rad) Lon(1e7rad) HMSL(mm) gpseed(cm/s) course(1e7rad) climb(cm/s)\n"); + log_airspeed_ets_started = TRUE; + } + sdLogWriteLog(pprzLogFile, "airspeed_ets: %d %d %8.4f %d %d %d %d %d %d %d %d %d\n", + airspeed_ets_raw, airspeed_ets_offset, airspeed_ets, + gps.fix, gps.tow, gps.week, + gps.lla_pos.lat, gps.lla_pos.lon, gps.hmsl, + gps.gspeed, gps.course, -gps.ned_vel.z); + } +#endif + + + // Transaction has been read airspeed_ets_i2c_trans.status = I2CTransDone; } diff --git a/sw/airborne/peripherals/ak8975.c b/sw/airborne/peripherals/ak8975.c new file mode 100644 index 0000000000..4a2f972c8d --- /dev/null +++ b/sw/airborne/peripherals/ak8975.c @@ -0,0 +1,197 @@ +/* + * Copyright (C) 2015 Xavier Paris, Gautier Hattenberger + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file peripherals/ak8975.c + * + * Driver for the AKM AK8975 magnetometer. + * + */ + +#include "peripherals/ak8975.h" +#include "mcu_periph/sys_time.h" + +#define AK8975_MEAS_TIME_MS 9 + +// Internal calibration coeff +// Currently fetched at startup but not used after +// Only relying on general IMU mag calibration +static float calibration_values[3] = { 0, 0, 0 }; + +static float __attribute__((unused)) get_ajusted_value(const int16_t val, const uint8_t axis) +{ + const float H = (float) val; + const float corr_factor = calibration_values[axis]; + const float Hadj = corr_factor * H; + + return Hadj; +} + +void ak8975_init(struct Ak8975 *ak, struct i2c_periph *i2c_p, uint8_t addr) +{ + /* set i2c_peripheral */ + ak->i2c_p = i2c_p; + /* set i2c address */ + ak->i2c_trans.slave_addr = addr; + + ak->i2c_trans.status = I2CTransDone; + + ak->initialized = FALSE; + ak->status = AK_STATUS_IDLE; + ak->init_status = AK_CONF_UNINIT; + ak->data_available = FALSE; +} + +// Configure +void ak8975_configure(struct Ak8975 *ak) +{ + // Only configure when not busy + if (ak->i2c_trans.status != I2CTransSuccess && ak->i2c_trans.status != I2CTransFailed + && ak->i2c_trans.status != I2CTransDone) { + return; + } + + // Only when succesfull continue with next + if (ak->i2c_trans.status == I2CTransSuccess) { + ak->init_status++; + } + + ak->i2c_trans.status = I2CTransDone; + switch (ak->init_status) { + + case AK_CONF_UNINIT: + // Set AK8975 in fuse ROM access mode to read ADC calibration data + ak->i2c_trans.buf[0] = AK8975_REG_CNTL_ADDR; + ak->i2c_trans.buf[1] = AK8975_MODE_FUSE_ACCESS; + i2c_transmit(ak->i2c_p, &(ak->i2c_trans), ak->i2c_trans.slave_addr, 2); + break; + + case AK_REQ_CALIBRATION: + // Request AK8975 for ADC calibration data + ak->i2c_trans.buf[0] = AK8975_REG_ASASX; + i2c_transceive(ak->i2c_p, &(ak->i2c_trans), ak->i2c_trans.slave_addr, 1, 3); + break; + + case AK_DISABLE_ACCESS_CALIBRATION: + // Read config + for (uint8_t i =0; i<=2; i++) + calibration_values[i] = + ((((float)(ak->i2c_trans.buf[i]) - 128.0f)*0.5f)/128.0f)+1.0f; + + // Set AK8975 in power-down mode to stop read calibration data + ak->i2c_trans.buf[0] = AK8975_REG_CNTL_ADDR; + ak->i2c_trans.buf[1] = AK8975_MODE_POWER_DOWN; + i2c_transmit(ak->i2c_p, &(ak->i2c_trans), ak->i2c_trans.slave_addr, 2); + break; + + case AK_CONF_REQUESTED: + ak->initialized = TRUE; + break; + + default: + break; + } +} + +void ak8975_read(struct Ak8975 *ak) +{ + if (ak->status != AK_STATUS_IDLE) { + return; + } + + // Send single measurement request + ak->i2c_trans.buf[0] = AK8975_REG_CNTL_ADDR; + ak->i2c_trans.buf[1] = AK8975_MODE_SINGLE_MEAS; + i2c_transmit(ak->i2c_p, &(ak->i2c_trans), ak->i2c_trans.slave_addr, 2); + ak->last_meas_time = get_sys_time_msec(); + ak->status = AK_STATUS_MEAS; +} + +// Get raw value +#define RawFromBuf(_buf,_idx) ((int16_t)(_buf[_idx] | (_buf[_idx+1] << 8))) +// Raw is actually a 14 bits signed value +#define Int16FromRaw(_raw) ( (_raw & 0x1FFF) > 0xFFF ? (_raw & 0x1FFF) - 0x2000 : (_raw & 0x0FFF) ) +void ak8975_event(struct Ak8975 *ak) +{ + if (!ak->initialized) { + return; + } + + switch (ak->status) { + + case AK_STATUS_MEAS: + // Send a read data command if measurement time is done (9ms max) + if (ak->i2c_trans.status == I2CTransSuccess && + (get_sys_time_msec() - ak->last_meas_time >= AK8975_MEAS_TIME_MS)) { + ak->i2c_trans.buf[0] = AK8975_REG_ST1_ADDR; + i2c_transceive(ak->i2c_p, &(ak->i2c_trans), ak->i2c_trans.slave_addr, 1, 8); + ak->status++; + } + break; + + case AK_STATUS_READ: + if (ak->i2c_trans.status == I2CTransSuccess) { + // Mag data : + // Status 1 + // 1 byte + // Measures : + // 2 bytes + // 2 bytes + // 2 bytes + // Status 2 + // 1 byte + + // Read status and error bytes + const bool_t dr = ak->i2c_trans.buf[0] & 0x01; // data ready + const bool_t de = ak->i2c_trans.buf[7] & 0x04; // data error + const bool_t mo = ak->i2c_trans.buf[7] & 0x08; // mag overflow + if (de || !dr) { + // read error or data not ready, keep reading + break; + } + if (mo) { + // overflow, back to idle + ak->status = AK_STATUS_IDLE; + } + // Copy the data + int16_t val; + val = RawFromBuf(ak->i2c_trans.buf, 1); + ak->data.vect.x = Int16FromRaw(val); + val = RawFromBuf(ak->i2c_trans.buf, 3); + ak->data.vect.y = Int16FromRaw(val); + val = RawFromBuf(ak->i2c_trans.buf, 5); + ak->data.vect.z = Int16FromRaw(val); + ak->data_available = TRUE; + // End reading, back to idle + ak->status = AK_STATUS_IDLE; + break; + } + break; + + default: + if (ak->i2c_trans.status == I2CTransSuccess || ak->i2c_trans.status == I2CTransFailed) { + // Goto idle + ak->i2c_trans.status = I2CTransDone; + ak->status = AK_STATUS_IDLE; + } + break; + } +} + diff --git a/sw/airborne/peripherals/ak8975.h b/sw/airborne/peripherals/ak8975.h new file mode 100644 index 0000000000..bf06c8fcc5 --- /dev/null +++ b/sw/airborne/peripherals/ak8975.h @@ -0,0 +1,92 @@ +/* + * Copyright (C) 2015 Xavier Paris, Gautier Hattenberger + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file peripherals/ak8975.h + * + * Driver for the AKM AK8975 magnetometer. + */ + +#ifndef AK8975_H +#define AK8975_H + +#include "std.h" +#include "mcu_periph/i2c.h" +#include "math/pprz_algebra_int.h" + +/* Address and register definitions */ +#define AK8975_I2C_SLV_ADDR (0x0C<<1) +#define AK8975_REG_ST1_ADDR 0x02 +#define AK8975_REG_CNTL_ADDR 0x0A +#define AK8975_REG_ASASX 0x10 +#define AK8975_MODE_FUSE_ACCESS 0b00001111 +#define AK8975_MODE_POWER_DOWN 0b00000000 +#define AK8975_MODE_SINGLE_MEAS 0b00000001 + +/** config status states */ +enum Ak8975ConfStatus { + AK_CONF_UNINIT, + AK_REQ_CALIBRATION, + AK_DISABLE_ACCESS_CALIBRATION, + AK_CONF_REQUESTED +}; + +/** Normal status states */ +enum Ak8975Status { + AK_STATUS_IDLE, + AK_STATUS_MEAS, + AK_STATUS_READ, + AK_STATUS_DONE +}; + +struct Ak8975 { + struct i2c_periph *i2c_p; ///< peripheral used for communcation + struct i2c_transaction i2c_trans; ///< i2c transaction used for communication with the ak8936 + bool_t initialized; ///< config done flag + + enum Ak8975Status status; ///< main status + enum Ak8975ConfStatus init_status; ///< init status + uint32_t last_meas_time; ///< last measurement time in ms + + volatile bool_t data_available; ///< data ready flag + union { + struct Int16Vect3 vect; ///< data vector in mag coordinate system + int16_t value[3]; ///< data values accessible by channel index + } data; +}; + + +// Functions +extern void ak8975_init(struct Ak8975 *ak, struct i2c_periph *i2c_p, uint8_t addr); +extern void ak8975_configure(struct Ak8975 *ak); +extern void ak8975_event(struct Ak8975 *ak); +extern void ak8975_read(struct Ak8975 *ak); + +/// convenience function: read or start configuration if not already initialized +static inline void ak8975_periodic(struct Ak8975 *ak) +{ + if (ak->initialized) { + ak8975_read(ak); + } else { + ak8975_configure(ak); + } +} + +#endif /* AK8975_H */ diff --git a/sw/airborne/peripherals/lsm303d_regs.h b/sw/airborne/peripherals/lsm303d_regs.h new file mode 100644 index 0000000000..5d55b46744 --- /dev/null +++ b/sw/airborne/peripherals/lsm303d_regs.h @@ -0,0 +1,152 @@ +/* + * Copyright (C) 2015 Felix Ruess + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file peripherals/lsm303d_regs.h + * Register defs for ST LSM303D 3D accelerometer and magnetometer. + * + * Has an I2C and SPI interface. + * The LSM303D has linear acceleration full scales of ±2g / ±4g / ±6g / ±8g / ±16g + * and a magnetic field full scale of ±2 / ±4 / ±8 / ±12 gauss. + */ + +#ifndef LSM303D_REGS_H +#define LSM303D_REGS_H + +/* Registers */ +#define LSM303D_REG_TEMP_OUT_L 0x05 +#define LSM303D_REG_TEMP_OUT_H 0x06 +#define LSM303D_REG_STATUS_M 0x07 +#define LSM303D_REG_OUT_X_L_M 0x08 +#define LSM303D_REG_OUT_X_H_M 0x09 +#define LSM303D_REG_OUT_Y_L_M 0x0A +#define LSM303D_REG_OUT_Y_H_M 0x0B +#define LSM303D_REG_OUT_Z_L_M 0x0C +#define LSM303D_REG_OUT_Z_H_M 0x0D + +#define LSM303D_REG_WHO_AM_I 0x0F + +#define LSM303D_REG_INT_CTRL_M 0x12 +#define LSM303D_REG_INT_SRC_M 0x13 +#define LSM303D_REG_INT_THS_L_M 0x14 +#define LSM303D_REG_INT_THS_H_M 0x15 +#define LSM303D_REG_OFFSET_X_L_M 0x16 +#define LSM303D_REG_OFFSET_X_H_M 0x17 +#define LSM303D_REG_OFFSET_Y_L_M 0x18 +#define LSM303D_REG_OFFSET_Y_H_M 0x19 +#define LSM303D_REG_OFFSET_Z_L_M 0x1A +#define LSM303D_REG_OFFSET_Z_H_M 0x1B +#define LSM303D_REG_REFERENCE_X 0x1C +#define LSM303D_REG_REFERENCE_Y 0x1D +#define LSM303D_REG_REFERENCE_Z 0x1E +#define LSM303D_REG_CTRL0 0x1F +#define LSM303D_REG_CTRL1 0x20 +#define LSM303D_REG_CTRL2 0x21 +#define LSM303D_REG_CTRL3 0x22 +#define LSM303D_REG_CTRL4 0x23 +#define LSM303D_REG_CTRL5 0x24 +#define LSM303D_REG_CTRL6 0x25 +#define LSM303D_REG_CTRL7 0x26 +#define LSM303D_REG_STATUS_A 0x27 +#define LSM303D_REG_OUT_X_L_A 0x28 +#define LSM303D_REG_OUT_X_H_A 0x29 +#define LSM303D_REG_OUT_Y_L_A 0x2A +#define LSM303D_REG_OUT_Y_H_A 0x2B +#define LSM303D_REG_OUT_Z_L_A 0x2C +#define LSM303D_REG_OUT_Z_H_A 0x2D +#define LSM303D_REG_FIFO_CTRL 0x2E +#define LSM303D_REG_FIFO_SRC 0x2F +#define LSM303D_REG_IG_CFG1 0x30 +#define LSM303D_REG_IG_SRC1 0x31 +#define LSM303D_REG_IG_THS1 0x32 +#define LSM303D_REG_IG_DUR1 0x33 +#define LSM303D_REG_IG_CFG2 0x34 +#define LSM303D_REG_IG_SRC2 0x35 +#define LSM303D_REG_IG_THS2 0x36 +#define LSM303D_REG_IG_DUR2 0x37 +#define LSM303D_REG_CLICK_CFG 0x38 +#define LSM303D_REG_CLICK_SRC 0x39 +#define LSM303D_REG_CLICK_THS 0x3A +#define LSM303D_REG_TIME_LIMIT 0x3B +#define LSM303D_REG_TIME_LATENCY 0x3C +#define LSM303D_REG_TIME_WINDOW 0x3D +#define LSM303D_REG_ACT_THS 0x3E +#define LSM303D_REG_ACT_DUR 0x3F + +/** LSM303D device identifier in LSM303D_REG_WHO_AM_I */ +#define LSM303D_WHO_AM_I 0x49 + +/** LSM303D acceleration data rate (bits 4-7 in LSM303D_REG_CTRL1) */ +enum Lsm303dAccelRates { + LSM303D_ACC_RATE_OFF = 0x0, + LSM303D_ACC_RATE_3_125HZ = 0x1, + LSM303D_ACC_RATE_6_25HZ = 0x2, + LSM303D_ACC_RATE_12_5HZ = 0x3, + LSM303D_ACC_RATE_25HZ = 0x4, + LSM303D_ACC_RATE_50HZ = 0x5, + LSM303D_ACC_RATE_100HZ = 0x6, + LSM303D_ACC_RATE_200HZ = 0x7, + LSM303D_ACC_RATE_400HZ = 0x8, + LSM303D_ACC_RATE_800HZ = 0x9, + LSM303D_ACC_RATE_1600HZ = 0xA +}; + +/* Bit definitions for LSM303D_REG_CTRL1 */ +#define LSM303D_AXEN (1 << 0) +#define LSM303D_AYEN (1 << 1) +#define LSM303D_AZEN (1 << 3) + +/** LSM303D accelerometer anti-alias filter bandwidth (bits 6-7 in LSM303D_REG_CTRL2) */ +enum Lsm303dAccelBandwidth { + LSM303D_ACC_BW_773HZ = 0, + LSM303D_ACC_BW_194HZ = 1, + LSM303D_ACC_BW_362HZ = 2, + LSM303D_ACC_BW_50HZ = 3 +}; + +/** LSM303D accelerometer anti-alias filter bandwidth (bits 3-5 in LSM303D_REG_CTRL2) */ +enum Lsm303dAccelRanges { + LSM303D_ACC_RANGE_2G = 0x0, + LSM303D_ACC_RANGE_4G = 0x1, + LSM303D_ACC_RANGE_6G = 0x2, + LSM303D_ACC_RANGE_8G = 0x3, + LSM303D_ACC_RANGE_16G = 0x4 +}; + +/** LSM303D magnetic data rate (bits 2-4 in LSM303D_REG_CTRL5) */ +enum Lsm303dMagRates { + LSM303D_MAG_RATE_3_125HZ = 0x0, + LSM303D_MAG_RATE_6_25HZ = 0x1, + LSM303D_MAG_RATE_12_5HZ = 0x2, + LSM303D_MAG_RATE_25HZ = 0x3, + LSM303D_MAG_RATE_50HZ = 0x4, + LSM303D_MAG_RATE_100HZ = 0x5 +}; + +/** LSM303D magnetic range (bits 5-6 in LSM303D_REG_CTRL6) */ +enum Lsm303dMagRange { + LSM303D_MAG_RANGE_2GAUSS = 0, + LSM303D_MAG_RANGE_4GAUSS = 1, + LSM303D_MAG_RANGE_8GAUSS = 2, + LSM303D_MAG_RANGE_12GAUSS = 3, +}; + +#endif // LSM303D_REGS_H diff --git a/sw/airborne/peripherals/mpu60x0.c b/sw/airborne/peripherals/mpu60x0.c index 70cf0e0233..4bfb5ffdff 100644 --- a/sw/airborne/peripherals/mpu60x0.c +++ b/sw/airborne/peripherals/mpu60x0.c @@ -44,6 +44,7 @@ void mpu60x0_set_default_config(struct Mpu60x0Config *c) */ c->nb_bytes = 15; c->nb_slaves = 0; + c->nb_slave_init = 0; c->i2c_bypass = FALSE; } diff --git a/sw/airborne/peripherals/mpu60x0.h b/sw/airborne/peripherals/mpu60x0.h index 419b5b59e1..693068ad3c 100644 --- a/sw/airborne/peripherals/mpu60x0.h +++ b/sw/airborne/peripherals/mpu60x0.h @@ -46,6 +46,11 @@ /// Default clock: PLL with X gyro reference #define MPU60X0_DEFAULT_CLK_SEL 1 +// Default number of I2C slaves +#ifndef MPU60X0_I2C_NB_SLAVES +#define MPU60X0_I2C_NB_SLAVES 5 +#endif + enum Mpu60x0ConfStatus { MPU60X0_CONF_UNINIT, MPU60X0_CONF_RESET, @@ -87,7 +92,8 @@ struct Mpu60x0Config { bool_t i2c_bypass; uint8_t nb_slaves; ///< number of used I2C slaves - struct Mpu60x0I2cSlave slaves[5]; ///< I2C slaves + uint8_t nb_slave_init; ///< number of already configured/initialized slaves + struct Mpu60x0I2cSlave slaves[MPU60X0_I2C_NB_SLAVES]; ///< I2C slaves enum Mpu60x0MstClk i2c_mst_clk; ///< MPU I2C master clock speed uint8_t i2c_mst_delay; ///< MPU I2C slaves delayed sample rate }; diff --git a/sw/airborne/peripherals/mpu60x0_i2c.c b/sw/airborne/peripherals/mpu60x0_i2c.c index 7157ea10e3..1a403896cc 100644 --- a/sw/airborne/peripherals/mpu60x0_i2c.c +++ b/sw/airborne/peripherals/mpu60x0_i2c.c @@ -130,7 +130,7 @@ void mpu60x0_i2c_event(struct Mpu60x0_I2c *mpu) } } -/** @todo: only one slave so far. */ +/** configure the registered I2C slaves */ bool_t mpu60x0_configure_i2c_slaves(Mpu60x0ConfigSet mpu_set, void *mpu) { struct Mpu60x0_I2c *mpu_i2c = (struct Mpu60x0_I2c *)(mpu); @@ -150,8 +150,15 @@ bool_t mpu60x0_configure_i2c_slaves(Mpu60x0ConfigSet mpu_set, void *mpu) mpu_i2c->slave_init_status++; break; case MPU60X0_I2C_CONF_SLAVES_CONFIGURE: - /* configure each slave. TODO: currently only one */ - if (mpu_i2c->config.slaves[0].configure(mpu_set, mpu)) { + /* configure each slave until all nb_slaves are done */ + if (mpu_i2c->config.nb_slave_init < mpu_i2c->config.nb_slaves && mpu_i2c->config.nb_slave_init < MPU60X0_I2C_NB_SLAVES) { + // proceed to next slave if configure for current one returns true + if (mpu_i2c->config.slaves[mpu_i2c->config.nb_slave_init].configure(mpu_set, mpu)) { + mpu_i2c->config.nb_slave_init++; + } + } + else { + /* all slave devies configured, continue MPU side configuration of I2C slave stuff */ mpu_i2c->slave_init_status++; } break; diff --git a/sw/airborne/peripherals/mpu60x0_spi.c b/sw/airborne/peripherals/mpu60x0_spi.c index 5b53f42b9d..b85885f069 100644 --- a/sw/airborne/peripherals/mpu60x0_spi.c +++ b/sw/airborne/peripherals/mpu60x0_spi.c @@ -148,7 +148,7 @@ void mpu60x0_spi_event(struct Mpu60x0_Spi *mpu) } } -/** @todo: only one slave so far. */ +/** configure the registered I2C slaves */ bool_t mpu60x0_configure_i2c_slaves(Mpu60x0ConfigSet mpu_set, void *mpu) { struct Mpu60x0_Spi *mpu_spi = (struct Mpu60x0_Spi *)(mpu); @@ -175,8 +175,15 @@ bool_t mpu60x0_configure_i2c_slaves(Mpu60x0ConfigSet mpu_set, void *mpu) mpu_spi->slave_init_status++; break; case MPU60X0_SPI_CONF_SLAVES_CONFIGURE: - /* configure first slave, only one slave supported so far */ - if (mpu_spi->config.slaves[0].configure(mpu_set, mpu)) { + /* configure each slave until all nb_slaves are done */ + if (mpu_spi->config.nb_slave_init < mpu_spi->config.nb_slaves && mpu_spi->config.nb_slave_init < MPU60X0_I2C_NB_SLAVES) { + // proceed to next slave if configure for current one returns true + if (mpu_spi->config.slaves[mpu_spi->config.nb_slave_init].configure(mpu_set, mpu)) { + mpu_spi->config.nb_slave_init++; + } + } + else { + /* all slave devies configured, continue MPU side configuration of I2C slave stuff */ mpu_spi->slave_init_status++; } break; diff --git a/sw/airborne/peripherals/mpu9250.c b/sw/airborne/peripherals/mpu9250.c index a2326bbc2e..a65f65ca62 100644 --- a/sw/airborne/peripherals/mpu9250.c +++ b/sw/airborne/peripherals/mpu9250.c @@ -45,6 +45,7 @@ void mpu9250_set_default_config(struct Mpu9250Config *c) */ c->nb_bytes = 15; c->nb_slaves = 0; + c->nb_slave_init = 0; c->i2c_bypass = FALSE; } diff --git a/sw/airborne/peripherals/mpu9250.h b/sw/airborne/peripherals/mpu9250.h index 81ca095cd5..fd8daa8057 100644 --- a/sw/airborne/peripherals/mpu9250.h +++ b/sw/airborne/peripherals/mpu9250.h @@ -47,6 +47,11 @@ /// Default clock: PLL with X gyro reference #define MPU9250_DEFAULT_CLK_SEL 1 +// Default number of I2C slaves +#ifndef MPU9250_I2C_NB_SLAVES +#define MPU9250_I2C_NB_SLAVES 5 +#endif + enum Mpu9250ConfStatus { MPU9250_CONF_UNINIT, MPU9250_CONF_RESET, @@ -90,7 +95,8 @@ struct Mpu9250Config { bool_t i2c_bypass; uint8_t nb_slaves; ///< number of used I2C slaves - struct Mpu9250I2cSlave slaves[5]; ///< I2C slaves + uint8_t nb_slave_init; ///< number of already configured/initialized slaves + struct Mpu9250I2cSlave slaves[MPU9250_I2C_NB_SLAVES]; ///< I2C slaves enum Mpu9250MstClk i2c_mst_clk; ///< MPU I2C master clock speed uint8_t i2c_mst_delay; ///< MPU I2C slaves delayed sample rate }; diff --git a/sw/airborne/peripherals/mpu9250_i2c.c b/sw/airborne/peripherals/mpu9250_i2c.c index 158ebdc22e..9cd52538a3 100644 --- a/sw/airborne/peripherals/mpu9250_i2c.c +++ b/sw/airborne/peripherals/mpu9250_i2c.c @@ -163,7 +163,7 @@ bool_t imu_mpu9250_configure_mag_slave(Mpu9250ConfigSet mpu_set __attribute__((u } } -/** @todo: only one slave so far. */ +/** configure the registered I2C slaves */ bool_t mpu9250_configure_i2c_slaves(Mpu9250ConfigSet mpu_set, void *mpu) { struct Mpu9250_I2c *mpu_i2c = (struct Mpu9250_I2c *)(mpu); @@ -183,8 +183,15 @@ bool_t mpu9250_configure_i2c_slaves(Mpu9250ConfigSet mpu_set, void *mpu) mpu_i2c->slave_init_status++; break; case MPU9250_I2C_CONF_SLAVES_CONFIGURE: - /* configure each slave. TODO: currently only one */ - if (mpu_i2c->config.slaves[0].configure(mpu_set, mpu)) { + /* configure each slave until all nb_slaves are done */ + if (mpu_i2c->config.nb_slave_init < mpu_i2c->config.nb_slaves && mpu_i2c->config.nb_slave_init < MPU9250_I2C_NB_SLAVES) { + // proceed to next slave if configure for current one returns true + if (mpu_i2c->config.slaves[mpu_i2c->config.nb_slave_init].configure(mpu_set, mpu)) { + mpu_i2c->config.nb_slave_init++; + } + } + else { + /* all slave devies configured, continue MPU side configuration of I2C slave stuff */ mpu_i2c->slave_init_status++; } break; diff --git a/sw/airborne/peripherals/mpu9250_spi.c b/sw/airborne/peripherals/mpu9250_spi.c index 6af6631f6b..ac6ed5667f 100644 --- a/sw/airborne/peripherals/mpu9250_spi.c +++ b/sw/airborne/peripherals/mpu9250_spi.c @@ -145,7 +145,7 @@ void mpu9250_spi_event(struct Mpu9250_Spi *mpu) } } -/** @todo: only one slave so far. */ +/** configure the registered I2C slaves */ bool_t mpu9250_configure_i2c_slaves(Mpu9250ConfigSet mpu_set, void *mpu) { struct Mpu9250_Spi *mpu_spi = (struct Mpu9250_Spi *)(mpu); @@ -172,8 +172,15 @@ bool_t mpu9250_configure_i2c_slaves(Mpu9250ConfigSet mpu_set, void *mpu) mpu_spi->slave_init_status++; break; case MPU9250_SPI_CONF_SLAVES_CONFIGURE: - /* configure first slave, only one slave supported so far */ - if (mpu_spi->config.slaves[0].configure(mpu_set, mpu)) { + /* configure each slave until all nb_slaves are done */ + if (mpu_spi->config.nb_slave_init < mpu_spi->config.nb_slaves && mpu_spi->config.nb_slave_init < MPU9250_I2C_NB_SLAVES) { + // proceed to next slave if configure for current one returns true + if (mpu_spi->config.slaves[mpu_spi->config.nb_slave_init].configure(mpu_set, mpu)) { + mpu_spi->config.nb_slave_init++; + } + } + else { + /* all slave devies configured, continue MPU side configuration of I2C slave stuff */ mpu_spi->slave_init_status++; } break; diff --git a/sw/airborne/subsystems/abi_sender_ids.h b/sw/airborne/subsystems/abi_sender_ids.h index 0750518d7b..29254371dd 100644 --- a/sw/airborne/subsystems/abi_sender_ids.h +++ b/sw/airborne/subsystems/abi_sender_ids.h @@ -230,5 +230,12 @@ #define PX4FLOW_VELOCITY_ID 17 #endif +/* + * IDs of RSSI measurements (message 13) + */ +#ifndef RSSI_BLUEGIGA_ID +#define RSSI_BLUEGIGA_ID 1 +#endif + #endif /* ABI_SENDER_IDS_H */ diff --git a/sw/airborne/subsystems/datalink/bluegiga.c b/sw/airborne/subsystems/datalink/bluegiga.c index 8985c5b498..3b8a2c9591 100644 --- a/sw/airborne/subsystems/datalink/bluegiga.c +++ b/sw/airborne/subsystems/datalink/bluegiga.c @@ -30,9 +30,19 @@ #include "mcu_periph/gpio.h" #include "mcu_periph/spi.h" +#ifdef MODEM_LED +#include "led.h" +#endif + +#include "subsystems/abi.h" + // for memset #include +//#include "subsystems/datalink/telemetry_common.h" +//#include "subsystems/datalink/telemetry.h" +#include "generated/periodic_telemetry.h" + #ifndef BLUEGIGA_SPI_DEV #error "bluegiga: must define a BLUEGIGA_SPI_DEV" #endif @@ -46,14 +56,18 @@ #define BLUEGIGA_DRDY_GPIO_PIN SUPERBITRF_DRDY_PIN #endif +#define TxStrengthOfSender(x) (x[1]) +#define RssiOfSender(x) (x[2]) +#define Pprz_StxOfMsg(x) (x[3]) +#define SenderIdOfMsg(x) (x[5]) + enum BlueGigaStatus coms_status; struct bluegiga_periph bluegiga_p; struct spi_transaction bluegiga_spi; -signed char bluegiga_rssi[256]; // values initialized with 127 -unsigned char telemetry_copy[20]; +uint8_t broadcast_msg[20]; -void bluegiga_load_tx(struct bluegiga_periph *p); +void bluegiga_load_tx(struct bluegiga_periph *p, struct spi_transaction *trans); void bluegiga_transmit(struct bluegiga_periph *p, uint8_t data); void bluegiga_receive(struct spi_transaction *trans); @@ -80,6 +94,8 @@ static int dev_char_available(struct bluegiga_periph *p) { return bluegiga_ch_available(p); } + +// note, need to run dev_char_available first static uint8_t dev_get_byte(struct bluegiga_periph *p) { uint8_t ret = p->rx_buf[p->rx_extract_idx]; @@ -115,7 +131,7 @@ static void send_bluegiga(struct transport_tx *trans, struct link_device *dev) if (now_ts > last_ts) { uint32_t rate = 1000 * bluegiga_p.bytes_recvd_since_last / (now_ts - last_ts); - pprz_msg_send_BLUEGIGA(trans, dev, AC_ID, &rate, 20, telemetry_copy); + pprz_msg_send_BLUEGIGA(trans, dev, AC_ID, &rate); bluegiga_p.bytes_recvd_since_last = 0; last_ts = now_ts; @@ -140,7 +156,7 @@ void bluegiga_init(struct bluegiga_periph *p) bluegiga_spi.cpha = SPICphaEdge2; bluegiga_spi.dss = SPIDss8bit; bluegiga_spi.bitorder = SPIMSBFirst; - bluegiga_spi.cdiv = SPIDiv64; + bluegiga_spi.cdiv = SPIDiv256; bluegiga_spi.after_cb = (SPICallback) trans_cb; // Configure generic link device @@ -157,18 +173,14 @@ void bluegiga_init(struct bluegiga_periph *p) p->tx_insert_idx = 0; p->tx_extract_idx = 0; - for (int i = 0; i < bluegiga_spi.input_length; i++) { - p->work_rx[i] = 0; - } - for (int i = 0; i < bluegiga_spi.output_length; i++) { - p->work_tx[i] = 0; - } - for (int i = 0; i < 255; i++) { - bluegiga_rssi[i] = 127; - } + memset(p->work_rx, 0, bluegiga_spi.input_length); + memset(p->work_tx, 0, bluegiga_spi.output_length); + + memset(broadcast_msg, 0, 19); p->bytes_recvd_since_last = 0; p->end_of_msg = p->tx_insert_idx; + p->connected = 0; // set DRDY interrupt pin for spi master triggered on falling edge gpio_setup_output(BLUEGIGA_DRDY_GPIO, BLUEGIGA_DRDY_GPIO_PIN); @@ -194,10 +206,11 @@ void bluegiga_transmit(struct bluegiga_periph *p, uint8_t data) } /* Load waiting data into tx peripheral buffer */ -void bluegiga_load_tx(struct bluegiga_periph *p) +void bluegiga_load_tx(struct bluegiga_periph *p, struct spi_transaction *trans) { + uint8_t packet_len; // check data available in buffer to send - uint8_t packet_len = ((p->end_of_msg - p->tx_extract_idx + BLUEGIGA_BUFFER_SIZE) % BLUEGIGA_BUFFER_SIZE); + packet_len = ((p->end_of_msg - p->tx_extract_idx + BLUEGIGA_BUFFER_SIZE) % BLUEGIGA_BUFFER_SIZE); if (packet_len > 19) { packet_len = 19; } @@ -214,7 +227,7 @@ void bluegiga_load_tx(struct bluegiga_periph *p) bluegiga_increment_buf(&p->tx_extract_idx, packet_len); // clear unused bytes - for (i = packet_len + 1; i < bluegiga_spi.output_length; i++) { + for (i = packet_len + 1; i < trans->output_length; i++) { p->work_tx[i] = 0; } @@ -235,97 +248,104 @@ void bluegiga_receive(struct spi_transaction *trans) for (uint8_t i = 0; i < trans->output_length; i++) { trans->output_buf[i] = 0; } + } else if (coms_status == BLUEGIGA_SENDING_BROADCAST) { + // sending second half of broadcast message + for (uint8_t i = 0; i < broadcast_msg[0]; i++) { + trans->output_buf[i] = broadcast_msg[i]; + } + coms_status = BLUEGIGA_SENDING; + return; } /* - * 0xff communication lost with ground station - * 0xfe RSSI value from broadcaster - * 0xfd Change in broadcast mode - * 0xfc Receive all recorded RSSI - * <=20 Data package from ground station + * >235 data package from broadcast mode + * 0x50 communication lost with ground station + * 0x51 interrupt handled + * <20 data package from connection */ uint8_t packet_len = 0; uint8_t read_offset = 0; switch (trans->input_buf[0]) { - case 0xff: // communication lost with ground station -#ifdef MODEM_LED - LED_OFF(MODEM_LED); -#endif - coms_status = BLUEGIGA_UNINIT; - gpio_set(BLUEGIGA_DRDY_GPIO, BLUEGIGA_DRDY_GPIO_PIN); // Reset interrupt pin - break; - case 0xfe: // RSSI value from broadcaster - bluegiga_rssi[trans->input_buf[1]] = trans->input_buf[2]; - packet_len = trans->input_buf[3]; - read_offset = 4; - break; - case 0xfd: // Change in broadcast mode - gpio_set(BLUEGIGA_DRDY_GPIO, BLUEGIGA_DRDY_GPIO_PIN); // Reset interrupt pin - - // fetch scan status - if (trans->input_buf[1] == 1) { - coms_status = BLUEGIGA_BROADCASTING; + case 0x50: // communication status changed + bluegiga_p.connected = trans->input_buf[1]; + if (bluegiga_p.connected) { + //telemetry_mode_Main = TELEMETRY_PROCESS_Main; } else { - coms_status = BLUEGIGA_UNINIT; + //telemetry_mode_Main = NB_TELEMETRY_MODES; // send no periodic telemetry } + coms_status = BLUEGIGA_IDLE; break; - case 0xfc: // Receive all recorded RSSI - for (uint8_t i = 0; i < trans->input_buf[1]; i++) { - bluegiga_rssi[trans->input_buf[2] + i] = trans->input_buf[3 + i]; - } + case 0x51: // Interrupt handled + gpio_set(BLUEGIGA_DRDY_GPIO, BLUEGIGA_DRDY_GPIO_PIN); // Reset interrupt pin break; default: - packet_len = trans->input_buf[0]; // length of transmitted message - read_offset = 1; + coms_status = BLUEGIGA_IDLE; + // compute length of transmitted message + if (trans->input_buf[0] < trans->input_length) { // normal connection mode + packet_len = trans->input_buf[0]; + read_offset = 1; + } else if (trans->input_buf[0] > 0xff - trans->input_length) { // broadcast mode + packet_len = 0xff - trans->input_buf[0]; + + int8_t tx_strength = TxStrengthOfSender(trans->input_buf); + int8_t rssi = RssiOfSender(trans->input_buf); + uint8_t ac_id = SenderIdOfMsg(trans->input_buf); + + if (Pprz_StxOfMsg(trans->input_buf) == STX) { + AbiSendMsgRSSI(RSSI_BLUEGIGA_ID, ac_id, tx_strength, rssi); + } + + read_offset = 3; + } } // handle incoming datalink message - if (packet_len > 0 && packet_len <= trans->input_length) { + if (packet_len > 0 && packet_len <= trans->input_length - read_offset) { +#ifdef MODEM_LED + LED_TOGGLE(MODEM_LED); +#endif // Handle received message for (uint8_t i = 0; i < packet_len; i++) { bluegiga_p.rx_buf[(bluegiga_p.rx_insert_idx + i) % BLUEGIGA_BUFFER_SIZE] = trans->input_buf[i + read_offset]; } bluegiga_increment_buf(&bluegiga_p.rx_insert_idx, packet_len); bluegiga_p.bytes_recvd_since_last += packet_len; - coms_status = BLUEGIGA_IDLE; - - for (uint8_t i = 0; i < trans->input_length; i++) { - telemetry_copy[i] = trans->input_buf[i]; - } - } else { - coms_status = BLUEGIGA_IDLE; } // load next message to be sent into work buffer, needs to be loaded before calling spi_slave_register - bluegiga_load_tx(&bluegiga_p); + bluegiga_load_tx(&bluegiga_p, trans); // register spi slave read for next transaction - spi_slave_register(&(BLUEGIGA_SPI_DEV), &bluegiga_spi); + spi_slave_register(&(BLUEGIGA_SPI_DEV), trans); } } -/* command bluetooth to switch to active scan mode to get rssi values from neighbouring drones */ -void bluegiga_scan(struct bluegiga_periph *p) +/* Send data for broadcast message to the bluegiga module + * maximum size of message is 22 bytes + */ +void bluegiga_broadcast_msg(struct bluegiga_periph *p, char *msg, uint8_t msg_len) { + if (msg_len == 0 || msg_len > 22) { + return; + } - memset(p->work_tx, 0, 20); - p->work_tx[0] = 0xfd; // change broadcast mode header + uint8_t max_length = 20; + p->work_tx[0] = msg_len; - coms_status = BLUEGIGA_SENDING; - - // trigger bluegiga to read direct command - gpio_clear(BLUEGIGA_DRDY_GPIO, BLUEGIGA_DRDY_GPIO_PIN); // set interrupt -} - -/* Request list of all recorded RSSI */ -void bluegiga_request_all_rssi(struct bluegiga_periph *p) -{ - - memset(p->work_tx, 0, 20); - p->work_tx[0] = 0xfc; - - coms_status = BLUEGIGA_SENDING; + if (msg_len < max_length) { + for (uint8_t i = 0; i < msg_len; i++) { + p->work_tx[i + 1] = msg[i]; + } + coms_status = BLUEGIGA_SENDING; + } else { + for (uint8_t i = 0; i < max_length - 1; i++) { + p->work_tx[i + 1] = msg[i]; + } + + memcpy(broadcast_msg, msg + max_length - 1, msg_len - (max_length - 1)); + coms_status = BLUEGIGA_SENDING_BROADCAST; + } // trigger bluegiga to read direct command gpio_clear(BLUEGIGA_DRDY_GPIO, BLUEGIGA_DRDY_GPIO_PIN); // set interrupt diff --git a/sw/airborne/subsystems/datalink/bluegiga.h b/sw/airborne/subsystems/datalink/bluegiga.h index 672ee8d41a..52ac935593 100644 --- a/sw/airborne/subsystems/datalink/bluegiga.h +++ b/sw/airborne/subsystems/datalink/bluegiga.h @@ -35,7 +35,7 @@ enum BlueGigaStatus { BLUEGIGA_UNINIT, /**< The com isn't initialized */ BLUEGIGA_IDLE, /**< The com is in idle */ BLUEGIGA_SENDING, /**< The com is sending */ - BLUEGIGA_BROADCASTING /**< The com is switched from data link to rssi scanning */ + BLUEGIGA_SENDING_BROADCAST /**< The com is switched from data link to rssi scanning */ }; #ifndef BLUEGIGA_BUFFER_SIZE @@ -64,6 +64,7 @@ struct bluegiga_periph { /* some administrative variables */ uint32_t bytes_recvd_since_last; uint8_t end_of_msg; + uint8_t connected; }; @@ -76,38 +77,6 @@ void bluegiga_increment_buf(uint8_t *buf_idx, uint8_t len); void bluegiga_init(struct bluegiga_periph *p); void bluegiga_scan(struct bluegiga_periph *p); -void bluegiga_request_all_rssi(struct bluegiga_periph *p); - -// BLUEGIGA is using pprz_transport -// FIXME it should not appear here, this will be fixed with the rx improvements some day... -// BLUEGIGA needs a specific read_buffer function -#include "pprzlink/pprz_transport.h" -#include "led.h" -static inline void bluegiga_read_buffer(struct bluegiga_periph *p, struct pprz_transport *t) -{ - do { - uint8_t c = 0; - do { - parse_pprz(t, p->rx_buf[(p->rx_extract_idx + c++) % BLUEGIGA_BUFFER_SIZE]); - } while (((p->rx_extract_idx + c) % BLUEGIGA_BUFFER_SIZE != p->rx_insert_idx) - && !(t->trans_rx.msg_received)); - // reached end of circular read buffer or message received - // if received, decode and advance - if (t->trans_rx.msg_received) { -#ifdef MODEM_LED - LED_TOGGLE(MODEM_LED); -#endif - DatalinkFillDlBuffer(t->trans_rx.payload, t->trans_rx.payload_len); - t->trans_rx.msg_received = FALSE; - } - bluegiga_increment_buf(&p->rx_extract_idx, c); - } while (bluegiga_ch_available(p)); // continue till all messages read -} - -// transmit previous data in buffer and parse data received -#define BlueGigaCheckAndParse(_dev,_trans) { \ - if (bluegiga_ch_available(&(_dev))) \ - bluegiga_read_buffer(&(_dev), &(_trans)); \ - } +void bluegiga_broadcast_msg(struct bluegiga_periph *p, char *msg, uint8_t msg_len); #endif /* BLUEGIGA_DATA_LINK_H */ diff --git a/sw/airborne/subsystems/datalink/datalink.h b/sw/airborne/subsystems/datalink/datalink.h index 7f17c5143d..2d4aa1ec6e 100644 --- a/sw/airborne/subsystems/datalink/datalink.h +++ b/sw/airborne/subsystems/datalink/datalink.h @@ -126,7 +126,7 @@ static inline void DlCheckAndParse(void) #elif defined DATALINK && DATALINK == BLUEGIGA #define DatalinkEvent() { \ - BlueGigaCheckAndParse(DOWNLINK_DEVICE, pprz_tp); \ + pprz_check_and_parse(&(DOWNLINK_DEVICE).device, &pprz_tp, dl_buffer, (uint8_t*)(&dl_msg_available)); \ DlCheckAndParse(); \ } diff --git a/sw/airborne/subsystems/gps.c b/sw/airborne/subsystems/gps.c index a15ae97b2a..15a94a50e5 100644 --- a/sw/airborne/subsystems/gps.c +++ b/sw/airborne/subsystems/gps.c @@ -25,7 +25,6 @@ */ #include "subsystems/gps.h" - #include "led.h" #ifdef GPS_POWER_GPIO @@ -90,10 +89,11 @@ static void send_gps(struct transport_tx *trans, struct link_device *dev) uint8_t zero = 0; int16_t climb = -gps.ned_vel.z; int16_t course = (DegOfRad(gps.course) / ((int32_t)1e6)); + struct UtmCoor_i utm = utm_int_from_gps(&gps, 0); pprz_msg_send_GPS(trans, dev, AC_ID, &gps.fix, - &gps.utm_pos.east, &gps.utm_pos.north, + &utm.east, &utm.north, &course, &gps.hmsl, &gps.gspeed, &climb, - &gps.week, &gps.tow, &gps.utm_pos.zone, &zero); + &gps.week, &gps.tow, &utm.zone, &zero); // send SVINFO for available satellites that have new data send_svinfo_available(trans, dev); } @@ -199,3 +199,65 @@ uint32_t gps_tow_from_sys_ticks(uint32_t sys_ticks) void WEAK gps_inject_data(uint8_t packet_id __attribute__((unused)), uint8_t length __attribute__((unused)), uint8_t *data __attribute__((unused))){ } + +/** + * Convenience function to get utm position from GPS state + */ +struct UtmCoor_f utm_float_from_gps(struct GpsState *gps_s, uint8_t zone) +{ + struct UtmCoor_f utm; + utm.alt = 0.f; + + if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_UTM_BIT)) { + // A real UTM position is available, use the correct zone + utm.zone = gps_s->utm_pos.zone; + utm.east = gps_s->utm_pos.east / 100.0f; + utm.north = gps_s->utm_pos.north / 100.0f; + } + else { + struct LlaCoor_f lla; + LLA_FLOAT_OF_BFP(lla, gps_s->lla_pos); + // Check if zone should be computed + if (zone > 0) { + utm.zone = zone; + } else { + utm.zone = (gps_s->lla_pos.lon / 1e7 + 180) / 6 + 1; + } + /* Recompute UTM coordinates in this zone */ + utm_of_lla_f(&utm, &lla); + } + + return utm; +} + +struct UtmCoor_i utm_int_from_gps(struct GpsState *gps_s, uint8_t zone) +{ + struct UtmCoor_i utm; + utm.alt = 0; + + if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_UTM_BIT)) { + // A real UTM position is available, use the correct zone + utm.zone = gps_s->utm_pos.zone; + utm.east = gps_s->utm_pos.east; + utm.north = gps_s->utm_pos.north; + } + else { + struct LlaCoor_f lla; + LLA_FLOAT_OF_BFP(lla, gps_s->lla_pos); + // Check if zone should be computed + if (zone > 0) { + utm.zone = zone; + } else { + utm.zone = (gps_s->lla_pos.lon / 1e7 + 180) / 6 + 1; + } + /* Recompute UTM coordinates in this zone */ + struct UtmCoor_f utm_f; + utm_f.zone = utm.zone; + utm_of_lla_f(&utm_f, &lla); + /* convert to fixed point in cm */ + utm.east = utm_f.east * 100; + utm.north = utm_f.north * 100; + } + + return utm; +} diff --git a/sw/airborne/subsystems/gps.h b/sw/airborne/subsystems/gps.h index 7e2679d88e..cc7957d927 100644 --- a/sw/airborne/subsystems/gps.h +++ b/sw/airborne/subsystems/gps.h @@ -30,6 +30,7 @@ #include "std.h" #include "math/pprz_geodetic_int.h" +#include "math/pprz_geodetic_float.h" #include "mcu_periph/sys_time.h" @@ -162,4 +163,22 @@ extern struct GpsTimeSync gps_time_sync; */ extern uint32_t gps_tow_from_sys_ticks(uint32_t sys_ticks); +/** + * Convenience function to get utm position in float from GPS structure. + * Beware that altitude is initialized to zero but not set to the correct value + * @param[in] gps pointer to the gps structure + * @param[in] zone set the utm zone in which the position should be computed, 0 to try to get it automatically from lla position + * @return utm position in float + */ +extern struct UtmCoor_f utm_float_from_gps(struct GpsState *gps, uint8_t zone); + +/** + * Convenience function to get utm position in int from GPS structure. + * Beware that altitude is initialized to zero but not set to the correct value + * @param[in] gps pointer to the gps structure + * @param[in] zone set the utm zone in which the position should be computed, 0 to try to get it automatically from lla position + * @return utm position in fixed point (cm) + */ +extern struct UtmCoor_i utm_int_from_gps(struct GpsState *gps_s, uint8_t zone); + #endif /* GPS_H */ diff --git a/sw/airborne/subsystems/gps/gps_datalink.c b/sw/airborne/subsystems/gps/gps_datalink.c index 1b6f4c3545..788a3569c8 100644 --- a/sw/airborne/subsystems/gps/gps_datalink.c +++ b/sw/airborne/subsystems/gps/gps_datalink.c @@ -27,35 +27,16 @@ * GPS structure to the values received. */ +#include "generated/flight_plan.h" // reference lla NAV_XXX0 + #include "subsystems/gps.h" #include "subsystems/abi.h" -// #include - -#if GPS_USE_DATALINK_SMALL -#ifndef GPS_LOCAL_ECEF_ORIGIN_X -#error Local x coordinate in ECEF of the remote GPS required -#endif - -#ifndef GPS_LOCAL_ECEF_ORIGIN_Y -#error Local y coordinate in ECEF of the remote GPS required -#endif - -#ifndef GPS_LOCAL_ECEF_ORIGIN_Z -#error Local z coordinate in ECEF of the remote GPS required -#endif - -struct EcefCoor_i tracking_ecef; - -struct LtpDef_i tracking_ltp; +// #include +struct LtpDef_i ltp_def; struct EnuCoor_i enu_pos, enu_speed; -struct EcefCoor_i ecef_pos, ecef_vel; - -struct LlaCoor_i lla_pos; -#endif - bool_t gps_available; ///< Is set to TRUE when a new REMOTE_GPS packet is received and parsed /** GPS initialization */ @@ -66,70 +47,67 @@ void gps_impl_init(void) gps.gspeed = 700; // To enable course setting gps.cacc = 0; // To enable course setting -#if GPS_USE_DATALINK_SMALL - tracking_ecef.x = GPS_LOCAL_ECEF_ORIGIN_X; - tracking_ecef.y = GPS_LOCAL_ECEF_ORIGIN_Y; - tracking_ecef.z = GPS_LOCAL_ECEF_ORIGIN_Z; + struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */ + llh_nav0.lat = NAV_LAT0; + llh_nav0.lon = NAV_LON0; + /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */ + llh_nav0.alt = NAV_ALT0 + NAV_MSL0; - ltp_def_from_ecef_i(&tracking_ltp, &tracking_ecef); -#endif + struct EcefCoor_i ecef_nav0; + ecef_of_lla_i(&ecef_nav0, &llh_nav0); + ltp_def_from_ecef_i(<p_def, &ecef_nav0); } -#ifdef GPS_USE_DATALINK_SMALL // Parse the REMOTE_GPS_SMALL datalink packet -void parse_gps_datalink_small(uint8_t num_sv, uint32_t pos_xyz, uint32_t speed_xy) +void parse_gps_datalink_small(uint8_t num_sv, uint32_t pos_xyz, uint32_t speed_xyz, int16_t heading) { - // Position in ENU coordinates - enu_pos.x = (int32_t)((pos_xyz >> 22) & 0x3FF); // bits 31-22 x position in cm - if (enu_pos.x & 0x200) { - enu_pos.x |= 0xFFFFFC00; // fix for twos complements + enu_pos.x = (int32_t)((pos_xyz >> 21) & 0x7FF); // bits 31-21 x position in cm + if (enu_pos.x & 0x400) { + enu_pos.x |= 0xFFFFF800; // sign extend for twos complements } - enu_pos.y = (int32_t)((pos_xyz >> 12) & 0x3FF); // bits 21-12 y position in cm - if (enu_pos.y & 0x200) { - enu_pos.y |= 0xFFFFFC00; // fix for twos complements + enu_pos.y = (int32_t)((pos_xyz >> 10) & 0x7FF); // bits 20-10 y position in cm + if (enu_pos.y & 0x400) { + enu_pos.y |= 0xFFFFF800; // sign extend for twos complements } - enu_pos.z = (int32_t)(pos_xyz >> 2 & 0x3FF); // bits 11-2 z position in cm - // bits 1 and 0 are free - - // printf("ENU Pos: %u (%d, %d, %d)\n", pos_xyz, enu_pos.x, enu_pos.y, enu_pos.z); + enu_pos.z = (int32_t)(pos_xyz & 0x3FF); // bits 9-0 z position in cm // Convert the ENU coordinates to ECEF - ecef_of_enu_point_i(&ecef_pos, &tracking_ltp, &enu_pos); - gps.ecef_pos = ecef_pos; + ecef_of_enu_point_i(&gps.ecef_pos, <p_def, &enu_pos); + SetBit(gps.valid_fields, GPS_VALID_POS_ECEF_BIT); - lla_of_ecef_i(&lla_pos, &ecef_pos); - gps.lla_pos = lla_pos; + lla_of_ecef_i(&gps.lla_pos, &gps.ecef_pos); SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT); - enu_speed.x = (int32_t)((speed_xy >> 22) & 0x3FF); // bits 31-22 speed x in cm/s - if (enu_speed.x & 0x200) { - enu_speed.x |= 0xFFFFFC00; // fix for twos complements + enu_speed.x = (int32_t)((speed_xyz >> 21) & 0x7FF); // bits 31-21 speed x in cm/s + if (enu_speed.x & 0x400) { + enu_speed.x |= 0xFFFFF800; // sign extend for twos complements } - enu_speed.y = (int32_t)((speed_xy >> 12) & 0x3FF); // bits 21-12 speed y in cm/s - if (enu_speed.y & 0x200) { - enu_speed.y |= 0xFFFFFC00; // fix for twos complements + enu_speed.y = (int32_t)((speed_xyz >> 10) & 0x7FF); // bits 20-10 speed y in cm/s + if (enu_speed.y & 0x400) { + enu_speed.y |= 0xFFFFF800; // sign extend for twos complements + } + enu_speed.z = (int32_t)((speed_xyz) & 0x3FF); // bits 9-0 speed z in cm/s + if (enu_speed.z & 0x200) { + enu_speed.z |= 0xFFFFFC00; // sign extend for twos complements } - enu_speed.z = 0; - // printf("ENU Speed: %u (%d, %d, %d)\n", speed_xy, enu_speed.x, enu_speed.y, enu_speed.z); - - ecef_of_enu_vect_i(&gps.ecef_vel , &tracking_ltp , &enu_speed); + ecef_of_enu_vect_i(&gps.ecef_vel , <p_def , &enu_speed); SetBit(gps.valid_fields, GPS_VALID_VEL_ECEF_BIT); - gps.hmsl = tracking_ltp.hmsl + enu_pos.z * 10; // TODO: try to compensate for the loss in accuracy + gps.ned_vel.x = enu_speed.y; + gps.ned_vel.y = enu_speed.x; + gps.ned_vel.z = -enu_speed.z; + SetBit(gps.valid_fields, GPS_VALID_VEL_NED_BIT); + + gps.hmsl = ltp_def.hmsl + enu_pos.z * 10; SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT); - gps.course = (int32_t)((speed_xy >> 2) & 0x3FF); // bits 11-2 heading in rad*1e2 - if (gps.course & 0x200) { - gps.course |= 0xFFFFFC00; // fix for twos complements - } - // printf("Heading: %d\n", gps.course); - gps.course *= 1e5; + gps.course = ((int32_t)heading) * 1e3; SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT); gps.num_sv = num_sv; - gps.tow = 0; // set time-of-weak to 0 + gps.tow = gps_tow_from_sys_ticks(sys_time.nb_tick); gps.fix = GPS_FIX_3D; // set 3D fix to true gps_available = TRUE; // set GPS available to true @@ -143,7 +121,6 @@ void parse_gps_datalink_small(uint8_t num_sv, uint32_t pos_xyz, uint32_t speed_x } AbiSendMsgGPS(GPS_DATALINK_ID, now_ts, &gps); } -#endif /** Parse the REMOTE_GPS datalink packet */ void parse_gps_datalink(uint8_t numsv, int32_t ecef_x, int32_t ecef_y, int32_t ecef_z, int32_t lat, int32_t lon, @@ -168,10 +145,20 @@ void parse_gps_datalink(uint8_t numsv, int32_t ecef_x, int32_t ecef_y, int32_t e gps.ecef_vel.z = ecef_zd; SetBit(gps.valid_fields, GPS_VALID_VEL_ECEF_BIT); + gps.ned_vel.x = enu_speed.y; + gps.ned_vel.y = enu_speed.x; + gps.ned_vel.z = -enu_speed.z; + SetBit(gps.valid_fields, GPS_VALID_VEL_NED_BIT); + gps.course = course; SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT); + gps.num_sv = numsv; - gps.tow = tow; + if (tow == 0) { + gps.tow = gps_tow_from_sys_ticks(sys_time.nb_tick); //tow; + } else { + gps.tow = gps_tow_from_sys_ticks(sys_time.nb_tick); //tow; + } gps.fix = GPS_FIX_3D; gps_available = TRUE; diff --git a/sw/airborne/subsystems/gps/gps_datalink.h b/sw/airborne/subsystems/gps/gps_datalink.h index 504ba49f9c..3d82d80277 100644 --- a/sw/airborne/subsystems/gps/gps_datalink.h +++ b/sw/airborne/subsystems/gps/gps_datalink.h @@ -35,9 +35,9 @@ #define GPS_NB_CHANNELS 0 extern bool_t gps_available; -#ifdef GPS_USE_DATALINK_SMALL -void parse_gps_datalink_small(uint8_t num_sv, uint32_t pos_xyz, uint32_t speed_xy); -#endif + +extern void parse_gps_datalink_small(uint8_t num_sv, uint32_t pos_xyz, uint32_t speed_xyz, int16_t heading); + extern void parse_gps_datalink(uint8_t numsv, int32_t ecef_x, int32_t ecef_y, int32_t ecef_z, int32_t lat, int32_t lon, int32_t alt, int32_t hmsl, int32_t ecef_xd, int32_t ecef_yd, int32_t ecef_zd, diff --git a/sw/airborne/subsystems/ins.c b/sw/airborne/subsystems/ins.c index 556c5f4e6a..178e76c0cd 100644 --- a/sw/airborne/subsystems/ins.c +++ b/sw/airborne/subsystems/ins.c @@ -72,20 +72,7 @@ void ins_init(void) void WEAK ins_reset_local_origin(void) { #if USE_GPS - struct UtmCoor_f utm; - - if (bit_is_set(gps.valid_fields, GPS_VALID_POS_UTM_BIT)) { - utm.zone = gps.utm_pos.zone; - utm.east = gps.utm_pos.east / 100.0f; - utm.north = gps.utm_pos.north / 100.0f; - } - else { - /* Recompute UTM coordinates in this zone */ - struct LlaCoor_f lla; - LLA_FLOAT_OF_BFP(lla, gps.lla_pos); - utm.zone = (gps.lla_pos.lon / 1e7 + 180) / 6 + 1; - utm_of_lla_f(&utm, &lla); - } + struct UtmCoor_f utm = utm_float_from_gps(&gps, 0); // ground_alt utm.alt = gps.hmsl / 1000.0f; diff --git a/sw/airborne/subsystems/ins/ins_alt_float.c b/sw/airborne/subsystems/ins/ins_alt_float.c index 9c0e71c4ef..77143cd4ea 100644 --- a/sw/airborne/subsystems/ins/ins_alt_float.c +++ b/sw/airborne/subsystems/ins/ins_alt_float.c @@ -49,6 +49,10 @@ #warning Please remove the obsolete ALT_KALMAN and ALT_KALMAN_ENABLED defines from your airframe file. #endif +#ifndef USE_INS_NAV_INIT +#define USE_INS_NAV_INIT TRUE +PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE") +#endif struct InsAltFloat ins_altf; @@ -87,11 +91,15 @@ void ins_alt_float_update_gps(struct GpsState *gps_s); void ins_alt_float_init(void) { - +#if USE_INS_NAV_INIT struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, ground_alt, nav_utm_zone0 }; stateSetLocalUtmOrigin_f(&utm0); + ins_altf.origin_initialized = TRUE; stateSetPositionUtm_f(&utm0); +#else + ins_altf.origin_initialized = FALSE; +#endif // set initial body to imu to 0 struct Int32Eulers b2i0 = { 0, 0, 0 }; @@ -113,20 +121,8 @@ void ins_alt_float_init(void) /** Reset the geographic reference to the current GPS fix */ void ins_reset_local_origin(void) { - struct UtmCoor_f utm; - - if (bit_is_set(gps.valid_fields, GPS_VALID_POS_UTM_BIT)) { - utm.zone = gps.utm_pos.zone; - utm.east = gps.utm_pos.east / 100.0f; - utm.north = gps.utm_pos.north / 100.0f; - } - else { - /* Recompute UTM coordinates in this zone */ - struct LlaCoor_f lla; - LLA_FLOAT_OF_BFP(lla, gps.lla_pos); - utm.zone = (gps.lla_pos.lon / 1e7 + 180) / 6 + 1; - utm_of_lla_f(&utm, &lla); - } + // get utm pos + struct UtmCoor_f utm = utm_float_from_gps(&gps, 0); // ground_alt utm.alt = gps.hmsl / 1000.0f; @@ -134,6 +130,8 @@ void ins_reset_local_origin(void) // reset state UTM ref stateSetLocalUtmOrigin_f(&utm); + ins_altf.origin_initialized = TRUE; + // reset filter flag ins_altf.reset_alt_ref = TRUE; } @@ -198,10 +196,15 @@ void ins_alt_float_update_baro(float pressure __attribute__((unused))) void ins_alt_float_update_gps(struct GpsState *gps_s) { #if USE_GPS - struct UtmCoor_f utm; - utm.east = gps_s->utm_pos.east / 100.0f; - utm.north = gps_s->utm_pos.north / 100.0f; - utm.zone = nav_utm_zone0; + if (gps_s->fix < GPS_FIX_3D) { + return; + } + + if (!ins_altf.origin_initialized) { + ins_reset_local_origin(); + } + + struct UtmCoor_f utm = utm_float_from_gps(gps_s, nav_utm_zone0); #if !USE_BAROMETER #ifdef GPS_DT diff --git a/sw/airborne/subsystems/ins/ins_alt_float.h b/sw/airborne/subsystems/ins/ins_alt_float.h index 7ced685cb2..5f0c147882 100644 --- a/sw/airborne/subsystems/ins/ins_alt_float.h +++ b/sw/airborne/subsystems/ins/ins_alt_float.h @@ -39,6 +39,7 @@ struct InsAltFloat { float alt_dot; ///< estimated vertical speed in m/s (positive-up) bool_t reset_alt_ref; ///< flag to request reset of altitude reference to current alt + bool_t origin_initialized; ///< TRUE if UTM origin was initialized #if USE_BAROMETER float qfe; diff --git a/sw/airborne/subsystems/ins/ins_float_invariant.c b/sw/airborne/subsystems/ins/ins_float_invariant.c index b81cffcab6..124c191d74 100644 --- a/sw/airborne/subsystems/ins/ins_float_invariant.c +++ b/sw/airborne/subsystems/ins/ins_float_invariant.c @@ -273,20 +273,7 @@ void ins_float_invariant_init(void) void ins_reset_local_origin(void) { #if INS_FINV_USE_UTM - struct UtmCoor_f utm; - if (bit_is_set(gps.valid_fields, GPS_VALID_POS_UTM_BIT)) { - utm.zone = gps.utm_pos.zone; - utm.east = gps.utm_pos.east / 100.0f; - utm.north = gps.utm_pos.north / 100.0f; - } - else { - /* Recompute UTM coordinates in this zone */ - struct LlaCoor_f lla; - LLA_FLOAT_OF_BFP(lla, gps.lla_pos); - struct UtmCoor_f utm; - utm.zone = (gps.lla_pos.lon / 1e7 + 180) / 6 + 1; - utm_of_lla_f(&utm, &lla); - } + struct UtmCoor_f utm = utm_float_from_gps(&gps, 0); // ground_alt utm.alt = gps.hmsl / 1000.0f; // reset state UTM ref @@ -441,9 +428,10 @@ void ins_float_invariant_update_gps(struct GpsState *gps_s) #if INS_FINV_USE_UTM if (state.utm_initialized_f) { + struct UtmCoor_f utm = utm_float_from_gps(gps_s, nav_utm_zone0); // position (local ned) - ins_float_inv.meas.pos_gps.x = (gps_s->utm_pos.north / 100.0f) - state.utm_origin_f.north; - ins_float_inv.meas.pos_gps.y = (gps_s->utm_pos.east / 100.0f) - state.utm_origin_f.east; + ins_float_inv.meas.pos_gps.x = utm.north - state.utm_origin_f.north; + ins_float_inv.meas.pos_gps.y = utm.east - state.utm_origin_f.east; ins_float_inv.meas.pos_gps.z = state.utm_origin_f.alt - (gps_s->hmsl / 1000.0f); // speed ins_float_inv.meas.speed_gps.x = gps_s->ned_vel.x / 100.0f; diff --git a/sw/airborne/subsystems/ins/ins_gps_passthrough_utm.c b/sw/airborne/subsystems/ins/ins_gps_passthrough_utm.c index e400029898..6aa8127229 100644 --- a/sw/airborne/subsystems/ins/ins_gps_passthrough_utm.c +++ b/sw/airborne/subsystems/ins/ins_gps_passthrough_utm.c @@ -46,20 +46,7 @@ void ins_gps_utm_init(void) void ins_reset_local_origin(void) { - struct UtmCoor_f utm; - - if (bit_is_set(gps.valid_fields, GPS_VALID_POS_UTM_BIT)) { - utm.zone = gps.utm_pos.zone; - utm.east = gps.utm_pos.east / 100.0f; - utm.north = gps.utm_pos.north / 100.0f; - } - else { - /* Recompute UTM coordinates in this zone */ - struct LlaCoor_f lla; - LLA_FLOAT_OF_BFP(lla, gps.lla_pos); - utm.zone = (gps.lla_pos.lon / 1e7 + 180) / 6 + 1; - utm_of_lla_f(&utm, &lla); - } + struct UtmCoor_f utm = utm_float_from_gps(&gps, 0); // ground_alt utm.alt = gps.hmsl / 1000.0f; @@ -82,10 +69,7 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)), uint32_t stamp __attribute__((unused)), struct GpsState *gps_s) { - struct UtmCoor_f utm; - utm.east = gps_s->utm_pos.east / 100.0f; - utm.north = gps_s->utm_pos.north / 100.0f; - utm.zone = nav_utm_zone0; + struct UtmCoor_f utm = utm_float_from_gps(gps_s, nav_utm_zone0); utm.alt = gps_s->hmsl / 1000.0f; // set position diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c index 10f67dfc8e..3fbad1560f 100644 --- a/sw/airborne/subsystems/ins/ins_int.c +++ b/sw/airborne/subsystems/ins/ins_int.c @@ -94,10 +94,16 @@ PRINT_CONFIG_MSG("INS_SONAR_UPDATE_ON_AGL defaulting to FALSE") #endif // USE_SONAR +#if USE_GPS #ifndef INS_VFF_R_GPS #define INS_VFF_R_GPS 2.0 #endif +#ifndef INS_VFF_VZ_R_GPS +#define INS_VFF_VZ_R_GPS 2.0 +#endif +#endif // USE_GPS + /** maximum number of propagation steps without any updates in between */ #ifndef INS_MAX_PROPAGATION_STEPS #define INS_MAX_PROPAGATION_STEPS 200 @@ -130,6 +136,9 @@ static void baro_cb(uint8_t sender_id, float pressure); #ifndef INS_INT_IMU_ID #define INS_INT_IMU_ID ABI_BROADCAST #endif +#ifndef INS_INT_GPS_ID +#define INS_INT_GPS_ID ABI_BROADCAST +#endif static abi_event accel_ev; static abi_event gps_ev; @@ -372,6 +381,9 @@ void ins_int_update_gps(struct GpsState *gps_s) #if INS_USE_GPS_ALT vff_update_z_conf(((float)gps_pos_cm_ned.z) / 100.0, INS_VFF_R_GPS); #endif +#if INS_USE_GPS_ALT_SPEED + vff_update_vz_conf(((float)gps_speed_cm_s_ned.z) / 100.0, INS_VFF_VZ_R_GPS); +#endif #if USE_HFF /* horizontal gps transformed to NED in meters as float */ @@ -516,12 +528,14 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)), } static void vel_est_cb(uint8_t sender_id __attribute__((unused)), - uint32_t stamp __attribute__((unused)), + uint32_t stamp, float x, float y, float z, float noise __attribute__((unused))) { struct FloatVect3 vel_body = {x, y, z}; + static uint32_t last_stamp = 0; + float dt = 0; /* rotate velocity estimate to nav/ltp frame */ struct FloatQuat q_b2n = *stateGetNedToBodyQuat_f(); @@ -529,7 +543,15 @@ static void vel_est_cb(uint8_t sender_id __attribute__((unused)), struct FloatVect3 vel_ned; float_quat_vmult(&vel_ned, &q_b2n, &vel_body); + if (last_stamp > 0) { + dt = (float)(stamp - last_stamp) * 1e-6; + } + + last_stamp = stamp; + #if USE_HFF + (void)dt; //dt is unused variable in this define + struct FloatVect2 vel = {vel_ned.x, vel_ned.y}; struct FloatVect2 Rvel = {noise, noise}; @@ -538,6 +560,10 @@ static void vel_est_cb(uint8_t sender_id __attribute__((unused)), #else ins_int.ltp_speed.x = SPEED_BFP_OF_REAL(vel_ned.x); ins_int.ltp_speed.y = SPEED_BFP_OF_REAL(vel_ned.y); + if (last_stamp > 0) { + ins_int.ltp_pos.x = ins_int.ltp_pos.x + POS_BFP_OF_REAL(dt * vel_ned.x); + ins_int.ltp_pos.y = ins_int.ltp_pos.y + POS_BFP_OF_REAL(dt * vel_ned.y); + } #endif ins_ned_to_state(); @@ -554,6 +580,6 @@ void ins_int_register(void) * Subscribe to scaled IMU measurements and attach callbacks */ AbiBindMsgIMU_ACCEL_INT32(INS_INT_IMU_ID, &accel_ev, accel_cb); - AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb); + AbiBindMsgGPS(INS_INT_GPS_ID, &gps_ev, gps_cb); AbiBindMsgVELOCITY_ESTIMATE(INS_INT_VEL_ID, &vel_est_ev, vel_est_cb); } diff --git a/sw/ground_segment/misc/natnet2ivy.c b/sw/ground_segment/misc/natnet2ivy.c index 9ad0a21318..f2b26e498f 100644 --- a/sw/ground_segment/misc/natnet2ivy.c +++ b/sw/ground_segment/misc/natnet2ivy.c @@ -19,14 +19,14 @@ * Boston, MA 02111-1307, USA. */ - /** \file natnet2ivy.c - * \brief NatNet (GPS) to ivy forwarder - * - * This receives aircraft position information through the Optitrack system - * NatNet UDP stream and forwards it to the ivy bus. An aircraft with the gps - * subsystem "datalink" is then able to parse the GPS position and use it to - * navigate inside the Optitrack system. - */ +/** \file natnet2ivy.c +* \brief NatNet (GPS) to ivy forwarder +* +* This receives aircraft position information through the Optitrack system +* NatNet UDP stream and forwards it to the ivy bus. An aircraft with the gps +* subsystem "datalink" is then able to parse the GPS position and use it to +* navigate inside the Optitrack system. +*/ #include #include @@ -126,8 +126,9 @@ double tracking_offset_angle; ///< The offset from the tracking system to float natnet_latency; /** Parse the packet from NatNet */ -void natnet_parse(unsigned char *in) { - int i,j,k; +void natnet_parse(unsigned char *in) +{ + int i, j, k; // Create a pointer to go trough the packet char *ptr = (char *)in; @@ -143,8 +144,7 @@ void natnet_parse(unsigned char *in) { memcpy(&nBytes, ptr, 2); ptr += 2; printf_natnet("Byte count : %d\n", nBytes); - if(MessageID == NAT_FRAMEOFDATA) // FRAME OF MOCAP DATA packet - { + if (MessageID == NAT_FRAMEOFDATA) { // FRAME OF MOCAP DATA packet // Frame number int frameNumber = 0; memcpy(&frameNumber, ptr, 4); ptr += 4; printf_natnet("Frame # : %d\n", frameNumber); @@ -154,8 +154,7 @@ void natnet_parse(unsigned char *in) { int nMarkerSets = 0; memcpy(&nMarkerSets, ptr, 4); ptr += 4; printf_natnet("Marker Set Count : %d\n", nMarkerSets); - for (i=0; i < nMarkerSets; i++) - { + for (i = 0; i < nMarkerSets; i++) { // Markerset name char szName[256]; strcpy(szName, ptr); @@ -167,24 +166,22 @@ void natnet_parse(unsigned char *in) { int nMarkers = 0; memcpy(&nMarkers, ptr, 4); ptr += 4; printf_natnet("Marker Count : %d\n", nMarkers); - for(j=0; j < nMarkers; j++) - { + for (j = 0; j < nMarkers; j++) { float x = 0; memcpy(&x, ptr, 4); ptr += 4; float y = 0; memcpy(&y, ptr, 4); ptr += 4; float z = 0; memcpy(&z, ptr, 4); ptr += 4; - printf_natnet("\tMarker %d : [x=%3.2f,y=%3.2f,z=%3.2f]\n",j,x,y,z); + printf_natnet("\tMarker %d : [x=%3.2f,y=%3.2f,z=%3.2f]\n", j, x, y, z); } } // Unidentified markers int nOtherMarkers = 0; memcpy(&nOtherMarkers, ptr, 4); ptr += 4; printf_natnet("Unidentified Marker Count : %d\n", nOtherMarkers); - for(j=0; j < nOtherMarkers; j++) - { + for (j = 0; j < nOtherMarkers; j++) { float x = 0.0f; memcpy(&x, ptr, 4); ptr += 4; float y = 0.0f; memcpy(&y, ptr, 4); ptr += 4; float z = 0.0f; memcpy(&z, ptr, 4); ptr += 4; - printf_natnet("\tMarker %d : pos = [%3.2f,%3.2f,%3.2f]\n",j,x,y,z); + printf_natnet("\tMarker %d : pos = [%3.2f,%3.2f,%3.2f]\n", j, x, y, z); } // ========== RIGID BODIES ========== @@ -194,13 +191,14 @@ void natnet_parse(unsigned char *in) { printf_natnet("Rigid Body Count : %d\n", nRigidBodies); // Check if there ie enough space for the rigid bodies - if(nRigidBodies > MAX_RIGIDBODIES) { - fprintf(stderr, "Could not sample all the rigid bodies because the amount of rigid bodies is bigger then %d (MAX_RIGIDBODIES).\r\n", MAX_RIGIDBODIES); + if (nRigidBodies > MAX_RIGIDBODIES) { + fprintf(stderr, + "Could not sample all the rigid bodies because the amount of rigid bodies is bigger then %d (MAX_RIGIDBODIES).\r\n", + MAX_RIGIDBODIES); exit(EXIT_FAILURE); } - for (j=0; j < nRigidBodies; j++) - { + for (j = 0; j < nRigidBodies; j++) { // rigid body pos/ori struct RigidBody old_rigid; memcpy(&old_rigid, &rigidBodies[j], sizeof(struct RigidBody)); @@ -214,15 +212,17 @@ void natnet_parse(unsigned char *in) { memcpy(&rigidBodies[j].qy, ptr, 4); ptr += 4; //qz --> QY memcpy(&rigidBodies[j].qw, ptr, 4); ptr += 4; //qw --> QW printf_natnet("ID (%d) : %d\n", j, rigidBodies[j].id); - printf_natnet("pos: [%3.2f,%3.2f,%3.2f]\n", rigidBodies[j].x,rigidBodies[j].y,rigidBodies[j].z); - printf_natnet("ori: [%3.2f,%3.2f,%3.2f,%3.2f]\n", rigidBodies[j].qx,rigidBodies[j].qy,rigidBodies[j].qz,rigidBodies[j].qw); + printf_natnet("pos: [%3.2f,%3.2f,%3.2f]\n", rigidBodies[j].x, rigidBodies[j].y, rigidBodies[j].z); + printf_natnet("ori: [%3.2f,%3.2f,%3.2f,%3.2f]\n", rigidBodies[j].qx, rigidBodies[j].qy, rigidBodies[j].qz, + rigidBodies[j].qw); // Differentiate the position to get the speed (TODO: crossreference with labeled markers for occlussion) rigidBodies[j].totalVelocitySamples++; - if(old_rigid.x != rigidBodies[j].x || old_rigid.y != rigidBodies[j].y || old_rigid.z != rigidBodies[j].z - || old_rigid.qx != rigidBodies[j].qx || old_rigid.qy != rigidBodies[j].qy || old_rigid.qz != rigidBodies[j].qz || old_rigid.qw != rigidBodies[j].qw) { + if (old_rigid.x != rigidBodies[j].x || old_rigid.y != rigidBodies[j].y || old_rigid.z != rigidBodies[j].z + || old_rigid.qx != rigidBodies[j].qx || old_rigid.qy != rigidBodies[j].qy || old_rigid.qz != rigidBodies[j].qz + || old_rigid.qw != rigidBodies[j].qw) { - if(old_rigid.posSampled) { + if (old_rigid.posSampled) { rigidBodies[j].vel_x += (rigidBodies[j].x - old_rigid.x); rigidBodies[j].vel_y += (rigidBodies[j].y - old_rigid.y); rigidBodies[j].vel_z += (rigidBodies[j].z - old_rigid.z); @@ -231,12 +231,12 @@ void natnet_parse(unsigned char *in) { rigidBodies[j].nSamples++; rigidBodies[j].posSampled = TRUE; - } - else + } else { rigidBodies[j].posSampled = FALSE; + } // When marker id changed, reset the velocity - if(old_rigid.id != rigidBodies[j].id) { + if (old_rigid.id != rigidBodies[j].id) { rigidBodies[j].vel_x = 0; rigidBodies[j].vel_y = 0; rigidBodies[j].vel_z = 0; @@ -249,71 +249,67 @@ void natnet_parse(unsigned char *in) { // Associated marker positions memcpy(&rigidBodies[j].nMarkers, ptr, 4); ptr += 4; printf_natnet("Marker Count: %d\n", rigidBodies[j].nMarkers); - int nBytes = rigidBodies[j].nMarkers*3*sizeof(float); - float* markerData = (float*)malloc(nBytes); + int nBytes = rigidBodies[j].nMarkers * 3 * sizeof(float); + float *markerData = (float *)malloc(nBytes); memcpy(markerData, ptr, nBytes); ptr += nBytes; - if(natnet_major >= 2) - { + if (natnet_major >= 2) { // Associated marker IDs - nBytes = rigidBodies[j].nMarkers*sizeof(int); - int* markerIDs = (int*)malloc(nBytes); + nBytes = rigidBodies[j].nMarkers * sizeof(int); + int *markerIDs = (int *)malloc(nBytes); memcpy(markerIDs, ptr, nBytes); ptr += nBytes; // Associated marker sizes - nBytes = rigidBodies[j].nMarkers*sizeof(float); - float* markerSizes = (float*)malloc(nBytes); + nBytes = rigidBodies[j].nMarkers * sizeof(float); + float *markerSizes = (float *)malloc(nBytes); memcpy(markerSizes, ptr, nBytes); ptr += nBytes; - for(k=0; k < rigidBodies[j].nMarkers; k++) - { - printf_natnet("\tMarker %d: id=%d\tsize=%3.1f\tpos=[%3.2f,%3.2f,%3.2f]\n", k, markerIDs[k], markerSizes[k], markerData[k*3], markerData[k*3+1],markerData[k*3+2]); + for (k = 0; k < rigidBodies[j].nMarkers; k++) { + printf_natnet("\tMarker %d: id=%d\tsize=%3.1f\tpos=[%3.2f,%3.2f,%3.2f]\n", k, markerIDs[k], markerSizes[k], + markerData[k * 3], markerData[k * 3 + 1], markerData[k * 3 + 2]); } - if(markerIDs) + if (markerIDs) { free(markerIDs); - if(markerSizes) + } + if (markerSizes) { free(markerSizes); + } - } - else - { - for(k=0; k < rigidBodies[j].nMarkers; k++) - { - printf_natnet("\tMarker %d: pos = [%3.2f,%3.2f,%3.2f]\n", k, markerData[k*3], markerData[k*3+1],markerData[k*3+2]); + } else { + for (k = 0; k < rigidBodies[j].nMarkers; k++) { + printf_natnet("\tMarker %d: pos = [%3.2f,%3.2f,%3.2f]\n", k, markerData[k * 3], markerData[k * 3 + 1], + markerData[k * 3 + 2]); } } - if(markerData) + if (markerData) { free(markerData); + } - if(natnet_major >= 2) - { + if (natnet_major >= 2) { // Mean marker error memcpy(&rigidBodies[j].error, ptr, 4); ptr += 4; printf_natnet("Mean marker error: %3.8f\n", rigidBodies[j].error); } // 2.6 and later - if( ((natnet_major == 2)&&(natnet_minor >= 6)) || (natnet_major > 2) || (natnet_major == 0) ) - { - // params - short params = 0; memcpy(¶ms, ptr, 2); ptr += 2; + if (((natnet_major == 2) && (natnet_minor >= 6)) || (natnet_major > 2) || (natnet_major == 0)) { + // params + short params = 0; memcpy(¶ms, ptr, 2); ptr += 2; // bool bTrackingValid = params & 0x01; // 0x01 : rigid body was successfully tracked in this frame } } // next rigid body // ========== SKELETONS ========== // Skeletons (version 2.1 and later) - if(((natnet_major == 2) && (natnet_minor>0)) || (natnet_major>2)) - { + if (((natnet_major == 2) && (natnet_minor > 0)) || (natnet_major > 2)) { int nSkeletons = 0; memcpy(&nSkeletons, ptr, 4); ptr += 4; printf_natnet("Skeleton Count : %d\n", nSkeletons); - for (j=0; j < nSkeletons; j++) - { + for (j = 0; j < nSkeletons; j++) { // Skeleton id int skeletonID = 0; memcpy(&skeletonID, ptr, 4); ptr += 4; @@ -321,8 +317,7 @@ void natnet_parse(unsigned char *in) { int nRigidBodies = 0; memcpy(&nRigidBodies, ptr, 4); ptr += 4; printf_natnet("Rigid Body Count : %d\n", nRigidBodies); - for (j=0; j < nRigidBodies; j++) - { + for (j = 0; j < nRigidBodies; j++) { // Rigid body pos/ori int ID = 0; memcpy(&ID, ptr, 4); ptr += 4; float x = 0.0f; memcpy(&x, ptr, 4); ptr += 4; @@ -333,32 +328,32 @@ void natnet_parse(unsigned char *in) { float qz = 0; memcpy(&qz, ptr, 4); ptr += 4; float qw = 0; memcpy(&qw, ptr, 4); ptr += 4; printf_natnet("ID : %d\n", ID); - printf_natnet("pos: [%3.2f,%3.2f,%3.2f]\n", x,y,z); - printf_natnet("ori: [%3.2f,%3.2f,%3.2f,%3.2f]\n", qx,qy,qz,qw); + printf_natnet("pos: [%3.2f,%3.2f,%3.2f]\n", x, y, z); + printf_natnet("ori: [%3.2f,%3.2f,%3.2f,%3.2f]\n", qx, qy, qz, qw); // Sssociated marker positions int nRigidMarkers = 0; memcpy(&nRigidMarkers, ptr, 4); ptr += 4; printf_natnet("Marker Count: %d\n", nRigidMarkers); - int nBytes = nRigidMarkers*3*sizeof(float); - float* markerData = (float*)malloc(nBytes); + int nBytes = nRigidMarkers * 3 * sizeof(float); + float *markerData = (float *)malloc(nBytes); memcpy(markerData, ptr, nBytes); ptr += nBytes; // Associated marker IDs - nBytes = nRigidMarkers*sizeof(int); - int* markerIDs = (int*)malloc(nBytes); + nBytes = nRigidMarkers * sizeof(int); + int *markerIDs = (int *)malloc(nBytes); memcpy(markerIDs, ptr, nBytes); ptr += nBytes; // Associated marker sizes - nBytes = nRigidMarkers*sizeof(float); - float* markerSizes = (float*)malloc(nBytes); + nBytes = nRigidMarkers * sizeof(float); + float *markerSizes = (float *)malloc(nBytes); memcpy(markerSizes, ptr, nBytes); ptr += nBytes; - for(k=0; k < nRigidMarkers; k++) - { - printf_natnet("\tMarker %d: id=%d\tsize=%3.1f\tpos=[%3.2f,%3.2f,%3.2f]\n", k, markerIDs[k], markerSizes[k], markerData[k*3], markerData[k*3+1],markerData[k*3+2]); + for (k = 0; k < nRigidMarkers; k++) { + printf_natnet("\tMarker %d: id=%d\tsize=%3.1f\tpos=[%3.2f,%3.2f,%3.2f]\n", k, markerIDs[k], markerSizes[k], + markerData[k * 3], markerData[k * 3 + 1], markerData[k * 3 + 2]); } // Mean marker error @@ -366,25 +361,26 @@ void natnet_parse(unsigned char *in) { printf_natnet("Mean marker error: %3.2f\n", fError); // Release resources - if(markerIDs) + if (markerIDs) { free(markerIDs); - if(markerSizes) + } + if (markerSizes) { free(markerSizes); - if(markerData) + } + if (markerData) { free(markerData); + } } // next rigid body } // next skeleton } // ========== LABELED MARKERS ========== // Labeled markers (version 2.3 and later) - if( ((natnet_major == 2)&&(natnet_minor>=3)) || (natnet_major>2)) - { + if (((natnet_major == 2) && (natnet_minor >= 3)) || (natnet_major > 2)) { int nLabeledMarkers = 0; memcpy(&nLabeledMarkers, ptr, 4); ptr += 4; printf_natnet("Labeled Marker Count : %d\n", nLabeledMarkers); - for (j=0; j < nLabeledMarkers; j++) - { + for (j = 0; j < nLabeledMarkers; j++) { int ID = 0; memcpy(&ID, ptr, 4); ptr += 4; float x = 0.0f; memcpy(&x, ptr, 4); ptr += 4; float y = 0.0f; memcpy(&y, ptr, 4); ptr += 4; @@ -392,7 +388,7 @@ void natnet_parse(unsigned char *in) { float size = 0.0f; memcpy(&size, ptr, 4); ptr += 4; printf_natnet("ID : %d\n", ID); - printf_natnet("pos : [%3.2f,%3.2f,%3.2f]\n", x,y,z); + printf_natnet("pos : [%3.2f,%3.2f,%3.2f]\n", x, y, z); printf_natnet("size: [%3.2f]\n", size); } } @@ -409,45 +405,47 @@ void natnet_parse(unsigned char *in) { // End of data tag int eod = 0; memcpy(&eod, ptr, 4); ptr += 4; printf_natnet("End Packet\n-------------\n"); - } - else - { + } else { printf("Error: Unrecognized packet type from Optitrack NatNet.\n"); } } /** The transmitter periodic function */ -gboolean timeout_transmit_callback(gpointer data) { +gboolean timeout_transmit_callback(gpointer data) +{ int i; // Loop trough all the available rigidbodies (TODO: optimize) - for(i = 0; i < MAX_RIGIDBODIES; i++) { + for (i = 0; i < MAX_RIGIDBODIES; i++) { // Check if ID's are correct - if(rigidBodies[i].id >= MAX_RIGIDBODIES) { - fprintf(stderr, "Could not parse rigid body %d from NatNet, because ID is higher then or equal to %d (MAX_RIGIDBODIES-1).\r\n", rigidBodies[i].id, MAX_RIGIDBODIES-1); + if (rigidBodies[i].id >= MAX_RIGIDBODIES) { + fprintf(stderr, + "Could not parse rigid body %d from NatNet, because ID is higher then or equal to %d (MAX_RIGIDBODIES-1).\r\n", + rigidBodies[i].id, MAX_RIGIDBODIES - 1); exit(EXIT_FAILURE); } // Check if we want to transmit (follow) this rigid - if(aircrafts[rigidBodies[i].id].ac_id == 0) + if (aircrafts[rigidBodies[i].id].ac_id == 0) { continue; + } // When we don track anymore and timeout or start tracking - if(rigidBodies[i].nSamples < 1 - && aircrafts[rigidBodies[i].id].connected - && (natnet_latency - aircrafts[rigidBodies[i].id].lastSample) > CONNECTION_TIMEOUT) { + if (rigidBodies[i].nSamples < 1 + && aircrafts[rigidBodies[i].id].connected + && (natnet_latency - aircrafts[rigidBodies[i].id].lastSample) > CONNECTION_TIMEOUT) { aircrafts[rigidBodies[i].id].connected = FALSE; fprintf(stderr, "#error Lost tracking rigid id %d, aircraft id %d.\n", - rigidBodies[i].id, aircrafts[rigidBodies[i].id].ac_id); - } - else if(rigidBodies[i].nSamples > 0 && !aircrafts[rigidBodies[i].id].connected) { + rigidBodies[i].id, aircrafts[rigidBodies[i].id].ac_id); + } else if (rigidBodies[i].nSamples > 0 && !aircrafts[rigidBodies[i].id].connected) { fprintf(stderr, "#pragma message: Now tracking rigid id %d, aircraft id %d.\n", - rigidBodies[i].id, aircrafts[rigidBodies[i].id].ac_id); + rigidBodies[i].id, aircrafts[rigidBodies[i].id].ac_id); } // Check if we still track the rigid - if(rigidBodies[i].nSamples < 1) + if (rigidBodies[i].nSamples < 1) { continue; + } // Update the last tracked aircrafts[rigidBodies[i].id].connected = TRUE; @@ -466,15 +464,15 @@ gboolean timeout_transmit_callback(gpointer data) { pos.z = rigidBodies[i].z; // Convert the position to ecef and lla based on the Optitrack LTP - ecef_of_enu_point_d(&ecef_pos ,&tracking_ltp ,&pos); + ecef_of_enu_point_d(&ecef_pos , &tracking_ltp , &pos); lla_of_ecef_d(&lla_pos, &ecef_pos); // Check if we have enough samples to estimate the velocity rigidBodies[i].nVelocityTransmit++; - if(rigidBodies[i].nVelocitySamples >= min_velocity_samples) { + if (rigidBodies[i].nVelocitySamples >= min_velocity_samples) { // Calculate the derevative of the sum to get the correct velocity (1 / freq_transmit) * (samples / total_samples) double sample_time = //((double)rigidBodies[i].nVelocitySamples / (double)rigidBodies[i].totalVelocitySamples) / - ((double)rigidBodies[i].nVelocityTransmit / (double)freq_transmit); + ((double)rigidBodies[i].nVelocityTransmit / (double)freq_transmit); rigidBodies[i].vel_x = rigidBodies[i].vel_x / sample_time; rigidBodies[i].vel_y = rigidBodies[i].vel_y / sample_time; rigidBodies[i].vel_z = rigidBodies[i].vel_z / sample_time; @@ -485,7 +483,7 @@ gboolean timeout_transmit_callback(gpointer data) { speed.z = rigidBodies[i].vel_z; // Conver the speed to ecef based on the Optitrack LTP - ecef_of_enu_vect_d(&rigidBodies[i].ecef_vel ,&tracking_ltp ,&speed); + ecef_of_enu_vect_d(&rigidBodies[i].ecef_vel , &tracking_ltp , &speed); } // Copy the quaternions and convert to euler angles for the heading @@ -496,62 +494,103 @@ gboolean timeout_transmit_callback(gpointer data) { double_eulers_of_quat(&orient_eulers, &orient); // Calculate the heading by adding the Natnet offset angle and normalizing it - double heading = -orient_eulers.psi+90.0/57.6 - tracking_offset_angle; //the optitrack axes are 90 degrees rotated wrt ENU + double heading = -orient_eulers.psi + 90.0 / 57.6 - + tracking_offset_angle; //the optitrack axes are 90 degrees rotated wrt ENU NormRadAngle(heading); - printf_debug("[%d -> %d]Samples: %d\t%d\t\tTiming: %3.3f latency\n", rigidBodies[i].id, aircrafts[rigidBodies[i].id].ac_id - , rigidBodies[i].nSamples, rigidBodies[i].nVelocitySamples, natnet_latency); + printf_debug("[%d -> %d]Samples: %d\t%d\t\tTiming: %3.3f latency\n", rigidBodies[i].id, + aircrafts[rigidBodies[i].id].ac_id + , rigidBodies[i].nSamples, rigidBodies[i].nVelocitySamples, natnet_latency); printf_debug(" Heading: %f\t\tPosition: %f\t%f\t%f\t\tVelocity: %f\t%f\t%f\n", DegOfRad(heading), - rigidBodies[i].x, rigidBodies[i].y, rigidBodies[i].z, - rigidBodies[i].ecef_vel.x, rigidBodies[i].ecef_vel.y, rigidBodies[i].ecef_vel.z); + rigidBodies[i].x, rigidBodies[i].y, rigidBodies[i].z, + rigidBodies[i].ecef_vel.x, rigidBodies[i].ecef_vel.y, rigidBodies[i].ecef_vel.z); // Transmit the REMOTE_GPS packet on the ivy bus (either small or big) - if(small_packets) { - /* The GPS messages are most likely too large to be send over either the datalink - * The local position is an int32 and the 10 LSBs of the x and y axis are compressed into + if (small_packets) { + /* The local position is an int32 and the 11 LSBs of the (signed) x and y axis are compressed into * a single integer. The z axis is considered unsigned and only the latter 10 LSBs are * used. */ - uint32_t pos_xyz = (((uint32_t)(pos.x*100.0)) & 0x3FF) << 22; // bits 31-22 x position in cm - pos_xyz |= (((uint32_t)(pos.y*100.0)) & 0x3FF) << 12; // bits 21-12 y position in cm - pos_xyz |= (((uint32_t)(pos.z*100.0)) & 0x3FF) << 2; // bits 11-2 z position in cm - // bits 1 and 0 are free + uint32_t pos_xyz; + // check if position within limits + if (fabs(pos.x * 100) < pow(2, 10)) { + pos_xyz = (((uint32_t)(pos.x * 100.0)) & 0x7FF) << 21; // bits 31-21 x position in cm + } else { + fprintf(stderr, "Warning!! X position out of maximum range of small message (±%.2fm): %.2f", pow(2, 10) / 100, pos.x); + pos_xyz = (((uint32_t)(pow(2, 10) * pos.x / fabs(pos.x))) & 0x7FF) << 21; // bits 31-21 x position in cm + } + + if (fabs(pos.y * 100) < pow(2, 10)) { + pos_xyz |= (((uint32_t)(pos.y * 100.0)) & 0x7FF) << 10; // bits 20-10 y position in cm + } else { + fprintf(stderr, "Warning!! Y position out of maximum range of small message (±%.2fm): %.2f", pow(2, 10) / 100, pos.y); + pos_xyz |= (((uint32_t)(pow(2, 10) * pos.y / fabs(pos.y))) & 0x7FF) << 10; // bits 20-10 y position in cm + } + + if (pos.z * 100 < pow(2, 10)) { + pos_xyz |= (((uint32_t)(pos.z * 100.0)) & 0x3FF); // bits 9-0 z position in cm + } else { + fprintf(stderr, "Warning!! Z position out of maximum range of small message (%.2fm): %.2f", pow(2, 10) / 100, pos.z); + pos_xyz |= (((uint32_t)(pow(2, 10))) & 0x3FF); // bits 9-0 z position in cm + } // printf("ENU Pos: %u (%.2f, %.2f, %.2f)\n", pos_xyz, pos.x, pos.y, pos.z); - uint32_t speed_xy = (((uint32_t)(speed.x*100.0)) & 0x3FF) << 22; // bits 31-22 speed x in cm/s - speed_xy |= (((uint32_t)(speed.y*100.0)) & 0x3FF) << 12; // bits 21-12 speed y in cm/s - speed_xy |= (((uint32_t)(heading*100.0)) & 0x3FF) << 2; // bits 11-2 heading in rad*1e2 (The heading is already subsampled) - // bits 1 and 0 are free + /* The speed is an int32 and the 11 LSBs of the x and y axis and 10 LSBs of z (all signed) are compressed into + * a single integer. + */ + uint32_t speed_xyz; + // check if speed within limits + if (fabs(speed.x * 100) < pow(2, 10)) { + speed_xyz = (((uint32_t)(speed.x * 100.0)) & 0x7FF) << 21; // bits 31-21 speed x in cm/s + } else { + fprintf(stderr, "Warning!! X Speed out of maximum range of small message (±%.2fm/s): %.2f", pow(2, 10) / 100, speed.x); + speed_xyz = (((uint32_t)(pow(2, 10) * speed.x / fabs(speed.x))) & 0x7FF) << 21; // bits 31-21 speed x in cm/s + } + + if (fabs(speed.y * 100) < pow(2, 10)) { + speed_xyz |= (((uint32_t)(speed.y * 100.0)) & 0x7FF) << 10; // bits 20-10 speed y in cm/s + } else { + fprintf(stderr, "Warning!! Y Speed out of maximum range of small message (±%.2fm/s): %.2f", pow(2, 10) / 100, speed.y); + speed_xyz |= (((uint32_t)(pow(2, 10) * speed.y / fabs(speed.y))) & 0x7FF) << 10; // bits 20-10 speed y in cm/s + } + + if (fabs(pos.z * 100) < pow(2, 9)) { + speed_xyz |= (((uint32_t)(speed.z * 100.0)) & 0x3FF); // bits 9-0 speed z in cm/s + } else { + fprintf(stderr, "Warning!! Z Speed out of maximum range of small message (±%.2fm/s): %.2f", pow(2, 9) / 100, speed.z); + speed_xyz |= (((uint32_t)(pow(2, 9) * speed.z / fabs(speed.z))) & 0x3FF); // bits 9-0 speed z in cm/s + } // printf("ENU Vel: %u (%.2f, %.2f, 0.0)\n", speed_xy, speed.x, speed.y); // printf("Heading: %.2f\n", heading); - IvySendMsg("0 REMOTE_GPS_SMALL %d %d %d %d", aircrafts[rigidBodies[i].id].ac_id, // uint8 rigid body ID (1 byte) - (uint8_t)rigidBodies[i].nMarkers, // status (1 byte) - pos_xyz, //uint32 ENU X, Y and Z in CM (4 bytes) - speed_xy); //uint32 ENU velocity X, Y in cm/s and heading in rad*1e2 (4 bytes) - } - else { + IvySendMsg("0 REMOTE_GPS_SMALL %d %d %d %d %d", aircrafts[rigidBodies[i].id].ac_id, // uint8 rigid body ID (1 byte) + (uint8_t)rigidBodies[i].nMarkers, // uint8 Number of markers (sv_num) (1 byte) + pos_xyz, // uint32 ENU X, Y and Z in CM (4 bytes) + speed_xyz, // uint32 ENU velocity X, Y, Z in cm/s (4 bytes) + (int16_t)(heading * 10000)); // int6_t heading in rad*1e4 (2 bytes) + + } else { IvySendMsg("0 REMOTE_GPS %d %d %d %d %d %d %d %d %d %d %d %d %d %d", aircrafts[rigidBodies[i].id].ac_id, - rigidBodies[i].nMarkers, //uint8 Number of markers (sv_num) - (int)(ecef_pos.x*100.0), //int32 ECEF X in CM - (int)(ecef_pos.y*100.0), //int32 ECEF Y in CM - (int)(ecef_pos.z*100.0), //int32 ECEF Z in CM - (int)(lla_pos.lat*10000000.0), //int32 LLA latitude in rad*1e7 - (int)(lla_pos.lon*10000000.0), //int32 LLA longitude in rad*1e7 - (int)(rigidBodies[i].z*1000.0), //int32 LLA altitude in mm above elipsoid - (int)(rigidBodies[i].z*1000.0), //int32 HMSL height above mean sea level in mm - (int)(rigidBodies[i].ecef_vel.x*100.0), //int32 ECEF velocity X in m/s - (int)(rigidBodies[i].ecef_vel.y*100.0), //int32 ECEF velocity Y in m/s - (int)(rigidBodies[i].ecef_vel.z*100.0), //int32 ECEF velocity Z in m/s - 0, - (int)(heading*10000000.0)); //int32 Course in rad*1e7 + rigidBodies[i].nMarkers, //uint8 Number of markers (sv_num) + (int)(ecef_pos.x * 100.0), //int32 ECEF X in CM + (int)(ecef_pos.y * 100.0), //int32 ECEF Y in CM + (int)(ecef_pos.z * 100.0), //int32 ECEF Z in CM + (int)(lla_pos.lat * 10000000.0), //int32 LLA latitude in rad*1e7 + (int)(lla_pos.lon * 10000000.0), //int32 LLA longitude in rad*1e7 + (int)(rigidBodies[i].z * 1000.0), //int32 LLA altitude in mm above elipsoid + (int)(rigidBodies[i].z * 1000.0), //int32 HMSL height above mean sea level in mm + (int)(rigidBodies[i].ecef_vel.x * 100.0), //int32 ECEF velocity X in m/s + (int)(rigidBodies[i].ecef_vel.y * 100.0), //int32 ECEF velocity Y in m/s + (int)(rigidBodies[i].ecef_vel.z * 100.0), //int32 ECEF velocity Z in m/s + 0, + (int)(heading * 10000000.0)); //int32 Course in rad*1e7 } // Reset the velocity differentiator if we calculated the velocity - if(rigidBodies[i].nVelocitySamples >= min_velocity_samples) { + if (rigidBodies[i].nVelocitySamples >= min_velocity_samples) { rigidBodies[i].vel_x = 0; rigidBodies[i].vel_y = 0; rigidBodies[i].vel_z = 0; @@ -567,7 +606,8 @@ gboolean timeout_transmit_callback(gpointer data) { } /** The NatNet sampler periodic function */ -static gboolean sample_data(GIOChannel *chan, GIOCondition cond, gpointer data) { +static gboolean sample_data(GIOChannel *chan, GIOCondition cond, gpointer data) +{ static unsigned char buffer_data[MAX_PACKETSIZE]; static int bytes_data = 0; @@ -575,7 +615,7 @@ static gboolean sample_data(GIOChannel *chan, GIOCondition cond, gpointer data) bytes_data += udp_socket_recv(&natnet_data, buffer_data, MAX_PACKETSIZE); // Parse NatNet data - if(bytes_data >= 2 && bytes_data >= buffer_data[1]) { + if (bytes_data >= 2 && bytes_data >= buffer_data[1]) { natnet_parse(buffer_data); bytes_data = 0; } @@ -585,7 +625,8 @@ static gboolean sample_data(GIOChannel *chan, GIOCondition cond, gpointer data) /** Print the program help */ -void print_help(char* filename) { +void print_help(char *filename) +{ static const char *usage = "Usage: %s [options]\n" " Options :\n" @@ -613,8 +654,9 @@ void print_help(char* filename) { } /** Check the amount of arguments */ -void check_argcount(int argc, char** argv, int i, int expected) { - if(i+expected >= argc) { +void check_argcount(int argc, char **argv, int i, int expected) +{ + if (i + expected >= argc) { fprintf(stderr, "Option %s expected %d arguments\r\n\r\n", argv[i], expected); print_help(argv[0]); exit(0); @@ -622,30 +664,31 @@ void check_argcount(int argc, char** argv, int i, int expected) { } /** Parse the options from the commandline */ -static void parse_options(int argc, char** argv) { +static void parse_options(int argc, char **argv) +{ int i, count_ac = 0; - for(i = 1; i < argc; ++i) { + for (i = 1; i < argc; ++i) { // Print help - if(strcmp(argv[i], "--help") == 0 || strcmp(argv[i], "-h") == 0) { + if (strcmp(argv[i], "--help") == 0 || strcmp(argv[i], "-h") == 0) { print_help(argv[0]); exit(0); } // Set the verbosity level - if(strcmp(argv[i], "--verbosity") == 0 || strcmp(argv[i], "-v") == 0) { + if (strcmp(argv[i], "--verbosity") == 0 || strcmp(argv[i], "-v") == 0) { check_argcount(argc, argv, i, 1); verbose = atoi(argv[++i]); } // Set an rigid body to ivy ac_id - else if(strcmp(argv[i], "-ac") == 0) { + else if (strcmp(argv[i], "-ac") == 0) { check_argcount(argc, argv, i, 2); int rigid_id = atoi(argv[++i]); uint8_t ac_id = atoi(argv[++i]); - if(rigid_id >= MAX_RIGIDBODIES) { + if (rigid_id >= MAX_RIGIDBODIES) { fprintf(stderr, "Rigid body ID must be less then %d (MAX_RIGIDBODIES)\n\n", MAX_RIGIDBODIES); print_help(argv[0]); exit(EXIT_FAILURE); @@ -655,19 +698,19 @@ static void parse_options(int argc, char** argv) { } // Set the NatNet multicast address - else if(strcmp(argv[i], "-multicast_addr") == 0) { + else if (strcmp(argv[i], "-multicast_addr") == 0) { check_argcount(argc, argv, i, 1); natnet_multicast_addr = argv[++i]; } // Set the NatNet server ip address - else if(strcmp(argv[i], "-server") == 0) { + else if (strcmp(argv[i], "-server") == 0) { check_argcount(argc, argv, i, 1); natnet_addr = argv[++i]; } // Set the NatNet server version - else if(strcmp(argv[i], "-version") == 0) { + else if (strcmp(argv[i], "-version") == 0) { check_argcount(argc, argv, i, 1); float version = atof(argv[++i]); @@ -675,13 +718,13 @@ static void parse_options(int argc, char** argv) { natnet_minor = (uint8_t)(version * 10.0) % 10; } // Set the NatNet server data port - else if(strcmp(argv[i], "-data_port") == 0) { + else if (strcmp(argv[i], "-data_port") == 0) { check_argcount(argc, argv, i, 1); natnet_data_port = atoi(argv[++i]); } // Set the NatNet server command port - else if(strcmp(argv[i], "-cmd_port") == 0) { + else if (strcmp(argv[i], "-cmd_port") == 0) { check_argcount(argc, argv, i, 1); natnet_cmd_port = atoi(argv[++i]); @@ -710,31 +753,31 @@ static void parse_options(int argc, char** argv) { ltp_def_from_ecef_d(&tracking_ltp, &tracking_ecef); } // Set the tracking system offset angle in degrees - else if(strcmp(argv[i], "-offset_angle") == 0) { + else if (strcmp(argv[i], "-offset_angle") == 0) { check_argcount(argc, argv, i, 1); tracking_offset_angle = atof(argv[++i]); } // Set the transmit frequency - else if(strcmp(argv[i], "-tf") == 0) { + else if (strcmp(argv[i], "-tf") == 0) { check_argcount(argc, argv, i, 1); freq_transmit = atoi(argv[++i]); } // Set the minimum amount of velocity samples for the differentiator - else if(strcmp(argv[i], "-vel_samples") == 0) { + else if (strcmp(argv[i], "-vel_samples") == 0) { check_argcount(argc, argv, i, 1); min_velocity_samples = atoi(argv[++i]); } // Set to use small packets - else if(strcmp(argv[i], "-small") == 0) { + else if (strcmp(argv[i], "-small") == 0) { small_packets = TRUE; } // Set the ivy bus - else if(strcmp(argv[i], "-ivy_bus") == 0) { + else if (strcmp(argv[i], "-ivy_bus") == 0) { check_argcount(argc, argv, i, 1); ivy_bus = argv[++i]; @@ -749,14 +792,14 @@ static void parse_options(int argc, char** argv) { } // Check if at least one aircraft is set - if(count_ac < 1) { + if (count_ac < 1) { fprintf(stderr, "You must specify at least one aircraft (-ac )\n\n"); print_help(argv[0]); exit(EXIT_FAILURE); } } -int main(int argc, char** argv) +int main(int argc, char **argv) { // Set the default tracking system position and angle struct EcefCoor_d tracking_ecef; @@ -768,10 +811,12 @@ int main(int argc, char** argv) // Parse the options from cmdline parse_options(argc, argv); - printf_debug("Tracking system Latitude: %f Longitude: %f Offset to North: %f degrees\n", DegOfRad(tracking_ltp.lla.lat), DegOfRad(tracking_ltp.lla.lon), DegOfRad(tracking_offset_angle)); + printf_debug("Tracking system Latitude: %f Longitude: %f Offset to North: %f degrees\n", DegOfRad(tracking_ltp.lla.lat), + DegOfRad(tracking_ltp.lla.lon), DegOfRad(tracking_offset_angle)); // Create the network connections - printf_debug("Starting NatNet listening (multicast address: %s, data port: %d, version: %d.%d)\n", natnet_multicast_addr, natnet_data_port, natnet_major, natnet_minor); + printf_debug("Starting NatNet listening (multicast address: %s, data port: %d, version: %d.%d)\n", + natnet_multicast_addr, natnet_data_port, natnet_major, natnet_minor); udp_socket_create(&natnet_data, "", -1, natnet_data_port, 0); // Only receiving udp_socket_subscribe_multicast(&natnet_data, natnet_multicast_addr); udp_socket_set_recvbuf(&natnet_data, 0x100000); // 1MB @@ -787,8 +832,8 @@ int main(int argc, char** argv) // Create the main timers printf_debug("Starting transmitting and sampling timeouts (transmitting frequency: %dHz, minimum velocity samples: %d)\n", - freq_transmit, min_velocity_samples); - g_timeout_add(1000/freq_transmit, timeout_transmit_callback, NULL); + freq_transmit, min_velocity_samples); + g_timeout_add(1000 / freq_transmit, timeout_transmit_callback, NULL); GIOChannel *sk = g_io_channel_unix_new(natnet_data.sockfd); g_io_add_watch(sk, G_IO_IN | G_IO_NVAL | G_IO_HUP, diff --git a/sw/ground_segment/python/guided_mode_example.py b/sw/ground_segment/python/guided_mode_example.py index 8555739bbc..ac6279e10d 100755 --- a/sw/ground_segment/python/guided_mode_example.py +++ b/sw/ground_segment/python/guided_mode_example.py @@ -118,16 +118,19 @@ class Guidance(object): if __name__ == '__main__': ac_id = 30 - g = Guidance(ac_id) - sleep(0.1) - g.set_guided_mode() - sleep(0.2) - g.goto_ned(north=10.0, east=5.0, down=-5.0, heading=radians(90)) - sleep(10) - g.goto_ned_relative(north=-5.0, east=-5.0, down=-2.0, yaw=-radians(45)) - sleep(10) - g.goto_body_relative(forward=0.0, right=5.0, down=2.0) - sleep(10) - g.set_nav_mode() - sleep(0.2) + try: + g = Guidance(ac_id) + sleep(0.1) + g.set_guided_mode() + sleep(0.2) + g.goto_ned(north=10.0, east=5.0, down=-5.0, heading=radians(90)) + sleep(10) + g.goto_ned_relative(north=-5.0, east=-5.0, down=-2.0, yaw=-radians(45)) + sleep(10) + g.goto_body_relative(forward=0.0, right=5.0, down=2.0) + sleep(10) + g.set_nav_mode() + sleep(0.2) + except KeyboardInterrupt: + print("Stopping on request") g.shutdown() diff --git a/sw/ground_segment/python/move_waypoint_example.py b/sw/ground_segment/python/move_waypoint_example.py index e2f92cfcaa..128b143060 100755 --- a/sw/ground_segment/python/move_waypoint_example.py +++ b/sw/ground_segment/python/move_waypoint_example.py @@ -4,6 +4,7 @@ from __future__ import print_function import sys from os import path, getenv +from time import sleep # if PAPARAZZI_SRC not set, then assume the tree containing this # file is a reasonable substitute @@ -13,6 +14,7 @@ sys.path.append(PPRZ_SRC + "/sw/lib/python") from ivy_msg_interface import IvyMessagesInterface from pprz_msg.message import PprzMessage + class WaypointMover(object): def __init__(self, verbose=False): self.verbose = verbose @@ -41,6 +43,12 @@ class WaypointMover(object): if __name__ == '__main__': - wm = WaypointMover() - wm.move_waypoint(ac_id=202, wp_id=3, lat=43.563, lon=1.481, alt=172.0) + try: + wm = WaypointMover() + # sleep shortly in oder to make sure Ivy is up, then message sent before shutting down again + sleep(0.1) + wm.move_waypoint(ac_id=202, wp_id=3, lat=43.563, lon=1.481, alt=172.0) + sleep(0.1) + except KeyboardInterrupt: + print("Stopping on request") wm.shutdown() diff --git a/sw/ground_segment/python/real_time_plot/plotpanel.py b/sw/ground_segment/python/real_time_plot/plotpanel.py index d2e14c2e22..c4172af87a 100644 --- a/sw/ground_segment/python/real_time_plot/plotpanel.py +++ b/sw/ground_segment/python/real_time_plot/plotpanel.py @@ -21,7 +21,7 @@ from pprz_msg import messages_xml_map class PlotData: - def __init__(self, ivy_msg_id, title, width, color=None): + def __init__(self, ivy_msg_id, title, width, color=None, scale=1.0): self.id = ivy_msg_id self.title = title self.SetPlotSize(width) @@ -32,14 +32,14 @@ class PlotData: self.std_dev = 0.0 self.real_time = False - self.scale = 1.0 + self.scale = scale self.offset = 0.0 if color is not None: self.color = color else: r, g, b = random.randint(0, 255), random.randint(0, 255), random.randint(0, 255) - self.color = wx.Color(r, g, b) + self.color = wx.Colour(r, g, b) def SetRealTime(self, value): self.real_time = value @@ -176,7 +176,7 @@ class PlotPanel(object): self.width = 800 self.height = 200 self.margin = min(self.height / 10, 20) - self.font = wx.Font(self.margin / 2, wx.DEFAULT, wx.FONTFLAG_DEFAULT, wx.FONTWEIGHT_NORMAL) + self.font = wx.Font(self.margin / 2, wx.DEFAULT, wx.FONTSTYLE_NORMAL, wx.FONTWEIGHT_NORMAL) self.pixmap = wx.EmptyBitmap(self.width, self.height) self.plot_size = self.width self.max = -1e32 @@ -253,7 +253,7 @@ class PlotPanel(object): def OnDropText(self, data): [ac_id, category, message, field, scale] = data.encode('ASCII').split(':') - self.BindCurve(int(ac_id), message, field) + self.BindCurve(int(ac_id), message, field, scale=float(scale)) def OnIvyMsg(self, agent, *larg): # print(larg[0]) @@ -282,7 +282,7 @@ class PlotPanel(object): plot.index = self.x_axis.index plot.AddPoint(point, self.x_axis) - def BindCurve(self, ac_id, message, field, color=None, use_as_x=False): + def BindCurve(self, ac_id, message, field, color=None, use_as_x=False, scale=1.0): # -- add this telemetry to our list of things to plot ... message_string = _IVY_STRING % (ac_id, message) # print('Binding to %s' % message_string) @@ -300,7 +300,7 @@ class PlotPanel(object): ivy_id = IvyBindMsg(self.OnIvyMsg, str(message_string)) title = '%i:%s:%s' % (ac_id, message, field) - self.plots[ac_id][message][field] = PlotData(ivy_id, title, self.plot_size, color) + self.plots[ac_id][message][field] = PlotData(ivy_id, title, self.plot_size, color, scale) self.frame.AddCurve(ivy_id, title, use_as_x) if use_as_x: self.x_axis = self.plots[ac_id][message][field] @@ -384,7 +384,7 @@ class PlotPanel(object): self.height = height self.plot_size = width self.margin = min(self.height / 10, 20) - self.font = wx.Font(self.margin / 2, wx.DEFAULT, wx.FONTFLAG_DEFAULT, wx.FONTWEIGHT_NORMAL) + self.font = wx.Font(self.margin / 2, wx.DEFAULT, wx.FONTSTYLE_NORMAL, wx.FONTWEIGHT_NORMAL) def OnTimer(self): self.timer.Restart(self.plot_interval) diff --git a/sw/ground_segment/tmtc/server.ml b/sw/ground_segment/tmtc/server.ml index bd31b31b85..30af64a196 100644 --- a/sw/ground_segment/tmtc/server.ml +++ b/sw/ground_segment/tmtc/server.ml @@ -41,7 +41,7 @@ module Tm_Pprz = PprzLink.Messages (struct let name = "telemetry" end) module Alerts_Pprz = PprzLink.Messages(struct let name = "alert" end) module Dl_Pprz = PprzLink.Messages (struct let name = "datalink" end) - +let dl_id = "ground_dl" (* Hack, should be [my_id] *) let (//) = Filename.concat let logs_path = Env.paparazzi_home // "var" // "logs" @@ -373,7 +373,7 @@ let send_aircraft_msg = fun ac -> "speed", cm_of_m a.gspeed; "climb", cm_of_m a.climb; "itow", PprzLink.Int64 a.itow] in - Dl_Pprz.message_send my_id "ACINFO" ac_info; + Dl_Pprz.message_send dl_id "ACINFO" ac_info; end; if !Kml.enabled then @@ -649,7 +649,7 @@ let send_intruder_acinfo = fun id intruder -> "speed", cm_of_m intruder.Intruder.gspeed; "climb", cm_of_m intruder.Intruder.climb; "itow", PprzLink.Int64 intruder.Intruder.itow] in - Dl_Pprz.message_send my_id "ACINFO" ac_info + Dl_Pprz.message_send dl_id "ACINFO" ac_info let periodic_handle_intruders = fun () -> (* remove old intruders after 10s *) @@ -737,8 +737,6 @@ let cm_of_m = fun f -> PprzLink.Int (truncate ((100. *. f) +. 0.5)) (** Convert to mm, with rounding *) let mm_of_m_32 = fun f -> PprzLink.Int32 (Int32.of_int (truncate ((1000. *. f) +. 0.5))) -let dl_id = "ground_dl" (* Hack, should be [my_id] *) - (** Got a ground.MOVE_WAYPOINT and send a datalink.MOVE_WP *) let move_wp = fun logging _sender vs -> let f = fun a -> List.assoc a vs diff --git a/sw/lib/python/ivy_msg_interface.py b/sw/lib/python/ivy_msg_interface.py index c7d999aa8e..48a691201b 100644 --- a/sw/lib/python/ivy_msg_interface.py +++ b/sw/lib/python/ivy_msg_interface.py @@ -28,15 +28,15 @@ class IvyMessagesInterface(object): IvyUnBindMsg(self.ivy_id) def shutdown(self): - self.stop() try: IvyStop() + self.stop() except IvyIllegalStateError as e: print(e) def __del__(self): try: - IvyUnBindMsg(self.ivy_id) + self.shutdown() except: pass @@ -72,28 +72,21 @@ class IvyMessagesInterface(object): return # check which message class it is - # pass non-telemetry messages with ac_id 0 - if data[0] in ["sim", "ground_dl", "dl"]: - if self.verbose: - print("ignoring message " + larg[0]) - sys.stdout.flush() + msg_name = data[1] + msg_class, msg_name = messages_xml_map.find_msg_by_name(msg_name) + if msg_class is None: + print("Ignoring unknown message " + larg[0]) return - elif data[0] in ["ground"]: - msg_class = data[0] - msg_name = data[1] - ac_id = 0 - values = list(filter(None, data[2:])) - else: + # pass non-telemetry messages with ac_id 0 + if msg_class == "telemetry": try: ac_id = int(data[0]) except ValueError: - if self.verbose: - print("ignoring message " + larg[0]) - sys.stdout.flush() - return - msg_class = "telemetry" - msg_name = data[1] - values = list(filter(None, data[2:])) + print("ignoring message " + larg[0]) + sys.stdout.flush() + else: + ac_id = 0 + values = list(filter(None, data[2:])) msg = PprzMessage(msg_class, msg_name) msg.set_values(values) self.callback(ac_id, msg) diff --git a/sw/lib/python/pprz_msg/messages_xml_map.py b/sw/lib/python/pprz_msg/messages_xml_map.py index bd74e02550..c0c78b0b0b 100755 --- a/sw/lib/python/pprz_msg/messages_xml_map.py +++ b/sw/lib/python/pprz_msg/messages_xml_map.py @@ -72,6 +72,17 @@ def parse_messages(messages_file=''): message_dictionary_coefs[class_name][message_id].append(1.) +def find_msg_by_name(name): + if not message_dictionary: + parse_messages() + for msg_class in message_dictionary: + if name in message_dictionary[msg_class]: + #print("found msg name %s in class %s" % (name, msg_class)) + return msg_class, name + print("Error: msg_name %s not found." % name) + return None, None + + def get_msgs(msg_class): if not message_dictionary: parse_messages() diff --git a/sw/tools/bluegiga_usb_dongle/main.c b/sw/tools/bluegiga_usb_dongle/main.c index e098263059..458ac0f1fa 100644 --- a/sw/tools/bluegiga_usb_dongle/main.c +++ b/sw/tools/bluegiga_usb_dongle/main.c @@ -134,6 +134,7 @@ FUNCTION ANALYSIS: // uncomment the following line to show outgoing/incoming BGAPI packet data //#define DEBUG +#define DEBUG_BROADCAST // timeout for serial port read operations #define UART_TIMEOUT 1000 @@ -166,7 +167,7 @@ int count[8] = {0, 0, 0, 0, 0, 0, 0, 0}, send_count[8] = {0, 0, 0, 0, 0, 0, 0, 0 int rssi_count = 0; int last0 = 0, last1 = 0; -uint8_t connection_interval = 10; // connection interval in ms +float connection_interval = 10.; // connection interval in ms int ac_id[8] = { -1, -1, -1, -1, -1, -1, -1, -1}; @@ -182,6 +183,7 @@ unsigned int send_extract_idx[8] = {0, 0, 0, 0, 0, 0, 0, 0}; int connected_devices = 0; //bd_addr found_devices[MAX_DEVICES]; int connected[] = {0, 0, 0, 0, 0, 0, 0, 0}; +bd_addr connected_addr[MAX_DEVICES]; int connect_all = 0; uint8 MAC_ADDR[] = {0x00, 0x00, 0x2d, 0x80, 0x07, 0x00}; @@ -190,6 +192,8 @@ uint8 MAC_ADDR[] = {0x00, 0x00, 0x2d, 0x80, 0x07, 0x00}; // {0x00,0x00,0x2d,0x80,0x07,0x00}; // beginning of all modules adresses +FILE* rssi_fp = NULL; + // list all possible pending actions enum actions { action_none, @@ -197,7 +201,8 @@ enum actions { action_connect, action_info, action_connect_all, - action_broadcast + action_broadcast, + action_broadcast_connect }; enum actions action = action_none; @@ -312,6 +317,19 @@ void print_bdaddr(bd_addr bdaddr) bdaddr.addr[0]); } +/** + * Copy Bluetooth MAC address in hexadecimal notation + * + * @param dst Bluetooth MAC address, destination + * @param src Bluetooth MAC address, source + */ +void cpy_bdaddr(uint8_t* dst, const uint8_t* src) +{ + uint8_t i = 0; + for(i = 0; i < 6; i++) + dst[i] = src[i]; +} + /** * Display raw BGAPI packet in hexadecimal notation * @@ -344,7 +362,7 @@ void send_api_packet(uint8 len1, uint8 *data1, uint16 len2, uint8 *data2) { #ifdef DEBUG // display outgoing BGAPI packet - print_raw_packet((struct ble_header *)data1, data2, 1); +// print_raw_packet((struct ble_header *)data1, data2); #endif if (uart_tx(len1, data1) || uart_tx(len2, data2)) { // uart_tx returns non-zero on failure @@ -386,7 +404,7 @@ int read_api_packet(int timeout_ms) #ifdef DEBUG // display incoming BGAPI packet - print_raw_packet(&hdr, data, 0); + print_raw_packet(&hdr, data); #endif if (!msg) { @@ -446,26 +464,41 @@ void ble_rsp_system_get_info(const struct ble_msg_system_get_info_rsp_t *msg) */ void ble_evt_gap_scan_response(const struct ble_msg_gap_scan_response_evt_t *msg) { - if (action == action_broadcast) { - fprintf(stderr, "advertisement from: "); - print_bdaddr(msg->sender); - fprintf(stderr, " data: "); - int i; - for (i = 0; i < msg->data.len; i++) { + +#ifdef DEBUG_BROADCAST + if(cmp_addr(msg->sender.addr, MAC_ADDR) >= 3 )// && msg->sender.addr[0] == 0xdf) + { + gettimeofday(&tm, NULL); //Time zone struct is obsolete, hence NULL + mytime = (double)tm.tv_sec + (double)tm.tv_usec / 1000000.0; + fprintf(stderr, "%f %x %d, ", mytime, msg->sender.addr[0], msg->rssi); + uint8_t i = 0; + for(i = 0; i < msg->data.len; i++) fprintf(stderr, "%02x ", msg->data.data[i]); - } fprintf(stderr, "\n"); + } +#endif + + if(rssi_fp) + { + fprintf(rssi_fp, "%f %d %d\n", mytime, msg->sender.addr[0], msg->rssi); + fflush(rssi_fp); + } + + if (action == action_broadcast) { if (sock[0]) - sendto(sock[0], msg->data.data, msg->data.len, MSG_DONTWAIT, + sendto(sock[0], msg->data.data+13, msg->data.len-13, MSG_DONTWAIT, (struct sockaddr *)&send_addr[0], sizeof(struct sockaddr)); - } else { + //printf("first: %02x, last: %02x\n", msg->data.data[13], msg->data.data[msg->data.len]); + } + + if (action != action_broadcast) { uint8_t i, j; char *name = NULL; // Check if this device already found, if not add to the list if (!connect_all) { - fprintf(stderr, "New device found: "); + //fprintf(stderr, "New device found: "); // Parse data for (i = 0; i < msg->data.len;) { @@ -535,7 +568,15 @@ void ble_evt_gap_scan_response(const struct ble_msg_gap_scan_response_evt_t *msg } // automatically connect if responding device has appropriate mac address header - if (connect_all && cmp_addr(msg->sender.addr, MAC_ADDR) >= 4) { + // check if bluegiga drone and connectable + if (connect_all && msg->packet_type == 0 && cmp_addr(msg->sender.addr, MAC_ADDR) >= 3) { + uint8 i = 0; + while(i++ < MAX_DEVICES) + { + if (!cmp_addr(msg->sender.addr, connected_addr[i].addr)) + return; + } + fprintf(stderr, "Trying to connect to "); print_bdaddr(msg->sender); fprintf(stderr, "\n"); //change_state(state_connecting); // connection interval unit 1.25ms @@ -575,6 +616,7 @@ void ble_evt_connection_status(const struct ble_msg_connection_status_evt_t *msg // Connection request completed else if (msg->flags & connection_completed) { if (msg->connection + 1 > connected_devices) { connected_devices++; } + cpy_bdaddr(connected_addr[msg->connection].addr, msg->address.addr); //change_state(state_connected); connection_interval = msg->conn_interval * 1.25; fprintf(stderr, "Connected, nr: %d, connection interval: %d = %fms\n", msg->connection, msg->conn_interval, @@ -612,13 +654,16 @@ void ble_evt_connection_status(const struct ble_msg_connection_status_evt_t *msg change_state(state_listening_measurements); enable_indications(msg->connection, drone_handle_configuration); if (connect_all) { - ble_cmd_gap_discover(gap_discover_generic); + ble_cmd_gap_discover(gap_discover_generic); } } // Find primary services else { change_state(state_finding_services); ble_cmd_attclient_read_by_group_type(msg->connection, FIRST_HANDLE, LAST_HANDLE, 2, primary_service_uuid); + if (connect_all) { + ble_cmd_gap_discover(gap_discover_generic); + } } } } @@ -677,9 +722,6 @@ void ble_evt_attclient_procedure_completed(const struct ble_msg_attclient_proced else { change_state(state_listening_measurements); enable_indications(msg->connection, drone_handle_configuration); - if (connect_all) { - ble_cmd_gap_discover(gap_discover_generic); - } } } @@ -708,6 +750,13 @@ void ble_evt_attclient_find_information_found(const struct ble_msg_attclient_fin drone_handle_measurement = msg->chrhandle; } else if (uuid == DRONE_BROADCAST_UUID) { drone_handle_broadcast = msg->chrhandle; + + // Handle for Drone Data configuration already known + if (action == action_broadcast_connect) { + unsigned char var[] = {1}; + printf("Request sent: %d\n", var[0]); + ble_cmd_attclient_attribute_write(msg->connection, drone_handle_broadcast, 1, &var); + } } } } @@ -749,6 +798,8 @@ void ble_evt_attclient_attribute_value(const struct ble_msg_attclient_attribute_ if (sock[msg->connection]) sendto(sock[msg->connection], msg->value.data, msg->value.len, MSG_DONTWAIT, (struct sockaddr *)&send_addr[msg->connection], sizeof(struct sockaddr)); + //printf("%02x %02x %02x %02x\n", msg->value.data[0], msg->value.data[1], msg->value.data[2], msg->value.data[3]); + //printf("%d\n", msg->value.len); } @@ -824,12 +875,19 @@ void *send_msg() // fprintf(stderr,"Long msg: %d, buff size: %d\n", bt_msg_len, diff); //ble_cmd_attclient_attribute_write(device, drone_handle_measurement, bt_msg_len[device], &data_buf[device][extract_idx[device]]); - ble_cmd_attclient_write_command(device, drone_handle_measurement, bt_msg_len, &data_buf[device][extract_idx[device]]); - extract_idx[device] = (extract_idx[device] + bt_msg_len) % BUF_SIZE; + uint16_t i = 0; + unsigned char buf[19]; + for (i = 0; i < bt_msg_len; i++) + { + buf[i] = data_buf[device][extract_idx[device]]; + extract_idx[device] = (extract_idx[device] + 1) % BUF_SIZE; + } + + ble_cmd_attclient_write_command(device, drone_handle_measurement, bt_msg_len, buf); } device++; } // next device - usleep(connection_interval * 1000); // send messages at max intervals of the connection interval + usleep(connection_interval * 1000*2); // send messages at max intervals of the connection interval, 2 safety factor } // repeat } pthread_exit(NULL); @@ -858,18 +916,21 @@ void *recv_paparazzi_comms() if (connected[device] && sock[device]) { bytes_recv = recvfrom(sock[device], recv_data, BUF_SIZE, MSG_DONTWAIT, (struct sockaddr *)&rec_addr[device], (socklen_t *)&sin_size); - if (bytes_recv > 0) { // TODO: can overtake extract! + // ensure we don't overtake reading + if (bytes_recv > 0 && bytes_recv - 1 <= (extract_idx[device] - insert_idx[device] - 1 + BUF_SIZE) % BUF_SIZE ) { + uint16_t i = 0; + for (i = 0; i < bytes_recv; i++){ + data_buf[device][insert_idx[device]] = recv_data[i]; + insert_idx[device] = (insert_idx[device] + 1) % BUF_SIZE; + } send_count[device] += bytes_recv; - memcpy(&data_buf[device][insert_idx[device]], recv_data, bytes_recv); - - insert_idx[device] = (insert_idx[device] + bytes_recv) % BUF_SIZE; } } device++; } } } - usleep(20000); // assuming connection interval 10ms, give a bit of overhead + usleep(connection_interval * 1000 * 2); // assuming connection interval 10ms, give a bit of overhead } pthread_exit(NULL); } @@ -918,7 +979,7 @@ int main(int argc, char *argv[]) action = action_scan; } else if (strcmp(argv[CLARG_ACTION], "info") == 0) { action = action_info; - } else if (strcmp(argv[CLARG_ACTION], "broadcast") == 0) { + } else if (strcmp(argv[CLARG_ACTION], "broadcast") == 0 || strcmp(argv[CLARG_ACTION], "broadcast_connect") == 0) { action = action_broadcast; if (argc > CLARG_ACTION + 2) { send_port = atoi(argv[CLARG_ACTION + 1]); @@ -927,6 +988,10 @@ int main(int argc, char *argv[]) usage(argv[0]); return 1; } + if (strcmp(argv[CLARG_ACTION], "broadcast_connect") == 0) { + connect_all = 1; + action = action_broadcast_connect; + } } else if (strcmp(argv[CLARG_ACTION], "all") == 0) { connect_all = 1; action = action_scan; @@ -968,6 +1033,23 @@ int main(int argc, char *argv[]) return 1; } + size_t i = 0; + for (i = 0; i < argc; i++) + { + if(strcmp(argv[i],"log") == 0){ + time_t timev; + time(&timev); + char timedate[256]; + strftime(timedate, 256, "var/logs/%Y%m%d_%H%M%S.rssilog", localtime(&timev)); + rssi_fp = fopen(timedate, "w"); + if (!rssi_fp) + { + fprintf(stderr,"Unable to open file for logging: %s\n Make sure to run from paparazzi home\n", timedate); + return -1; + } + } + } + // set BGLib output function pointer to "send_api_packet" function bglib_output = send_api_packet; @@ -1003,8 +1085,12 @@ int main(int argc, char *argv[]) // get the mac address of the dongle ble_cmd_system_address_get(); + // advertise interval scales 625us, min, max, channels (0x07 = 3, 0x03 = 2, 0x04 = 1) + if (action == action_broadcast) + ble_cmd_gap_set_adv_parameters(0x20, 0x28, 0x07); + // Execute action - if (action == action_scan) { + if (action == action_scan || action == action_broadcast || action == action_broadcast_connect) { ble_cmd_gap_discover(gap_discover_generic); } else if (action == action_info) { ble_cmd_system_get_info(); @@ -1012,9 +1098,6 @@ int main(int argc, char *argv[]) fprintf(stderr, "Trying to connect\n"); change_state(state_connecting); ble_cmd_gap_connect_direct(&connect_addr, gap_address_type_public, 6, 16, 100, 0); - } else if (action == action_broadcast) { - ble_cmd_gap_set_adv_parameters(0x200, 0x200, - 0x07); // advertise interval scales 625us, min, max, channels (0x07 = 3, 0x03 = 2, 0x04 = 1) } pthread_create(&threads[0], NULL, send_msg, NULL); @@ -1032,4 +1115,7 @@ int main(int argc, char *argv[]) uart_close(); pthread_exit(NULL); + + if (rssi_fp) + fclose(rssi_fp); } diff --git a/sw/tools/calibration/calibrate.py b/sw/tools/calibration/calibrate.py index 74f15b0c37..fb28092b4f 100755 --- a/sw/tools/calibration/calibrate.py +++ b/sw/tools/calibration/calibrate.py @@ -44,9 +44,9 @@ def main(): parser.add_option("-p", "--plot", help="Show resulting plots", action="store_true", dest="plot") - parser.add_option("-a", "--auto_threshold", - help="Try to automatically determine noise threshold", - action="store_true", dest="auto_threshold") + parser.add_option("--noise_threshold", + help="specify noise threshold instead of automatically determining it", + action="store", dest="noise_threshold", default=0) parser.add_option("-v", "--verbose", action="store_true", dest="verbose") (options, args) = parser.parse_args() @@ -75,7 +75,7 @@ def main(): sensor_ref = 9.81 sensor_res = 10 noise_window = 20 - noise_threshold = 40 + noise_threshold = options.noise_threshold elif options.sensor == "MAG": sensor_ref = 1. sensor_res = 11 @@ -98,10 +98,12 @@ def main(): print("Error: all IMU_"+options.sensor+"_RAW measurements are zero!") sys.exit(1) - # estimate the noise threshold - # find the median of measurement vector lenght - if options.auto_threshold: - meas_median = scipy.median(scipy.array([scipy.linalg.norm(v) for v in measurements])) + # estimate the noise threshold if not explicitly given + if noise_threshold <= 0: + # mean over all measurements (flattended array) as approx neutral value + neutral = scipy.mean(measurements) + # find the median of measurement vector length after subtracting approximate neutral + meas_median = scipy.median(scipy.array([scipy.linalg.norm(v - neutral) for v in measurements])) # set noise threshold to be below 10% of that noise_threshold = meas_median * 0.1 if options.verbose: @@ -113,7 +115,8 @@ def main(): print("remaining "+str(len(flt_meas))+" after filtering") if len(flt_meas) == 0: print("Error: found zero IMU_" + options.sensor + "_RAW measurements for aircraft with id " + options.ac_id + - " in log file after filtering!\nMaybe try the --auto_threshold option.") + " in log file after filtering with noise threshold of " + noise_threshold + + "!\nMaybe try specifying manually with the --noise_threshold option.") if options.plot: calibration_utils.plot_measurements(options.sensor, measurements) sys.exit(1) diff --git a/sw/tools/generators/gen_aircraft.ml b/sw/tools/generators/gen_aircraft.ml index b07fc52fa0..2ad9df9a93 100644 --- a/sw/tools/generators/gen_aircraft.ml +++ b/sw/tools/generators/gen_aircraft.ml @@ -107,7 +107,7 @@ let module_xml2mk = fun f target m -> let section = let targets = Gen_common.targets_of_field section Env.default_module_targets in if Gen_common.test_targets target targets then section else Xml.Element ("makefile", [], []) - in + in Xml.iter (fun field -> match String.lowercase (Xml.tag field) with @@ -163,7 +163,7 @@ let subsystem_xml2mk = fun f firmware s -> fprintf f "\n# -subsystem: '%s'\n" name; let s_config, rest = ExtXml.partition_tag "configure" (Xml.children s) in let s_defines, _ = ExtXml.partition_tag "define" rest in - List.iter (configure_xml2mk f) s_config; + (*List.iter (configure_xml2mk f) s_config;*) List.iter (fun def -> define_xml2mk f def) s_defines; (* include subsystem *) (* TODO test if file exists with the generator ? *) let s_name = name ^ s_type ^ ".makefile" in @@ -174,6 +174,10 @@ let subsystem_xml2mk = fun f firmware s -> fprintf f "\tinclude $(CFG_SHARED)/%s\n" s_name; fprintf f "endif\n" +let subsystem_configure_xml2mk = fun f s -> + let s_config, _ = ExtXml.partition_tag "configure" (Xml.children s) in + List.iter (configure_xml2mk f) s_config + let mod_or_subsys_xml2mk = fun f global_targets firmware target xml -> try let m = Gen_common.get_module xml global_targets in @@ -208,7 +212,11 @@ let parse_firmware = fun makefile_ac ac_xml firmware -> end; List.iter (configure_xml2mk makefile_ac) config; List.iter (configure_xml2mk makefile_ac) t_config; - fprintf makefile_ac "include $(PAPARAZZI_SRC)/conf/boards/%s.makefile\n" (Xml.attrib target "board"); + List.iter (subsystem_configure_xml2mk makefile_ac) subsystems; + List.iter (subsystem_configure_xml2mk makefile_ac) t_subsystems; + List.iter (subsystem_configure_xml2mk makefile_ac) mods; + List.iter (subsystem_configure_xml2mk makefile_ac) t_mods; + fprintf makefile_ac "\ninclude $(PAPARAZZI_SRC)/conf/boards/%s.makefile\n" (Xml.attrib target "board"); fprintf makefile_ac "include $(PAPARAZZI_SRC)/conf/firmwares/%s.makefile\n" (Xml.attrib firmware "name"); List.iter (fun def -> define_xml2mk makefile_ac def) defines; List.iter (fun def -> define_xml2mk makefile_ac def) t_defines;