Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-24 22:05:58 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'paparazzi/master' into pprzlink

Browse Source 
fix conflicts and update bluegiga datalink event
This commit is contained in:
Gautier Hattenberger
2016-01-14 14:32:06 +01:00
parent 0d09106431 d6f35c2aa6
commit e0258e10a8
65 changed files with 1455 additions and 656 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/TUDELFT/tudelft_KM_control_panel.xml
+2 -2
View File
@@ -68,8 +68,8 @@
<arg flag="-speech" />
</program>
<program name="NatNet">
<arg flag="-ac" constant="1"/>
<arg flag="12"/>
<arg flag="-ac" constant="7"/>
<arg flag="114"/>
<arg flag="-small"/>
<arg flag="-tf" constant="5"/>
</program>
conf/abi.xml
+6
View File
@@ -72,6 +72,12 @@
<field name="z" type="float" unit="m/s"/>
<field name="noise" type="float"/>
</message>
<message name="RSSI" id="13">
<field name="ac_id" type="uint8_t"/>
<field name="source_strength" type="int8_t" unit="dB"/>
<field name="rssi" type="int8_t" unit="dB"/>
</message>
</msg_class>
conf/airframes/BR/ladybird_kit_bart_bluegiga_optitrack.xml
+1 -9
View File
@@ -159,13 +159,7 @@
<section name="INS" prefix="INS_">
<define name="USE_GPS_ALT" value="1"/>
</section>
<section name="GPS" prefix="GPS_">
<define name="USE_DATALINK_SMALL" value="1"/>
<define name="LOCAL_ECEF_ORIGIN_X" value="392433200"/>
<define name="LOCAL_ECEF_ORIGIN_Y" value="30036200"/>
<define name="LOCAL_ECEF_ORIGIN_Z" value="500219700"/>
<define name="USE_GPS_ALT_SPEED" value="1"/>
</section>
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
@@ -183,8 +177,6 @@
<define name="H_Z" value="0.92060036"/>
<define name="USE_GPS_HEADING" value="1"/>
</section>
<define name="USE_GPS" value="TRUE"/>
<define name="USE_GPS_HEADING" value="TRUE"/>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="290"/>
conf/airframes/FLIXR/flixr_zmr250_elle0.xml
+2 -2
View File
@@ -180,8 +180,8 @@
</section>
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="ne_motor, se_motor, sw_motor, nw_motor" type="string[]"/>
<define name="JSBSIM_MODEL" value="simple_x_quad" type="string"/>
<define name="ACTUATOR_NAMES" value="nw_motor, ne_motor, se_motor, sw_motor" type="string[]"/>
<define name="JSBSIM_MODEL" value="simple_x_quad_ccw" type="string"/>
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
<!-- mode switch on joystick channel 5 (axis numbering starting at zero) -->
<define name="JS_AXIS_MODE" value="4"/>
conf/airframes/LS/ls_ladybird_lisa_s_bluegiga_small_gps_messages.xml
-7
View File
@@ -173,13 +173,6 @@
<define name="USE_GPS_ALT" value="1"/>
</section>
<section name="GPS" prefix="GPS_">
<define name="USE_DATALINK_SMALL" value="1"/>
<define name="LOCAL_ECEF_ORIGIN_X" value="392433200"/>
<define name="LOCAL_ECEF_ORIGIN_Y" value="30036200"/>
<define name="LOCAL_ECEF_ORIGIN_Z" value="500219700"/>
</section>
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="HOVER_KP" value="276"/>
<define name="HOVER_KD" value="455"/>
conf/airframes/LS/ls_quadrotor_bebop_small_gps_messages.xml
-7
View File
@@ -102,13 +102,6 @@
<define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
</section>
<section name="GPS" prefix="GPS_">
<define name="USE_DATALINK_SMALL" value="1"/>
<define name="LOCAL_ECEF_ORIGIN_X" value="392433200"/>
<define name="LOCAL_ECEF_ORIGIN_Y" value="30036200"/>
<define name="LOCAL_ECEF_ORIGIN_Z" value="500219700"/>
</section>
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<!-- setpoints -->
<define name="SP_MAX_P" value="140" unit="deg/s"/>
conf/airframes/TUDELFT/tudelft_ladylisa_bluegiga_stereoboard.xml
+8 -6
View File
@@ -181,14 +181,14 @@
<define name="H_X" value="0.39049610"/>
<define name="H_Y" value="0.00278894"/>
<define name="H_Z" value="0.92060036"/>
<define name="USE_GPS_HEADING" value="1"/>
<define name="USE_GPS_HEADING" value="0"/>
</section>
<define name="USE_GPS" value="FALSE"/>
<define name="USE_GPS_HEADING" value="FALSE"/>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="PGAIN" value="40"/>
<define name="DGAIN" value="10"/>
<define name="PGAIN" value="120"/>
<define name="DGAIN" value="40"/>
<define name="IGAIN" value="0"/>
<define name="REF_MAX_SPEED" value="0.5"/>
<define name="USE_REF" value="FALSE"/>
@@ -252,6 +252,8 @@
<subsystem name="gps" type="datalink"/>
<subsystem name="stabilization" type="int_quat"/>
<subsystem name="ahrs" type="int_cmpl_quat"/>
<subsystem name="ins" />
<subsystem name="ins">
<define name="INS_INT_GPS_ID" value="ABI_DISABLE"/>
</subsystem>
</firmware>
</airframe>
conf/boards/naze32_rev4.makefile
+2 -2
View File
@@ -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
#
#
conf/boards/naze32_rev5.makefile
+2 -2
View File
@@ -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
#
#
conf/firmwares/subsystems/fixedwing/gps_ublox_utm.makefile
+2 -26
View File
@@ -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 <subsystem name="gps" type="ublox_utm"/> with <subsystem name="gps" type="ublox"/>)
conf/firmwares/subsystems/fixedwing/gps_piksi.makefile → conf/firmwares/subsystems/shared/gps_piksi.makefile
View File
conf/firmwares/subsystems/shared/gps_ublox.makefile
+1 -1
View File
@@ -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)
conf/firmwares/subsystems/shared/imu_apogee_mpu9150.makefile
+16
View File
@@ -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
conf/flash_modes.xml
+1
View File
@@ -23,6 +23,7 @@
<board name="lia_.*"/>
<board name="lisa_[ms]_.*"/>
<board name="elle*"/>
<board name="naze32*"/>
</boards>
</mode>
<mode name="USB (IAP)">
conf/messages.xml
+3 -3
View File
@@ -856,7 +856,6 @@
<message name="BLUEGIGA" id="106">
<field name="data_rate" type="uint32" unit="bytes/s"/>
<field name="last_msg" type="uint8[]"/>
</message>
<!--107 is free -->
@@ -2479,8 +2478,9 @@
<message name="REMOTE_GPS_SMALL" id="54" link="broadcasted">
<field name="ac_id" type="uint8"/>
<field name="numsv" type="uint8"/>
<field name="pos_xyz" type="uint32" unit="cm">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</field>
<field name="speed_xy" type="uint32" unit="cm/s">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</field>
<field name="pos_xyz" type="uint32" unit="cm">bits 31-21 x position in cm : bits 20-10 y position in cm : bits 9-0 z position in cm</field>
<field name="speed_xyz" type="uint32" unit="cm/s">bits 31-21 speed x in cm/s : bits 20-10 speed y in cm/s : bits 9-0 speed z in cm/s</field>
<field name="heading" type="int16" unit="rad*1e4"/>
</message>
<message name="REMOTE_GPS" id="55" link="broadcasted">
conf/modules/airspeed_ets.xml
+1
View File
@@ -25,6 +25,7 @@
<define name="AIRSPEED_ETS_SYNC_SEND" description="flag to transmit the data as it is acquired"/>
<define name="USE_AIRSPEED_ETS" value="TRUE|FALSE" description="set airspeed in state interface"/>
<define name="AIRSPEED_ETS_3RD_PARTY_MODE" description="read raw value for sensor in third-party mode"/>
<define name="AIRSPEED_ETS_SDLOG" value="TRUE|FALSE" description="start logging to SD card"/>
</doc>
<header>
conf/modules/humid_sht.xml
+1
View File
@@ -8,6 +8,7 @@
</description>
<define name="SHT_DAT_GPIO" value="port,pin" description="data GPIO (e.g.: GPIOA,GPIO8)"/>
<define name="SHT_SCK_GPIO" value="port,pin" description="clock GPIO (e.g.: GPIOA,GPIO9)"/>
<define name="SHT_SDLOG" value="TRUE|FALSE" description="start logging to SD card"/>
</doc>
<header>
<file name="humid_sht.h"/>
conf/modules/temp_temod.xml
+6 -5
View File
@@ -5,6 +5,7 @@
<description>Hygrosens TEMOD-I2C-Rx temperature sensor</description>
<configure name="TEMOD_I2C_DEV" value="i2cX" description="select i2c peripheral to use (default i2c0)"/>
<define name="TEMOD_TYPE" value="type" description="device type (default TEMOD_I2C_R1)"/>
<define name="TEMOD_SDLOG" value="TRUE|FALSE" description="start logging to SD card"/>
</doc>
<header>
<file name="temp_temod.h"/>
@@ -14,12 +15,12 @@
<event fun="temod_event()"/>
<makefile target="ap">
<raw>
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)
</raw>
<define name="USE_$(TEMOD_DEV_UPPER)"/>
<define name="TEMOD_DEV" value="$(TEMOD_DEV_LOWER)"/>
<define name="USE_$(TEMOD_I2C_DEV_UPPER)"/>
<define name="TEMOD_I2C_DEV" value="$(TEMOD_I2C_DEV_LOWER)"/>
<file name="temp_temod.c"/>
</makefile>
</module>
sw/airborne/arch/sim/sim_gps.c
+1 -1
View File
@@ -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);
sw/airborne/boards/apogee/imu_apogee.c
+49 -1
View File
@@ -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
}
sw/airborne/boards/apogee/imu_apogee.h
+6
View File
@@ -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;
sw/airborne/boards/elle0_common.h
+17 -30
View File
@@ -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)
sw/airborne/firmwares/rotorcraft/datalink.c
+20 -18
View File
@@ -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; }
sw/airborne/modules/cam_control/point.c
+1 -1
View File
@@ -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);
sw/airborne/modules/ins/ins_xsens.c
+1 -4
View File
@@ -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
sw/airborne/modules/ins/ins_xsens700.c
+1 -4
View File
@@ -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
sw/airborne/modules/meteo/humid_sht.c
+28
View File
@@ -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
}
}
}
sw/airborne/modules/meteo/meteo_france_DAQ.c
+5 -3
View File
@@ -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);
}
}
sw/airborne/modules/meteo/temp_temod.c
+31
View File
@@ -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
}
}
sw/airborne/modules/sensors/airspeed_ets.c
+28
View File
@@ -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;
}
sw/airborne/peripherals/ak8975.c
+197
View File
@@ -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
* <http://www.gnu.org/licenses/>.
*/
/**
* @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;
}
}
sw/airborne/peripherals/ak8975.h
+92
View File
@@ -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
* <http://www.gnu.org/licenses/>.
*/
/**
* @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 */
sw/airborne/peripherals/lsm303d_regs.h
+152
View File
@@ -0,0 +1,152 @@
/*
* Copyright (C) 2015 Felix Ruess <felix.ruess@gmail.com>
*
* 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
sw/airborne/peripherals/mpu60x0.c
+1
View File
@@ -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;
}
sw/airborne/peripherals/mpu60x0.h
+7 -1
View File
@@ -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
};
sw/airborne/peripherals/mpu60x0_i2c.c
+10 -3
View File
@@ -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;
sw/airborne/peripherals/mpu60x0_spi.c
+10 -3
View File
@@ -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;
sw/airborne/peripherals/mpu9250.c
+1
View File
@@ -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;
}
sw/airborne/peripherals/mpu9250.h
+7 -1
View File
@@ -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
};
sw/airborne/peripherals/mpu9250_i2c.c
+10 -3
View File
@@ -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;
sw/airborne/peripherals/mpu9250_spi.c
+10 -3
View File
@@ -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;
sw/airborne/subsystems/abi_sender_ids.h
+7
View File
@@ -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 */
sw/airborne/subsystems/datalink/bluegiga.c
+95 -75
View File
@@ -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 <string.h>
//#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
sw/airborne/subsystems/datalink/bluegiga.h
+3 -34
View File
@@ -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 */
sw/airborne/subsystems/datalink/datalink.h
+1 -1
View File
@@ -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(); \
}
sw/airborne/subsystems/gps.c
+65 -3
View File
@@ -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;
}
sw/airborne/subsystems/gps.h
+19
View File
@@ -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 */
sw/airborne/subsystems/gps/gps_datalink.c
+53 -66
View File
@@ -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 <stdio.h>
#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 <stdio.h>
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(&ltp_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, &ltp_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 , &ltp_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;
sw/airborne/subsystems/gps/gps_datalink.h
+3 -3
View File
@@ -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,
sw/airborne/subsystems/ins.c
+1 -14
View File
@@ -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;

Some files were not shown because too many files have changed in this diff Show More