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

[imu] directly send ABI messages from implementation

Browse Source
This commit is contained in:
Felix Ruess
2015-03-31 01:01:23 +02:00
parent 6759ea17b2
commit 06367c6708
64 changed files with 597 additions and 891 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/boards/apogee/imu_apogee.c
+7 -5
View File
@@ -31,6 +31,7 @@
#include "boards/apogee/imu_apogee.h"
#include "mcu_periph/i2c.h"
#include "led.h"
#include "subsystems/abi.h"
// Downlink
#include "mcu_periph/uart.h"
@@ -80,9 +81,6 @@ 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;
imu_apogee.gyr_valid = FALSE;
imu_apogee.acc_valid = FALSE;
}
void imu_periodic(void)
@@ -104,6 +102,8 @@ void imu_apogee_downlink_raw(void)
void imu_apogee_event(void)
{
uint32_t now_ts = get_sys_time_usec();
// If the itg3200 I2C transaction has succeeded: convert the data
mpu60x0_i2c_event(&imu_apogee.mpu);
if (imu_apogee.mpu.data_available) {
@@ -112,8 +112,10 @@ void imu_apogee_event(void)
VECT3_ASSIGN(imu.accel_unscaled, imu_apogee.mpu.data_accel.vect.x, -imu_apogee.mpu.data_accel.vect.y,
-imu_apogee.mpu.data_accel.vect.z);
imu_apogee.mpu.data_available = FALSE;
imu_apogee.gyr_valid = TRUE;
imu_apogee.acc_valid = TRUE;
imu_scale_gyro(&imu);
imu_scale_accel(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_APOGEE_ID, now_ts, &imu.gyro);
AbiSendMsgIMU_ACCEL_INT32(IMU_APOGEE_ID, now_ts, &imu.accel);
}
}
sw/airborne/boards/apogee/imu_apogee.h
+1 -16
View File
@@ -95,8 +95,6 @@
#endif
struct ImuApogee {
volatile bool_t gyr_valid;
volatile bool_t acc_valid;
struct Mpu60x0_I2c mpu;
};
@@ -112,19 +110,6 @@ extern void imu_periodic(void);
extern void imu_apogee_event(void);
extern void imu_apogee_downlink_raw(void);
static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void),
void (* _mag_handler)(void) __attribute__((unused)))
{
imu_apogee_event();
if (imu_apogee.gyr_valid) {
imu_apogee.gyr_valid = FALSE;
_gyro_handler();
}
if (imu_apogee.acc_valid) {
imu_apogee.acc_valid = FALSE;
_accel_handler();
}
}
#define ImuEvent imu_apogee_event
#endif // IMU_APOGEE_H
sw/airborne/boards/ardrone/navdata.c
+16 -2
View File
@@ -201,7 +201,6 @@ bool_t navdata_init()
// Reset available flags
navdata_available = FALSE;
navdata.baro_calibrated = FALSE;
navdata.imu_available = FALSE;
navdata.baro_available = FALSE;
navdata.imu_lost = FALSE;
@@ -320,6 +319,20 @@ static void *navdata_read(void *data __attribute__((unused)))
return NULL;
}
#include "subsystems/imu.h"
static void navdata_publish_imu(void)
{
RATES_ASSIGN(imu.gyro_unscaled, navdata.measure.vx, -navdata.measure.vy, -navdata.measure.vz);
VECT3_ASSIGN(imu.accel_unscaled, navdata.measure.ax, 4096 - navdata.measure.ay, 4096 - navdata.measure.az);
VECT3_ASSIGN(imu.mag_unscaled, -navdata.measure.mx, -navdata.measure.my, -navdata.measure.mz);
imu_scale_gyro(&imu);
imu_scale_accel(&imu);
imu_scale_mag(&imu);
uint32_t now_ts = get_sys_time_usec();
AbiSendMsgIMU_GYRO_INT32(IMU_ARDRONE2_ID, now_ts, &imu.gyro);
AbiSendMsgIMU_ACCEL_INT32(IMU_ARDRONE2_ID, now_ts, &imu.accel);
AbiSendMsgIMU_MAG_INT32(IMU_ARDRONE2_ID, now_ts, &imu.mag);
}
/**
* Update the navdata (event loop)
@@ -377,7 +390,8 @@ void navdata_update()
}
#endif
navdata.imu_available = TRUE;
navdata_publish_imu();
navdata.packetsRead++;
}
else {
sw/airborne/boards/ardrone/navdata.h
-1
View File
@@ -122,7 +122,6 @@ struct navdata_t {
struct bmp180_calib_t bmp180_calib; //< BMP180 calibration receieved from navboard
bool_t baro_calibrated; //< Whenever the baro is calibrated
bool_t imu_available; //< Whenever the imu is available
bool_t baro_available; //< Whenever the baro is available
bool_t imu_lost; //< Whenever the imu is lost
};
sw/airborne/boards/hbmini/imu_hbmini.c
+8 -10
View File
@@ -34,6 +34,7 @@
#include "imu_hbmini.h"
#include "mcu_periph/i2c.h"
#include "led.h"
#include "subsystems/abi.h"
// Downlink
#include "mcu_periph/uart.h"
@@ -61,17 +62,11 @@ struct ImuHbmini imu_hbmini;
void imu_impl_init(void)
{
max1168_init();
/////////////////////////////////////////////////////////////////////
// HMC58XX
hmc58xx_init(&imu_hbmini.hmc, &(IMU_HBMINI_I2C_DEV), HMC58XX_ADDR);
imu_hbmini.gyr_valid = FALSE;
imu_hbmini.acc_valid = FALSE;
imu_hbmini.mag_valid = FALSE;
}
void imu_periodic(void)
@@ -97,10 +92,10 @@ void imu_hbmini_downlink_raw(void)
void imu_hbmini_event(void)
{
uint32_t now_ts = get_sys_time_usec();
max1168_event();
if (max1168_status == MAX1168_DATA_AVAILABLE) {
imu.gyro_unscaled.p = max1168_values[IMU_GYRO_P_CHAN];
imu.gyro_unscaled.q = max1168_values[IMU_GYRO_Q_CHAN];
@@ -109,8 +104,10 @@ void imu_hbmini_event(void)
imu.accel_unscaled.y = max1168_values[IMU_ACCEL_Y_CHAN];
imu.accel_unscaled.z = max1168_values[IMU_ACCEL_Z_CHAN];
max1168_status = MAX1168_IDLE;
imu_hbmini.gyr_valid = TRUE;
imu_hbmini.acc_valid = TRUE;
imu_scale_gyro(&imu);
imu_scale_accel(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_HBMINI_ID, now_ts, &imu.gyro);
AbiSendMsgIMU_ACCEL_INT32(IMU_HBMINI_ID, now_ts, &imu.accel);
}
// HMC58XX event task
@@ -121,7 +118,8 @@ void imu_hbmini_event(void)
imu.mag_unscaled.x = imu_hbmini.hmc.data.value[IMU_MAG_Z_CHAN];
imu_hbmini.hmc.data_available = FALSE;
imu_hbmini.mag_valid = TRUE;
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_HBMINI_ID, now_ts, &imu.mag);
}
}
sw/airborne/boards/hbmini/imu_hbmini.h
+1 -23
View File
@@ -37,8 +37,6 @@
#include "generated/airframe.h"
#include "subsystems/imu.h"
//#include "peripherals/itg3200.h"
//#include "peripherals/adxl345_i2c.h"
#include "peripherals/max1168.h"
#include "peripherals/hmc58xx.h"
@@ -61,11 +59,6 @@
struct ImuHbmini {
volatile bool_t gyr_valid;
volatile bool_t acc_valid;
volatile bool_t mag_valid;
//struct Itg3200 itg;
//struct Adxl345_I2c adxl;
struct Hmc58xx hmc;
};
@@ -75,21 +68,6 @@ extern struct ImuHbmini imu_hbmini;
extern void imu_hbmini_event(void);
extern void imu_hbmini_downlink_raw(void);
static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void))
{
imu_hbmini_event();
if (imu_hbmini.gyr_valid) {
imu_hbmini.gyr_valid = FALSE;
_gyro_handler();
}
if (imu_hbmini.acc_valid) {
imu_hbmini.acc_valid = FALSE;
_accel_handler();
}
if (imu_hbmini.mag_valid) {
imu_hbmini.mag_valid = FALSE;
_mag_handler();
}
}
#define ImuEvent imu_hbmini_event
#endif
sw/airborne/boards/krooz/imu_krooz.c
+8 -7
View File
@@ -34,6 +34,7 @@
#include "mcu_periph/i2c.h"
#include "led.h"
#include "filters/median_filter.h"
#include "subsystems/abi.h"
// Downlink
#include "mcu_periph/uart.h"
@@ -88,10 +89,6 @@ void imu_impl_init(void)
VECT3_ASSIGN(imu_krooz.accel_sum, 0, 0, 0);
imu_krooz.meas_nb = 0;
imu_krooz.gyr_valid = FALSE;
imu_krooz.acc_valid = FALSE;
imu_krooz.mag_valid = FALSE;
imu_krooz.hmc_eoc = FALSE;
imu_krooz.mpu_eoc = FALSE;
@@ -127,8 +124,11 @@ void imu_periodic(void)
VECT3_ASSIGN(imu_krooz.accel_sum, 0, 0, 0);
imu_krooz.meas_nb = 0;
imu_krooz.gyr_valid = TRUE;
imu_krooz.acc_valid = TRUE;
uint32_t now_ts = get_sys_time_usec();
imu_scale_gyro(&imu);
imu_scale_accel(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_KROOZ_ID, now_ts, &imu.gyro);
AbiSendMsgIMU_ACCEL_INT32(IMU_KROOZ_ID, now_ts, &imu.accel);
}
//RunOnceEvery(10,imu_krooz_downlink_raw());
@@ -170,6 +170,7 @@ void imu_krooz_event(void)
VECT3_ASSIGN(imu.mag_unscaled, imu_krooz.hmc.data.vect.y, -imu_krooz.hmc.data.vect.x, imu_krooz.hmc.data.vect.z);
UpdateMedianFilterVect3Int(median_mag, imu.mag_unscaled);
imu_krooz.hmc.data_available = FALSE;
imu_krooz.mag_valid = TRUE;
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_KROOZ_ID, get_sys_time_usec(), &imu.mag);
}
}
sw/airborne/boards/krooz/imu_krooz.h
+1 -20
View File
@@ -105,9 +105,6 @@
#endif
struct ImuKrooz {
volatile bool_t gyr_valid;
volatile bool_t acc_valid;
volatile bool_t mag_valid;
volatile bool_t mpu_eoc;
volatile bool_t hmc_eoc;
struct Mpu60x0_I2c mpu;
@@ -131,22 +128,6 @@ extern void imu_periodic(void);
extern void imu_krooz_event(void);
extern void imu_krooz_downlink_raw(void);
static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void),
void (* _mag_handler)(void) __attribute__((unused)))
{
imu_krooz_event();
if (imu_krooz.gyr_valid) {
imu_krooz.gyr_valid = FALSE;
_gyro_handler();
}
if (imu_krooz.acc_valid) {
imu_krooz.acc_valid = FALSE;
_accel_handler();
}
if (imu_krooz.mag_valid) {
imu_krooz.mag_valid = FALSE;
_mag_handler();
}
}
#define ImuEvent imu_krooz_event
#endif // IMU_KROOZ_H
sw/airborne/boards/krooz/imu_krooz_memsic.c
+7 -7
View File
@@ -36,6 +36,7 @@
#include "led.h"
#include "filters/median_filter.h"
#include "mcu_periph/sys_time.h"
#include "subsystems/abi.h"
#if !defined KROOZ_LOWPASS_FILTER && !defined KROOZ_SMPLRT_DIV
#define KROOZ_LOWPASS_FILTER MPU60X0_DLPF_256HZ
@@ -89,10 +90,6 @@ void imu_impl_init(void)
VECT3_ASSIGN(imu_krooz.accel_sum, 0, 0, 0);
imu_krooz.meas_nb = 0;
imu_krooz.gyr_valid = FALSE;
imu_krooz.acc_valid = FALSE;
imu_krooz.mag_valid = FALSE;
imu_krooz.hmc_eoc = FALSE;
imu_krooz.mpu_eoc = FALSE;
@@ -133,7 +130,8 @@ void imu_periodic(void)
RATES_ASSIGN(imu_krooz.rates_sum, 0, 0, 0);
imu_krooz.meas_nb = 0;
imu_krooz.gyr_valid = TRUE;
imu_scale_gyro(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_KROOZ_ID, now_ts, &imu.gyro);
}
if (imu_krooz.meas_nb_acc.x && imu_krooz.meas_nb_acc.y && imu_krooz.meas_nb_acc.z) {
@@ -151,7 +149,8 @@ void imu_periodic(void)
INT_VECT3_ZERO(imu_krooz.accel_sum);
INT_VECT3_ZERO(imu_krooz.meas_nb_acc);
imu_krooz.acc_valid = TRUE;
imu_scale_accel(&imu);
AbiSendMsgIMU_ACCEL_INT32(IMU_KROOZ_ID, now_ts, &imu.accel);
}
RunOnceEvery(128, {axis_nb = 5;});
@@ -224,6 +223,7 @@ void imu_krooz_event(void)
VECT3_ASSIGN(imu.mag_unscaled, imu_krooz.hmc.data.vect.y, -imu_krooz.hmc.data.vect.x, imu_krooz.hmc.data.vect.z);
UpdateMedianFilterVect3Int(median_mag, imu.mag_unscaled);
imu_krooz.hmc.data_available = FALSE;
imu_krooz.mag_valid = TRUE;
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_KROOZ_ID, get_sys_time_usec(), &imu.mag);
}
}
sw/airborne/boards/krooz/imu_krooz_memsic.h
+1 -20
View File
@@ -108,9 +108,6 @@
#endif
struct ImuKrooz {
volatile bool_t gyr_valid;
volatile bool_t acc_valid;
volatile bool_t mag_valid;
volatile bool_t mpu_eoc;
volatile bool_t hmc_eoc;
struct Mpu60x0_I2c mpu;
@@ -137,22 +134,6 @@ extern void imu_periodic(void);
extern void imu_krooz_event(void);
extern void imu_krooz_downlink_raw(void);
static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void),
void (* _mag_handler)(void) __attribute__((unused)))
{
imu_krooz_event();
if (imu_krooz.gyr_valid) {
imu_krooz.gyr_valid = FALSE;
_gyro_handler();
}
if (imu_krooz.acc_valid) {
imu_krooz.acc_valid = FALSE;
_accel_handler();
}
if (imu_krooz.mag_valid) {
imu_krooz.mag_valid = FALSE;
_mag_handler();
}
}
#define ImuEvent imu_krooz_event
#endif // IMU_KROOZ_H
sw/airborne/boards/navgo/imu_navgo.c
+8 -7
View File
@@ -34,6 +34,7 @@
#include "imu_navgo.h"
#include "mcu_periph/i2c.h"
#include "led.h"
#include "subsystems/abi.h"
// Downlink
#include "mcu_periph/uart.h"
@@ -87,10 +88,6 @@ void imu_impl_init(void)
InitMedianFilterVect3Int(median_accel);
InitMedianFilterVect3Int(median_mag);
#endif
imu_navgo.gyr_valid = FALSE;
imu_navgo.acc_valid = FALSE;
imu_navgo.mag_valid = FALSE;
}
void imu_periodic(void)
@@ -122,6 +119,7 @@ void imu_navgo_downlink_raw(void)
void imu_navgo_event(void)
{
uint32_t now_ts = get_sys_time_usec();
// If the itg3200 I2C transaction has succeeded: convert the data
itg3200_event(&imu_navgo.itg);
@@ -131,7 +129,8 @@ void imu_navgo_event(void)
UpdateMedianFilterRatesInt(median_gyro, imu.gyro_unscaled);
#endif
imu_navgo.itg.data_available = FALSE;
imu_navgo.gyr_valid = TRUE;
imu_scale_gyro(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_NAVGO_ID, now_ts, &imu.gyro);
}
// If the adxl345 I2C transaction has succeeded: convert the data
@@ -142,7 +141,8 @@ void imu_navgo_event(void)
UpdateMedianFilterVect3Int(median_accel, imu.accel_unscaled);
#endif
imu_navgo.adxl.data_available = FALSE;
imu_navgo.acc_valid = TRUE;
imu_scale_accel(&imu);
AbiSendMsgIMU_ACCEL_INT32(IMU_NAVGO_ID, now_ts, &imu.accel);
}
// HMC58XX event task
@@ -153,7 +153,8 @@ void imu_navgo_event(void)
UpdateMedianFilterVect3Int(median_mag, imu.mag_unscaled);
#endif
imu_navgo.hmc.data_available = FALSE;
imu_navgo.mag_valid = TRUE;
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_NAVGO_ID, now_ts, &imu.mag);
}
}
sw/airborne/boards/navgo/imu_navgo.h
+2 -21
View File
@@ -60,9 +60,6 @@
struct ImuNavgo {
volatile bool_t gyr_valid;
volatile bool_t acc_valid;
volatile bool_t mag_valid;
struct Itg3200 itg;
struct Adxl345_I2c adxl;
struct Hmc58xx hmc;
@@ -74,22 +71,6 @@ extern struct ImuNavgo imu_navgo;
extern void imu_navgo_event(void);
extern void imu_navgo_downlink_raw(void);
#define ImuEvent imu_navgo_event
static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void))
{
imu_navgo_event();
if (imu_navgo.gyr_valid) {
imu_navgo.gyr_valid = FALSE;
_gyro_handler();
}
if (imu_navgo.acc_valid) {
imu_navgo.acc_valid = FALSE;
_accel_handler();
}
if (imu_navgo.mag_valid) {
imu_navgo.mag_valid = FALSE;
_mag_handler();
}
}
#endif // PPZUAVIMU_H
#endif // IMU_NAVGO_H
sw/airborne/boards/umarim/imu_umarim.c
+7 -5
View File
@@ -33,6 +33,7 @@
#include "imu_umarim.h"
#include "mcu_periph/i2c.h"
#include "generated/airframe.h"
#include "subsystems/abi.h"
// Downlink
#include "mcu_periph/uart.h"
@@ -77,9 +78,6 @@ void imu_impl_init(void)
// change the default data rate
imu_umarim.adxl.config.rate = UMARIM_ACCEL_RATE;
imu_umarim.adxl.config.range = UMARIM_ACCEL_RANGE;
imu_umarim.gyr_valid = FALSE;
imu_umarim.acc_valid = FALSE;
}
void imu_periodic(void)
@@ -104,12 +102,15 @@ void imu_umarim_downlink_raw(void)
void imu_umarim_event(void)
{
uint32_t now_ts = get_sys_time_usec();
// If the itg3200 I2C transaction has succeeded: convert the data
itg3200_event(&imu_umarim.itg);
if (imu_umarim.itg.data_available) {
RATES_COPY(imu.gyro_unscaled, imu_umarim.itg.data.rates);
imu_umarim.itg.data_available = FALSE;
imu_umarim.gyr_valid = TRUE;
imu_scale_gyro(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_UMARIM_ID, now_ts, &imu.gyro);
}
// If the adxl345 I2C transaction has succeeded: convert the data
@@ -118,7 +119,8 @@ void imu_umarim_event(void)
VECT3_ASSIGN(imu.accel_unscaled, imu_umarim.adxl.data.vect.y, -imu_umarim.adxl.data.vect.x,
imu_umarim.adxl.data.vect.z);
imu_umarim.adxl.data_available = FALSE;
imu_umarim.acc_valid = TRUE;
imu_scale_accel(&imu);
AbiSendMsgIMU_ACCEL_INT32(IMU_UMARIM_ID, now_ts, &imu.accel);
}
}
sw/airborne/boards/umarim/imu_umarim.h
+1 -16
View File
@@ -106,8 +106,6 @@ extern volatile bool_t gyr_valid;
extern volatile bool_t acc_valid;
struct ImuUmarim {
volatile bool_t gyr_valid;
volatile bool_t acc_valid;
struct Itg3200 itg;
struct Adxl345_I2c adxl;
};
@@ -124,19 +122,6 @@ extern void imu_periodic(void);
extern void imu_umarim_event(void);
extern void imu_umarim_downlink_raw(void);
static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void),
void (* _mag_handler)(void) __attribute__((unused)))
{
imu_umarim_event();
if (imu_umarim.gyr_valid) {
imu_umarim.gyr_valid = FALSE;
_gyro_handler();
}
if (imu_umarim.acc_valid) {
imu_umarim.acc_valid = FALSE;
_accel_handler();
}
}
#define ImuEvent imu_umarim_event
#endif // PPZUAVIMU_H
sw/airborne/firmwares/fixedwing/main_ap.c
+1 -56
View File
@@ -135,17 +135,12 @@ PRINT_CONFIG_VAR(BARO_PERIODIC_FREQUENCY)
#if USE_IMU
#ifdef AHRS_PROPAGATE_FREQUENCY
#if (AHRS_PROPAGATE_FREQUENCY > PERIODIC_FREQUENCY)
#warning "PERIODIC_FREQUENCY should be least equal or greater than AHRS_PROPAGATE_FREQUENCY"
INFO_VALUE("it is recommended to configure in your airframe PERIODIC_FREQUENCY to at least ", AHRS_PROPAGATE_FREQUENCY)
#endif
#endif
static inline void on_gyro_event(void);
static inline void on_accel_event(void);
static inline void on_mag_event(void);
#endif // USE_IMU
#if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1
@@ -675,7 +670,7 @@ void event_task_ap(void)
#endif /* SINGLE_MCU */
#if USE_IMU
ImuEvent(on_gyro_event, on_accel_event, on_mag_event);
ImuEvent();
#endif
#ifdef InsEvent
@@ -715,53 +710,3 @@ void event_task_ap(void)
} /* event_task_ap() */
#if USE_IMU
static inline void on_accel_event(void)
{
// current timestamp
uint32_t now_ts = get_sys_time_usec();
imu_scale_accel(&imu);
AbiSendMsgIMU_ACCEL_INT32(1, now_ts, &imu.accel);
}
static inline void on_gyro_event(void)
{
// current timestamp
uint32_t now_ts = get_sys_time_usec();
imu_scale_gyro(&imu);
AbiSendMsgIMU_GYRO_INT32(1, now_ts, &imu.gyro_prev);
#if USE_AHRS_ALIGNER
if (ahrs_aligner.status != AHRS_ALIGNER_LOCKED) {
ahrs_aligner_run();
return;
}
#endif
#if defined SITL && USE_NPS
if (nps_bypass_ahrs) { sim_overwrite_ahrs(); }
#endif
#ifdef AHRS_TRIGGERED_ATTITUDE_LOOP
new_ins_attitude = 1;
#endif
}
static inline void on_mag_event(void)
{
#if USE_MAGNETOMETER
// current timestamp
uint32_t now_ts = get_sys_time_usec();
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(1, now_ts, &imu.mag);
#endif
}
#endif // USE_IMU
sw/airborne/firmwares/rotorcraft/main.c
+1 -53
View File
@@ -105,11 +105,6 @@ INFO_VALUE("it is recommended to configure in your airframe PERIODIC_FREQUENCY t
#endif
#endif
static inline void on_gyro_event(void);
static inline void on_accel_event(void);
static inline void on_mag_event(void);
tid_t main_periodic_tid; ///< id for main_periodic() timer
tid_t modules_tid; ///< id for modules_periodic_task() timer
tid_t failsafe_tid; ///< id for failsafe_check() timer
@@ -319,7 +314,7 @@ STATIC_INLINE void main_event(void)
RadioControlEvent(autopilot_on_rc_frame);
}
ImuEvent(on_gyro_event, on_accel_event, on_mag_event);
ImuEvent();
#if USE_BARO_BOARD
BaroEvent();
@@ -334,51 +329,4 @@ STATIC_INLINE void main_event(void)
#endif
modules_event_task();
}
static inline void on_accel_event( void ) {
// current timestamp
uint32_t now_ts = get_sys_time_usec();
imu_scale_accel(&imu);
AbiSendMsgIMU_ACCEL_INT32(1, now_ts, &imu.accel);
}
static inline void on_gyro_event( void ) {
// current timestamp
uint32_t now_ts = get_sys_time_usec();
imu_scale_gyro(&imu);
AbiSendMsgIMU_GYRO_INT32(1, now_ts, &imu.gyro_prev);
#if USE_AHRS_ALIGNER
if (ahrs_aligner.status != AHRS_ALIGNER_LOCKED) {
ahrs_aligner_run();
return;
}
#endif
#ifdef SITL
if (nps_bypass_ahrs) sim_overwrite_ahrs();
#endif
#ifdef USE_VEHICLE_INTERFACE
vi_notify_imu_available();
#endif
}
static inline void on_mag_event(void)
{
imu_scale_mag(&imu);
// current timestamp
uint32_t now_ts = get_sys_time_usec();
AbiSendMsgIMU_MAG_INT32(1, now_ts, &imu.mag);
#ifdef USE_VEHICLE_INTERFACE
vi_notify_mag_available();
#endif
}
sw/airborne/modules/ins/ins_xsens.c
+19
View File
@@ -415,25 +415,44 @@ void handle_ins_msg(void)
#endif
#if USE_IMU
uint32_t now_ts = get_sys_time_usec();
#ifdef XSENS_BACKWARDS
if (imu_xsens.gyro_available) {
RATES_ASSIGN(imu.gyro_unscaled, -RATE_BFP_OF_REAL(ins_p), -RATE_BFP_OF_REAL(ins_q), RATE_BFP_OF_REAL(ins_r));
imu_xsens.gyro_available = FALSE;
imu_scale_gyro(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_XSENS_ID, now_ts, &imu.gyro);
}
if (imu_xsens.accel_available) {
VECT3_ASSIGN(imu.accel_unscaled, -ACCEL_BFP_OF_REAL(ins_ax), -ACCEL_BFP_OF_REAL(ins_ay), ACCEL_BFP_OF_REAL(ins_az));
imu_xsens.accel_available = FALSE;
imu_scale_accel(&imu);
AbiSendMsgIMU_ACCEL_INT32(IMU_XSENS_ID, now_ts, &imu.accel);
}
if (imu_xsens.mag_available) {
VECT3_ASSIGN(imu.mag_unscaled, -MAG_BFP_OF_REAL(ins_mx), -MAG_BFP_OF_REAL(ins_my), MAG_BFP_OF_REAL(ins_mz));
imu_xsens.mag_available = FALSE;
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_XSENS_ID, now_ts, &imu.mag);
}
#else
if (imu_xsens.gyro_available) {
RATES_ASSIGN(imu.gyro_unscaled, RATE_BFP_OF_REAL(ins_p), RATE_BFP_OF_REAL(ins_q), RATE_BFP_OF_REAL(ins_r));
imu_xsens.gyro_available = FALSE;
imu_scale_gyro(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_XSENS_ID, now_ts, &imu.gyro);
}
if (imu_xsens.accel_available) {
VECT3_ASSIGN(imu.accel_unscaled, ACCEL_BFP_OF_REAL(ins_ax), ACCEL_BFP_OF_REAL(ins_ay), ACCEL_BFP_OF_REAL(ins_az));
imu_xsens.accel_available = FALSE;
imu_scale_accel(&imu);
AbiSendMsgIMU_ACCEL_INT32(IMU_XSENS_ID, now_ts, &imu.accel);
}
if (imu_xsens.mag_available) {
VECT3_ASSIGN(imu.mag_unscaled, MAG_BFP_OF_REAL(ins_mx), MAG_BFP_OF_REAL(ins_my), MAG_BFP_OF_REAL(ins_mz));
imu_xsens.mag_available = FALSE;
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_XSENS_ID, now_ts, &imu.mag);
}
#endif /* XSENS_BACKWARDS */
#endif /* USE_IMU */
sw/airborne/modules/ins/ins_xsens.h
+2 -14
View File
@@ -53,6 +53,7 @@ extern void xsens_periodic(void);
*/
#if USE_IMU
#include "subsystems/imu.h"
#include "subsystems/abi.h"
struct ImuXsens {
bool_t gyro_available;
@@ -61,20 +62,7 @@ struct ImuXsens {
};
extern struct ImuXsens imu_xsens;
#define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) { \
if (imu_xsens.accel_available) { \
imu_xsens.accel_available = FALSE; \
_accel_handler(); \
} \
if (imu_xsens.gyro_available) { \
imu_xsens.gyro_available = FALSE; \
_gyro_handler(); \
} \
if (imu_xsens.mag_available) { \
imu_xsens.mag_available = FALSE; \
_mag_handler(); \
} \
}
#define ImuEvent() {}
#endif /* USE_IMU */
sw/airborne/modules/sensors/imu_aspirin2.c
+6 -8
View File
@@ -22,6 +22,7 @@
#include "imu_aspirin2.h"
#include "mcu_periph/i2c.h"
#include "led.h"
#include "subsystems/abi.h"
// Downlink
#include "mcu_periph/uart.h"
@@ -34,11 +35,6 @@
#include "../../peripherals/mpu60x0.h"
// #include "../../peripherals/hmc5843.h"
// Results
volatile bool_t mag_valid;
volatile bool_t gyr_valid;
volatile bool_t acc_valid;
// Communication
struct i2c_transaction aspirin2_mpu60x0;
@@ -189,6 +185,7 @@ void aspirin2_subsystem_downlink_raw(void)
void aspirin2_subsystem_event(void)
{
int32_t x, y, z;
uint32_t now_ts = get_sys_time_usec();
// If the itg3200 I2C transaction has succeeded: convert the data
if (aspirin2_mpu60x0.status == I2CTransSuccess) {
@@ -208,9 +205,10 @@ void aspirin2_subsystem_event(void)
// Is this is new data
if (aspirin2_mpu60x0.buf[0] & 0x01) {
gyr_valid = TRUE;
acc_valid = TRUE;
} else {
imu_scale_gyro(&imu);
imu_scale_accel(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_PPZUAV_ID, now_ts, &imu.gyro);
AbiSendMsgIMU_ACCEL_INT32(IMU_PPZUAV_ID, now_ts, &imu.accel);
}
aspirin2_mpu60x0.status =
sw/airborne/modules/sensors/imu_aspirin2.h
+1 -20
View File
@@ -24,34 +24,15 @@
#include "std.h"
#include "subsystems/imu.h"
extern volatile bool_t gyr_valid;
extern volatile bool_t acc_valid;
extern volatile bool_t mag_valid;
/* must be defined in order to be IMU code: declared in imu.h
extern void imu_impl_init(void);
extern void imu_periodic(void);
*/
#define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) { \
aspirin2_subsystem_event(); \
if (gyr_valid) { \
gyr_valid = FALSE; \
_gyro_handler(); \
} \
if (acc_valid) { \
acc_valid = FALSE; \
_accel_handler(); \
} \
if (mag_valid) { \
mag_valid = FALSE; \
_mag_handler(); \
} \
}
/* Own Extra Functions */
extern void aspirin2_subsystem_event(void);
extern void aspirin2_subsystem_downlink_raw(void);
#define ImuEvent aspirin2_subsystem_event
#endif // PPZUAVIMU_H
sw/airborne/peripherals/hmc5843.h
-7
View File
@@ -71,11 +71,4 @@ extern void hmc5843_idle_task(void);
#include <string.h>
#define MagEvent(_m_handler) { \
hmc5843_idle_task(); \
if (hmc5843.data_available) { \
_m_handler(); \
} \
}
#endif /* HMC5843_H */
sw/airborne/subsystems/abi_sender_ids.h
+100
View File
@@ -158,4 +158,104 @@
#define GPS_SIM_ID 11
#endif
/*
* IDs of IMU sensors (accel, gyro)
*/
#ifndef IMU_ANALOG_ID
#define IMU_ANALOG_ID 1
#endif
#ifndef IMU_ARDRONE2_ID
#define IMU_ARDRONE2_ID 2
#endif
#ifndef IMU_ASPIRIN_ID
#define IMU_ASPIRIN_ID 3
#endif
#ifndef IMU_ASPIRIN2_ID
#define IMU_ASPIRIN2_ID 4
#endif
#ifndef IMU_B2_ID
#define IMU_B2_ID 5
#endif
#ifndef IMU_CRISTA_ID
#define IMU_CRISTA_ID 6
#endif
#ifndef IMU_DROTEK_ID
#define IMU_DROTEK_ID 7
#endif
#ifndef IMU_GL1_ID
#define IMU_GL1_ID 8
#endif
#ifndef IMU_MPU6000_ID
#define IMU_MPU6000_ID 9
#endif
#ifndef IMU_MPU6000_HMC_ID
#define IMU_MPU6000_HMC_ID 10
#endif
#ifndef IMU_MPU9250_ID
#define IMU_MPU9250_ID 11
#endif
#ifndef IMU_NAVSTIK_ID
#define IMU_NAVSTIK_ID 12
#endif
#ifndef IMU_NPS_ID
#define IMU_NPS_ID 13
#endif
#ifndef IMU_PPZUAV_ID
#define IMU_PPZUAV_ID 14
#endif
#ifndef IMU_PX4_ID
#define IMU_PX4_ID 15
#endif
#ifndef IMU_UM6_ID
#define IMU_UM6_ID 16
#endif
#ifndef IMU_NAVGO_ID
#define IMU_NAVGO_ID 17
#endif
#ifndef IMU_HBMINI_ID
#define IMU_HBMINI_ID 18
#endif
#ifndef IMU_KROOZ_ID
#define IMU_KROOZ_ID 19
#endif
#ifndef IMU_UMARIM_ID
#define IMU_UMARIM_ID 20
#endif
#ifndef IMU_GX3_ID
#define IMU_GX3_ID 21
#endif
#ifndef IMU_APOGEE_ID
#define IMU_APOGEE_ID 22
#endif
#ifndef IMU_BEBOP_ID
#define IMU_BEBOP_ID 23
#endif
#ifndef IMU_XSENS_ID
#define IMU_XSENS_ID 24
#endif
#endif /* ABI_SENDER_IDS_H */
sw/airborne/subsystems/ahrs/ahrs_aligner.c
+17
View File
@@ -44,6 +44,20 @@ static struct Int32Vect3 mag_sum;
static int32_t ref_sensor_samples[SAMPLES_NB];
static uint32_t samples_idx;
#ifndef AHRS_ALIGNER_IMU_ID
#define AHRS_ALIGNER_IMU_ID ABI_BROADCAST
#endif
static abi_event gyro_ev;
static void gyro_cb(uint8_t sender_id __attribute__((unused)),
uint32_t stamp __attribute__((unused)),
struct Int32Rates *gyro __attribute__((unused)))
{
if (ahrs_aligner.status != AHRS_ALIGNER_LOCKED) {
ahrs_aligner_run();
}
}
#if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h"
@@ -73,6 +87,9 @@ void ahrs_aligner_init(void)
ahrs_aligner.noise = 0;
ahrs_aligner.low_noise_cnt = 0;
// for now: only bind to gyro message and still read from global imu struct
AbiBindMsgIMU_GYRO_INT32(AHRS_ALIGNER_IMU_ID, &gyro_ev, gyro_cb);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "FILTER_ALIGNER", send_aligner);
#endif
sw/airborne/subsystems/ahrs/ahrs_gx3.c
+8
View File
@@ -349,3 +349,11 @@ void ahrs_gx3_register(void)
void imu_scale_gyro(struct Imu *_imu __attribute__((unused))) {}
void imu_scale_accel(struct Imu *_imu __attribute__((unused))) {}
void imu_scale_mag(struct Imu *_imu __attribute__((unused))) {}
void ahrs_gx3_publish_imu(void)
{
uint32_t now_ts = get_sys_time_usec();
AbiSendMsgIMU_GYRO_INT32(IMU_GX3_ID, now_ts, &imu.gyro);
AbiSendMsgIMU_ACCEL_INT32(IMU_GX3_ID, now_ts, &imu.accel);
AbiSendMsgIMU_MAG_INT32(IMU_GX3_ID, now_ts, &imu.mag);
}
sw/airborne/subsystems/ahrs/ahrs_gx3.h
+3 -4
View File
@@ -91,6 +91,7 @@ extern struct AhrsGX3 ahrs_gx3;
extern void ahrs_gx3_init(void);
extern void ahrs_gx3_align(void);
extern void ahrs_gx3_register(void);
extern void ahrs_gx3_publish_imu(void);
static inline void ReadGX3Buffer(void)
{
@@ -99,16 +100,14 @@ static inline void ReadGX3Buffer(void)
}
}
static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void))
static inline void ImuEvent(void)
{
if (uart_char_available(&GX3_PORT)) {
ReadGX3Buffer();
}
if (ahrs_gx3.packet.msg_available) {
gx3_packet_read_message();
_gyro_handler();
_accel_handler();
_mag_handler();
ahrs_gx3_publish_imu();
ahrs_gx3.packet.msg_available = FALSE;
}
}
sw/airborne/subsystems/imu/imu_analog.c
+7 -3
View File
@@ -21,8 +21,8 @@
#include "imu_analog.h"
#include "mcu_periph/adc.h"
#include "subsystems/abi.h"
volatile bool_t analog_imu_available;
int imu_overrun;
static struct adc_buf analog_imu_adc_buf[NB_ANALOG_IMU_ADC];
@@ -30,7 +30,6 @@ static struct adc_buf analog_imu_adc_buf[NB_ANALOG_IMU_ADC];
void imu_impl_init(void)
{
analog_imu_available = FALSE;
imu_overrun = 0;
#ifdef ADC_CHANNEL_GYRO_P
@@ -59,6 +58,8 @@ void imu_periodic(void)
// Actual Nr of ADC measurements per channel per periodic loop
static int last_head = 0;
uint32_t now_ts = get_sys_time_usec();
imu_overrun = analog_imu_adc_buf[0].head - last_head;
if (imu_overrun < 0) {
imu_overrun += ADC_CHANNEL_GYRO_NB_SAMPLES;
@@ -85,7 +86,10 @@ void imu_periodic(void)
imu.accel_unscaled.z = analog_imu_adc_buf[5].sum / ADC_CHANNEL_ACCEL_NB_SAMPLES;
#endif
analog_imu_available = TRUE;
imu_scale_gyro(&imu);
imu_scale_accel(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_ANALOG_ID, now_ts, &imu.gyro);
AbiSendMsgIMU_ACCEL_INT32(IMU_ANALOG_ID, now_ts, &imu.accel);
}
// if not all gyros are used, override the imu_scale_gyro handler
sw/airborne/subsystems/imu/imu_analog.h
+1 -11
View File
@@ -69,18 +69,8 @@
#include "subsystems/imu.h"
extern volatile bool_t analog_imu_available;
extern int imu_overrun;
static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void),
void (* _mag_handler)(void) __attribute__((unused)))
{
if (analog_imu_available) {
analog_imu_available = FALSE;
_gyro_handler();
_accel_handler();
}
}
#define ImuEvent() {}
#endif /* IMU_ANALOG_H */
sw/airborne/subsystems/imu/imu_ardrone2_raw.h
+1 -19
View File
@@ -84,25 +84,7 @@
*/
#include "subsystems/imu.h"
static inline void imu_ardrone2_event(void (* _gyro_handler)(void), void (* _accel_handler)(void),
void (* _mag_handler)(void))
{
navdata_update();
//checks if the navboard has a new dataset ready
if (navdata.imu_available == TRUE) {
navdata.imu_available = FALSE;
RATES_ASSIGN(imu.gyro_unscaled, navdata.measure.vx, -navdata.measure.vy, -navdata.measure.vz);
VECT3_ASSIGN(imu.accel_unscaled, navdata.measure.ax, 4096 - navdata.measure.ay, 4096 - navdata.measure.az);
VECT3_ASSIGN(imu.mag_unscaled, -navdata.measure.mx, -navdata.measure.my, -navdata.measure.mz);
_gyro_handler();
_accel_handler();
_mag_handler();
}
}
#define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) { \
imu_ardrone2_event(_gyro_handler, _accel_handler, _mag_handler); \
}
#define ImuEvent navdata_update
#endif /* IMU_ARDRONE2_RAW_H_ */
sw/airborne/subsystems/imu/imu_ardrone2_sdk.c
+1 -1
View File
@@ -21,7 +21,7 @@
/**
* @file subsystems/imu/imu_ardrone2_sdk.c
* IMU implementation for ardrone2-sdk.
* dummy IMU implementation for ardrone2-sdk.
*/
#include "subsystems/imu.h"
sw/airborne/subsystems/imu/imu_ardrone2_sdk.h
+3 -9
View File
@@ -21,20 +21,14 @@
/**
* @file subsystems/imu/imu_ardrone2_sdk.h
* IMU implementation for ardrone2-sdk.
* dummy IMU implementation for ardrone2-sdk.
*/
#ifndef IMU_ARDRONE2_SDK_H_
#define IMU_ARDRONE2_SDK_H_
#include "subsystems/imu.h"
#include "generated/airframe.h"
static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void))
{
_gyro_handler();
_accel_handler();
_mag_handler();
}
#define ImuEvent() {}
#endif /* IMU_ARDRONE2_SDK_H_ */
sw/airborne/subsystems/imu/imu_aspirin.c
+19 -1
View File
@@ -26,7 +26,7 @@
*/
#include "subsystems/imu.h"
#include "subsystems/abi.h"
#include "mcu_periph/i2c.h"
#include "mcu_periph/spi.h"
@@ -117,6 +117,8 @@ void imu_periodic(void)
void imu_aspirin_event(void)
{
uint32_t now_ts = get_sys_time_usec();
adxl345_spi_event(&imu_aspirin.acc_adxl);
if (imu_aspirin.acc_adxl.data_available) {
VECT3_COPY(imu.accel_unscaled, imu_aspirin.acc_adxl.data.vect);
@@ -145,4 +147,20 @@ void imu_aspirin_event(void)
imu_aspirin.mag_hmc.data_available = FALSE;
imu_aspirin.mag_valid = TRUE;
}
if (imu_aspirin.gyro_valid) {
imu_aspirin.gyro_valid = FALSE;
imu_scale_gyro(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_ASPIRIN_ID, now_ts, &imu.gyro);
}
if (imu_aspirin.accel_valid) {
imu_aspirin.accel_valid = FALSE;
imu_scale_accel(&imu);
AbiSendMsgIMU_ACCEL_INT32(IMU_ASPIRIN_ID, now_ts, &imu.accel);
}
if (imu_aspirin.mag_valid) {
imu_aspirin.mag_valid = FALSE;
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_ASPIRIN_ID, now_ts, &imu.mag);
}
}
sw/airborne/subsystems/imu/imu_aspirin.h
+1 -17
View File
@@ -59,22 +59,6 @@ extern void imu_aspirin_event(void);
extern void imu_aspirin_arch_init(void);
#endif
static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void))
{
imu_aspirin_event();
if (imu_aspirin.gyro_valid) {
imu_aspirin.gyro_valid = FALSE;
_gyro_handler();
}
if (imu_aspirin.accel_valid) {
imu_aspirin.accel_valid = FALSE;
_accel_handler();
}
if (imu_aspirin.mag_valid) {
imu_aspirin.mag_valid = FALSE;
_mag_handler();
}
}
#define ImuEvent imu_aspirin_event
#endif /* IMU_ASPIRIN_H */
sw/airborne/subsystems/imu/imu_aspirin_2_spi.c
+10 -7
View File
@@ -25,6 +25,7 @@
*/
#include "subsystems/imu.h"
#include "subsystems/abi.h"
#include "mcu_periph/sys_time.h"
#include "mcu_periph/spi.h"
#include "peripherals/hmc58xx_regs.h"
@@ -104,10 +105,6 @@ bool_t imu_aspirin2_configure_mag_slave(Mpu60x0ConfigSet mpu_set, void *mpu);
void imu_impl_init(void)
{
imu_aspirin2.accel_valid = FALSE;
imu_aspirin2.gyro_valid = FALSE;
imu_aspirin2.mag_valid = FALSE;
mpu60x0_spi_init(&imu_aspirin2.mpu, &(ASPIRIN_2_SPI_DEV), ASPIRIN_2_SPI_SLAVE_IDX);
// change the default configuration
imu_aspirin2.mpu.config.smplrt_div = ASPIRIN_2_SMPLRT_DIV;
@@ -161,6 +158,8 @@ void imu_periodic(void)
void imu_aspirin2_event(void)
{
uint32_t now_ts = get_sys_time_usec();
mpu60x0_spi_event(&imu_aspirin2.mpu);
if (imu_aspirin2.mpu.data_available) {
/* HMC5883 has xzy order of axes in returned data */
@@ -223,9 +222,13 @@ void imu_aspirin2_event(void)
#endif
imu_aspirin2.mpu.data_available = FALSE;
imu_aspirin2.gyro_valid = TRUE;
imu_aspirin2.accel_valid = TRUE;
imu_aspirin2.mag_valid = TRUE;
imu_scale_gyro(&imu);
imu_scale_accel(&imu);
imu_scale_mag(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_ASPIRIN2_ID, now_ts, &imu.gyro);
AbiSendMsgIMU_ACCEL_INT32(IMU_ASPIRIN2_ID, now_ts, &imu.accel);
AbiSendMsgIMU_MAG_INT32(IMU_ASPIRIN2_ID, now_ts, &imu.mag);
}
}
sw/airborne/subsystems/imu/imu_aspirin_2_spi.h
+1 -20
View File
@@ -36,9 +36,6 @@
#include "peripherals/mpu60x0_spi.h"
struct ImuAspirin2Spi {
volatile bool_t gyro_valid;
volatile bool_t accel_valid;
volatile bool_t mag_valid;
struct Mpu60x0_Spi mpu;
struct spi_transaction wait_slave4_trans;
@@ -51,22 +48,6 @@ extern struct ImuAspirin2Spi imu_aspirin2;
extern void imu_aspirin2_event(void);
static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void))
{
imu_aspirin2_event();
if (imu_aspirin2.gyro_valid) {
imu_aspirin2.gyro_valid = FALSE;
_gyro_handler();
}
if (imu_aspirin2.accel_valid) {
imu_aspirin2.accel_valid = FALSE;
_accel_handler();
}
if (imu_aspirin2.mag_valid) {
imu_aspirin2.mag_valid = FALSE;
_mag_handler();
}
}
#define ImuEvent imu_aspirin2_event
#endif /* IMU_ASPIRIN_2_H */
sw/airborne/subsystems/imu/imu_aspirin_i2c.c
+9 -8
View File
@@ -26,7 +26,7 @@
*/
#include "subsystems/imu.h"
#include "subsystems/abi.h"
#include "mcu_periph/i2c.h"
// Set SPI_CS High to enable I2C mode of ADXL345
@@ -82,10 +82,6 @@ struct ImuAspirinI2c imu_aspirin;
void imu_impl_init(void)
{
imu_aspirin.accel_valid = FALSE;
imu_aspirin.gyro_valid = FALSE;
imu_aspirin.mag_valid = FALSE;
/* Set accel configuration */
adxl345_i2c_init(&imu_aspirin.acc_adxl, &(ASPIRIN_I2C_DEV), ADXL345_ADDR);
// set the data rate
@@ -132,11 +128,14 @@ void imu_periodic(void)
void imu_aspirin_i2c_event(void)
{
uint32_t now_ts = get_sys_time_usec();
adxl345_i2c_event(&imu_aspirin.acc_adxl);
if (imu_aspirin.acc_adxl.data_available) {
VECT3_COPY(imu.accel_unscaled, imu_aspirin.acc_adxl.data.vect);
imu_aspirin.acc_adxl.data_available = FALSE;
imu_aspirin.accel_valid = TRUE;
imu_scale_accel(&imu);
AbiSendMsgIMU_ACCEL_INT32(IMU_ASPIRIN_ID, now_ts, &imu.accel);
}
/* If the itg3200 I2C transaction has succeeded: convert the data */
@@ -144,7 +143,8 @@ void imu_aspirin_i2c_event(void)
if (imu_aspirin.gyro_itg.data_available) {
RATES_COPY(imu.gyro_unscaled, imu_aspirin.gyro_itg.data.rates);
imu_aspirin.gyro_itg.data_available = FALSE;
imu_aspirin.gyro_valid = TRUE;
imu_scale_gyro(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_ASPIRIN_ID, now_ts, &imu.gyro);
}
/* HMC58XX event task */
@@ -158,6 +158,7 @@ void imu_aspirin_i2c_event(void)
imu.mag_unscaled.z = imu_aspirin.mag_hmc.data.vect.z;
#endif
imu_aspirin.mag_hmc.data_available = FALSE;
imu_aspirin.mag_valid = TRUE;
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_ASPIRIN_ID, now_ts, &imu.mag);
}
}
sw/airborne/subsystems/imu/imu_aspirin_i2c.h
+1 -19
View File
@@ -41,9 +41,6 @@
struct ImuAspirinI2c {
volatile uint8_t accel_valid;
volatile uint8_t gyro_valid;
volatile uint8_t mag_valid;
struct Adxl345_I2c acc_adxl;
struct Itg3200 gyro_itg;
struct Hmc58xx mag_hmc;
@@ -53,21 +50,6 @@ extern struct ImuAspirinI2c imu_aspirin;
extern void imu_aspirin_i2c_event(void);
static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void))
{
imu_aspirin_i2c_event();
if (imu_aspirin.gyro_valid) {
imu_aspirin.gyro_valid = FALSE;
_gyro_handler();
}
if (imu_aspirin.accel_valid) {
imu_aspirin.accel_valid = FALSE;
_accel_handler();
}
if (imu_aspirin.mag_valid) {
imu_aspirin.mag_valid = FALSE;
_mag_handler();
}
}
#define ImuEvent imu_aspirin_i2c_event
#endif /* IMU_ASPIRIN_I2C_H */
sw/airborne/subsystems/imu/imu_b2.c
+86
View File
@@ -80,3 +80,89 @@ void imu_scale_mag(struct Imu *_imu)
#elif defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_NONE
void imu_scale_mag(struct Imu *_imu __attribute__((unused))) {}
#endif
/** Event functions for imu_b2.
*/
#include "subsystems/abi.h"
#include "mcu_periph/sys_time.h"
#if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2100
static inline void ImuMagEvent(void)
{
ms2100_event(&ms2100);
if (ms2100.status == MS2100_DATA_AVAILABLE) {
imu.mag_unscaled.x = ms2100.data.value[IMU_MAG_X_CHAN];
imu.mag_unscaled.y = ms2100.data.value[IMU_MAG_Y_CHAN];
imu.mag_unscaled.z = ms2100.data.value[IMU_MAG_Z_CHAN];
ms2100.status = MS2100_IDLE;
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_B2_ID, get_sys_time_usec(), &imu.mag);
}
}
#elif defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_AMI601
static inline void foo_handler(void) {}
static inline void ImuMagEvent(void)
{
AMI601Event(foo_handler);
if (ami601_status == AMI601_DATA_AVAILABLE) {
imu.mag_unscaled.x = ami601_values[IMU_MAG_X_CHAN];
imu.mag_unscaled.y = ami601_values[IMU_MAG_Y_CHAN];
imu.mag_unscaled.z = ami601_values[IMU_MAG_Z_CHAN];
ami601_status = AMI601_IDLE;
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_B2_ID, get_sys_time_usec(), &imu.mag);
}
}
#elif defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_HMC5843
static inline void foo_handler(void) {}
static inline void ImuMagEvent(void)
{
hmc5843_idle_task();
if (hmc5843.data_available) {
imu.mag_unscaled.x = hmc5843.data.value[IMU_MAG_X_CHAN];
imu.mag_unscaled.y = hmc5843.data.value[IMU_MAG_Y_CHAN];
imu.mag_unscaled.z = hmc5843.data.value[IMU_MAG_Z_CHAN];
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_B2_ID, get_sys_time_usec(), &imu.mag);
hmc5843.data_available = FALSE;
}
}
#elif defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_HMC58XX
static inline void ImuMagEvent(void)
{
hmc58xx_event(&imu_b2.mag_hmc);
if (imu_b2.mag_hmc.data_available) {
imu.mag_unscaled.x = imu_b2.mag_hmc.data.value[IMU_MAG_X_CHAN];
imu.mag_unscaled.y = imu_b2.mag_hmc.data.value[IMU_MAG_Y_CHAN];
imu.mag_unscaled.z = imu_b2.mag_hmc.data.value[IMU_MAG_Z_CHAN];
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_B2_ID, get_sys_time_usec(), &imu.mag);
imu_b2.mag_hmc.data_available = FALSE;
}
}
#else
#define ImuMagEvent() {}
#endif
void imu_b2_event(void)
{
max1168_event();
if (max1168_status == MAX1168_DATA_AVAILABLE) {
uint32_t now_ts = get_sys_time_usec();
imu.gyro_unscaled.p = max1168_values[IMU_GYRO_P_CHAN];
imu.gyro_unscaled.q = max1168_values[IMU_GYRO_Q_CHAN];
imu.gyro_unscaled.r = max1168_values[IMU_GYRO_R_CHAN];
imu.accel_unscaled.x = max1168_values[IMU_ACCEL_X_CHAN];
imu.accel_unscaled.y = max1168_values[IMU_ACCEL_Y_CHAN];
imu.accel_unscaled.z = max1168_values[IMU_ACCEL_Z_CHAN];
max1168_status = MAX1168_IDLE;
imu_scale_gyro(&imu);
imu_scale_accel(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_ASPIRIN2_ID, now_ts, &imu.gyro);
AbiSendMsgIMU_ACCEL_INT32(IMU_ASPIRIN2_ID, now_ts, &imu.accel);
}
ImuMagEvent();
}
sw/airborne/subsystems/imu/imu_b2.h
+11 -79
View File
@@ -41,8 +41,16 @@
#define IMU_B2_MAG_HMC5843 3
#define IMU_B2_MAG_HMC58XX 4
#if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_HMC58XX
#if defined IMU_B2_MAG_TYPE
#if IMU_B2_MAG_TYPE == IMU_B2_MAG_HMC58XX
#include "peripherals/hmc58xx.h"
#elif IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2100
#include "peripherals/ms2100.h"
#elif IMU_B2_MAG_TYPE == IMU_B2_MAG_AMI601
#include "peripherals/ami601.h"
#elif IMU_B2_MAG_TYPE == IMU_B2_MAG_HMC5843
#include "peripherals/hmc5843.h"
#endif
#endif
#ifdef IMU_B2_VERSION_1_0
@@ -164,84 +172,8 @@ struct ImuBooz2 {
};
extern struct ImuBooz2 imu_b2;
extern void imu_b2_event(void);
/** Event functions and macros for imu_b2.
*/
#if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2100
#include "peripherals/ms2100.h"
static inline void ImuMagEvent(void (* _mag_handler)(void))
{
ms2100_event(&ms2100);
if (ms2100.status == MS2100_DATA_AVAILABLE) {
imu.mag_unscaled.x = ms2100.data.value[IMU_MAG_X_CHAN];
imu.mag_unscaled.y = ms2100.data.value[IMU_MAG_Y_CHAN];
imu.mag_unscaled.z = ms2100.data.value[IMU_MAG_Z_CHAN];
ms2100.status = MS2100_IDLE;
_mag_handler();
}
}
#elif defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_AMI601
#include "peripherals/ami601.h"
static inline void foo_handler(void) {}
static inline void ImuMagEvent(void (* _mag_handler)(void))
{
AMI601Event(foo_handler);
if (ami601_status == AMI601_DATA_AVAILABLE) {
imu.mag_unscaled.x = ami601_values[IMU_MAG_X_CHAN];
imu.mag_unscaled.y = ami601_values[IMU_MAG_Y_CHAN];
imu.mag_unscaled.z = ami601_values[IMU_MAG_Z_CHAN];
ami601_status = AMI601_IDLE;
_mag_handler();
}
}
#elif defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_HMC5843
#include "peripherals/hmc5843.h"
static inline void foo_handler(void) {}
static inline void ImuMagEvent(void (* _mag_handler)(void))
{
MagEvent(foo_handler);
if (hmc5843.data_available) {
imu.mag_unscaled.x = hmc5843.data.value[IMU_MAG_X_CHAN];
imu.mag_unscaled.y = hmc5843.data.value[IMU_MAG_Y_CHAN];
imu.mag_unscaled.z = hmc5843.data.value[IMU_MAG_Z_CHAN];
_mag_handler();
hmc5843.data_available = FALSE;
}
}
#elif defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_HMC58XX
static inline void ImuMagEvent(void (* _mag_handler)(void))
{
hmc58xx_event(&imu_b2.mag_hmc);
if (imu_b2.mag_hmc.data_available) {
imu.mag_unscaled.x = imu_b2.mag_hmc.data.value[IMU_MAG_X_CHAN];
imu.mag_unscaled.y = imu_b2.mag_hmc.data.value[IMU_MAG_Y_CHAN];
imu.mag_unscaled.z = imu_b2.mag_hmc.data.value[IMU_MAG_Z_CHAN];
_mag_handler();
imu_b2.mag_hmc.data_available = FALSE;
}
}
#else
#define ImuMagEvent(_mag_handler) {}
#endif
static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void),
void (* _mag_handler)(void))
{
max1168_event();
if (max1168_status == MAX1168_DATA_AVAILABLE) {
imu.gyro_unscaled.p = max1168_values[IMU_GYRO_P_CHAN];
imu.gyro_unscaled.q = max1168_values[IMU_GYRO_Q_CHAN];
imu.gyro_unscaled.r = max1168_values[IMU_GYRO_R_CHAN];
imu.accel_unscaled.x = max1168_values[IMU_ACCEL_X_CHAN];
imu.accel_unscaled.y = max1168_values[IMU_ACCEL_Y_CHAN];
imu.accel_unscaled.z = max1168_values[IMU_ACCEL_Z_CHAN];
max1168_status = MAX1168_IDLE;
_gyro_handler();
_accel_handler();
}
ImuMagEvent(_mag_handler);
}
#define ImuEvent imu_b2_event
#endif /* IMU_B2_H */
sw/airborne/subsystems/imu/imu_bebop.c
+9 -7
View File
@@ -25,6 +25,7 @@
*/
#include "subsystems/imu.h"
#include "subsystems/abi.h"
#include "mcu_periph/i2c.h"
@@ -77,10 +78,6 @@ struct ImuBebop imu_bebop;
*/
void imu_impl_init(void)
{
imu_bebop.accel_valid = FALSE;
imu_bebop.gyro_valid = FALSE;
imu_bebop.mag_valid = FALSE;
/* MPU-60X0 */
mpu60x0_i2c_init(&imu_bebop.mpu, &(BEBOP_MPU_I2C_DEV), MPU60X0_ADDR >> 1);
imu_bebop.mpu.config.smplrt_div = BEBOP_SMPLRT_DIV;
@@ -111,6 +108,8 @@ void imu_periodic(void)
*/
void imu_bebop_event(void)
{
uint32_t now_ts = get_sys_time_usec();
/* MPU-60x0 event taks */
mpu60x0_i2c_event(&imu_bebop.mpu);
@@ -122,8 +121,10 @@ void imu_bebop_event(void)
-imu_bebop.mpu.data_accel.vect.z);
imu_bebop.mpu.data_available = FALSE;
imu_bebop.gyro_valid = TRUE;
imu_bebop.accel_valid = TRUE;
imu_scale_gyro(&imu);
imu_scale_accel(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_BEBOP_ID, now_ts, &imu.gyro);
AbiSendMsgIMU_ACCEL_INT32(IMU_BEBOP_ID, now_ts, &imu.accel);
}
/* AKM8963 event task */
@@ -134,6 +135,7 @@ void imu_bebop_event(void)
VECT3_ASSIGN(imu.mag_unscaled, -imu_bebop.ak.data.vect.y, imu_bebop.ak.data.vect.x, imu_bebop.ak.data.vect.z);
imu_bebop.ak.data_available = FALSE;
imu_bebop.mag_valid = TRUE;
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_BEBOP_ID, now_ts, &imu.mag);
}
}
sw/airborne/subsystems/imu/imu_bebop.h
+1 -19
View File
@@ -71,9 +71,6 @@
/** Everything that is in the bebop IMU */
struct ImuBebop {
volatile uint8_t accel_valid; ///< Shows when the accelerometer is valid
volatile uint8_t gyro_valid; ///< Shows when the gyrometer is valid
volatile uint8_t mag_valid; ///< Shows when the Magneto is valid
struct Mpu60x0_I2c mpu; ///< The MPU gyro/accel device
struct Ak8963 ak; ///< The AK8963 mag
};
@@ -81,21 +78,6 @@ struct ImuBebop {
extern struct ImuBebop imu_bebop;
extern void imu_bebop_event(void);
static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void))
{
imu_bebop_event();
if (imu_bebop.gyro_valid) {
imu_bebop.gyro_valid = FALSE;
_gyro_handler();
}
if (imu_bebop.accel_valid) {
imu_bebop.accel_valid = FALSE;
_accel_handler();
}
if (imu_bebop.mag_valid) {
imu_bebop.mag_valid = FALSE;
_mag_handler();
}
}
#define ImuEvent imu_bebop_event
#endif /* IMU_BEBOP_H */
sw/airborne/subsystems/imu/imu_crista.c
+55
View File
@@ -20,6 +20,7 @@
*/
#include "subsystems/imu.h"
#include "subsystems/abi.h"
volatile bool_t ADS8344_available;
uint16_t ADS8344_values[ADS8344_NB_CHANNELS];
@@ -51,3 +52,57 @@ void imu_periodic(void)
hmc5843_periodic();
#endif
}
#ifdef USE_AMI601
#include "peripherals/ami601.h"
#define foo_handler() {}
static inline void ImuMagEvent()
{
AMI601Event(foo_handler);
if (ami601_status == AMI601_DATA_AVAILABLE) {
imu.mag_unscaled.x = ami601_values[IMU_MAG_X_CHAN];
imu.mag_unscaled.y = ami601_values[IMU_MAG_Y_CHAN];
imu.mag_unscaled.z = ami601_values[IMU_MAG_Z_CHAN];
ami601_status = AMI601_IDLE;
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_CRISTA_ID, now_ts, &imu.mag);
}
}
#elif defined USE_HMC5843
#include "peripherals/hmc5843.h"
static inline void ImuMagEvent(void)
{
hmc5843_idle_task();
if (hmc5843.data_available) {
imu.mag_unscaled.x = hmc5843.data.value[IMU_MAG_X_CHAN];
imu.mag_unscaled.y = hmc5843.data.value[IMU_MAG_Y_CHAN];
imu.mag_unscaled.z = hmc5843.data.value[IMU_MAG_Z_CHAN];
hmc5843.data_available = FALSE;
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_CRISTA_ID, now_ts, &imu.mag);
}
}
#else
#define ImuMagEvent() {}
#endif
void imu_christa_event(void)
{
if (ADS8344_available) {
ADS8344_available = FALSE;
imu.gyro_unscaled.p = ADS8344_values[IMU_GYRO_P_CHAN];
imu.gyro_unscaled.q = ADS8344_values[IMU_GYRO_Q_CHAN];
imu.gyro_unscaled.r = ADS8344_values[IMU_GYRO_R_CHAN];
imu.accel_unscaled.x = ADS8344_values[IMU_ACCEL_X_CHAN];
imu.accel_unscaled.y = ADS8344_values[IMU_ACCEL_Y_CHAN];
imu.accel_unscaled.z = ADS8344_values[IMU_ACCEL_Z_CHAN];
/* spare 3, temp 7 */
imu_scale_gyro(&imu);
imu_scale_accel(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_CRISTA_ID, now_ts, &imu.gyro);
AbiSendMsgIMU_ACCEL_INT32(IMU_CRISTA_ID, now_ts, &imu.accel);
}
ImuMagEvent();
}
sw/airborne/subsystems/imu/imu_crista.h
+2 -46
View File
@@ -29,57 +29,13 @@
extern uint16_t ADS8344_values[ADS8344_NB_CHANNELS];
extern volatile bool_t ADS8344_available;
#define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) { \
if (ADS8344_available) { \
ADS8344_available = FALSE; \
imu.gyro_unscaled.p = ADS8344_values[IMU_GYRO_P_CHAN]; \
imu.gyro_unscaled.q = ADS8344_values[IMU_GYRO_Q_CHAN]; \
imu.gyro_unscaled.r = ADS8344_values[IMU_GYRO_R_CHAN]; \
imu.accel_unscaled.x = ADS8344_values[IMU_ACCEL_X_CHAN]; \
imu.accel_unscaled.y = ADS8344_values[IMU_ACCEL_Y_CHAN]; \
imu.accel_unscaled.z = ADS8344_values[IMU_ACCEL_Z_CHAN]; \
/* spare 3, temp 7 */ \
_gyro_handler(); \
_accel_handler(); \
} \
ImuMagEvent(_mag_handler); \
}
#ifdef USE_AMI601
#include "peripherals/ami601.h"
#define foo_handler() {}
#define ImuMagEvent(_mag_handler) { \
AMI601Event(foo_handler); \
if (ami601_status == AMI601_DATA_AVAILABLE) { \
imu.mag_unscaled.x = ami601_values[IMU_MAG_X_CHAN]; \
imu.mag_unscaled.y = ami601_values[IMU_MAG_Y_CHAN]; \
imu.mag_unscaled.z = ami601_values[IMU_MAG_Z_CHAN]; \
ami601_status = AMI601_IDLE; \
_mag_handler(); \
} \
}
#elif defined USE_HMC5843
#include "peripherals/hmc5843.h"
#define foo_handler() {}
#define ImuMagEvent(_mag_handler) { \
MagEvent(foo_handler); \
if (hmc5843.data_available) { \
imu.mag_unscaled.x = hmc5843.data.value[IMU_MAG_X_CHAN]; \
imu.mag_unscaled.y = hmc5843.data.value[IMU_MAG_Y_CHAN]; \
imu.mag_unscaled.z = hmc5843.data.value[IMU_MAG_Z_CHAN]; \
_mag_handler(); \
hmc5843.data_available = FALSE; \
} \
}
#else
#define ImuMagEvent(_mag_handler) {}
#endif
/* underlying architecture */
#include "subsystems/imu/imu_crista_arch.h"
/* must be defined by underlying architecture */
extern void imu_crista_arch_init(void);
extern void imu_christa_event(void);
#define ImuEvent imu_christa_event
#endif /* IMU_CRISTA_H */
sw/airborne/subsystems/imu/imu_drotek_10dof_v2.c
+9 -8
View File
@@ -33,7 +33,7 @@
*/
#include "subsystems/imu.h"
#include "subsystems/abi.h"
#include "mcu_periph/i2c.h"
#if !defined DROTEK_2_LOWPASS_FILTER && !defined DROTEK_2_SMPLRT_DIV
@@ -101,10 +101,6 @@ void imu_impl_init(void)
// use hmc mag via passthrough
imu_drotek2.mpu.config.i2c_bypass = TRUE;
imu_drotek2.gyro_valid = FALSE;
imu_drotek2.accel_valid = FALSE;
imu_drotek2.mag_valid = FALSE;
}
void imu_periodic(void)
@@ -120,6 +116,8 @@ void imu_periodic(void)
void imu_drotek2_event(void)
{
uint32_t now_ts = get_sys_time_usec();
// If the MPU6050 I2C transaction has succeeded: convert the data
mpu60x0_i2c_event(&imu_drotek2.mpu);
@@ -139,8 +137,10 @@ void imu_drotek2_event(void)
#endif
imu_drotek2.mpu.data_available = FALSE;
imu_drotek2.gyro_valid = TRUE;
imu_drotek2.accel_valid = TRUE;
imu_scale_gyro(&imu);
imu_scale_accel(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_DROTEK_ID, now_ts, &imu.gyro);
AbiSendMsgIMU_ACCEL_INT32(IMU_DROTEK_ID, now_ts, &imu.accel);
}
/* HMC58XX event task */
@@ -154,7 +154,8 @@ void imu_drotek2_event(void)
VECT3_COPY(imu.mag_unscaled, imu_drotek2.hmc.data.vect);
#endif
imu_drotek2.hmc.data_available = FALSE;
imu_drotek2.mag_valid = TRUE;
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_DROTEK_ID, now_ts, &imu.mag);
}
}
sw/airborne/subsystems/imu/imu_drotek_10dof_v2.h
+1 -20
View File
@@ -74,9 +74,6 @@
struct ImuDrotek2 {
volatile bool_t gyro_valid;
volatile bool_t accel_valid;
volatile bool_t mag_valid;
struct Mpu60x0_I2c mpu;
struct Hmc58xx hmc;
};
@@ -86,22 +83,6 @@ extern struct ImuDrotek2 imu_drotek2;
extern void imu_drotek2_event(void);
extern bool_t imu_drotek2_configure_mag_slave(Mpu60x0ConfigSet mpu_set, void *mpu);
static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void))
{
imu_drotek2_event();
if (imu_drotek2.gyro_valid) {
imu_drotek2.gyro_valid = FALSE;
_gyro_handler();
}
if (imu_drotek2.accel_valid) {
imu_drotek2.accel_valid = FALSE;
_accel_handler();
}
if (imu_drotek2.mag_valid) {
imu_drotek2.mag_valid = FALSE;
_mag_handler();
}
}
#define ImuEvent imu_drotek2_event
#endif /* IMU_DROTEK_10DOF_V2_H */
sw/airborne/subsystems/imu/imu_gl1.c
+9 -7
View File
@@ -27,6 +27,7 @@
*/
#include "subsystems/imu.h"
#include "subsystems/abi.h"
#include "mcu_periph/i2c.h"
PRINT_CONFIG_VAR(GL1_I2C_DEV)
@@ -78,10 +79,6 @@ struct ImuGL1I2c imu_gl1;
void imu_impl_init(void)
{
imu_gl1.accel_valid = FALSE;
imu_gl1.gyro_valid = FALSE;
imu_gl1.mag_valid = FALSE;
/* Set accel configuration */
adxl345_i2c_init(&imu_gl1.acc_adxl, &(GL1_I2C_DEV), ADXL345_ADDR);
// set the data rate
@@ -120,6 +117,8 @@ void imu_periodic(void)
void imu_gl1_i2c_event(void)
{
uint32_t now_ts = get_sys_time_usec();
adxl345_i2c_event(&imu_gl1.acc_adxl);
if (imu_gl1.acc_adxl.data_available) {
VECT3_COPY(imu.accel_unscaled, imu_gl1.acc_adxl.data.vect);
@@ -127,7 +126,8 @@ void imu_gl1_i2c_event(void)
imu.accel_unscaled.y = imu_gl1.acc_adxl.data.vect.x;
imu.accel_unscaled.z = -imu_gl1.acc_adxl.data.vect.z;
imu_gl1.acc_adxl.data_available = FALSE;
imu_gl1.accel_valid = TRUE;
imu_scale_accel(&imu);
AbiSendMsgIMU_ACCEL_INT32(IMU_GL1_ID, now_ts, &imu.accel);
}
/* If the lg34200 I2C transaction has succeeded: convert the data */
@@ -138,7 +138,8 @@ void imu_gl1_i2c_event(void)
imu.gyro_unscaled.q = imu_gl1.gyro_l3g.data.rates.p;
imu.gyro_unscaled.r = -imu_gl1.gyro_l3g.data.rates.r;
imu_gl1.gyro_l3g.data_available = FALSE;
imu_gl1.gyro_valid = TRUE;
imu_scale_gyro(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_GL1_ID, now_ts, &imu.gyro);
}
/* HMC58XX event task */
@@ -149,6 +150,7 @@ void imu_gl1_i2c_event(void)
imu.mag_unscaled.x = imu_gl1.mag_hmc.data.vect.y;
imu.mag_unscaled.z = -imu_gl1.mag_hmc.data.vect.z;
imu_gl1.mag_hmc.data_available = FALSE;
imu_gl1.mag_valid = TRUE;
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_GL1_ID, now_ts, &imu.mag);
}
}
sw/airborne/subsystems/imu/imu_gl1.h
+1 -19
View File
@@ -41,9 +41,6 @@
#include "peripherals/adxl345_i2c.h"
struct ImuGL1I2c {
volatile uint8_t accel_valid;
volatile uint8_t gyro_valid;
volatile uint8_t mag_valid;
struct Adxl345_I2c acc_adxl;
struct L3g4200 gyro_l3g;
struct Hmc58xx mag_hmc;
@@ -53,21 +50,6 @@ extern struct ImuGL1I2c imu_gl1;
extern void imu_gl1_i2c_event(void);
static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void))
{
imu_gl1_i2c_event();
if (imu_gl1.gyro_valid) {
imu_gl1.gyro_valid = FALSE;
_gyro_handler();
}
if (imu_gl1.accel_valid) {
imu_gl1.accel_valid = FALSE;
_accel_handler();
}
if (imu_gl1.mag_valid) {
imu_gl1.mag_valid = FALSE;
_mag_handler();
}
}
#define ImuEvent imu_gl1_i2c_event
#endif /* IMU_GL1_H */
sw/airborne/subsystems/imu/imu_mpu6000.c
+6 -6
View File
@@ -25,7 +25,7 @@
*/
#include "subsystems/imu.h"
#include "subsystems/abi.h"
#include "mcu_periph/spi.h"
/* SPI defaults set in subsystem makefile, can be configured from airframe file */
@@ -71,9 +71,6 @@ struct ImuMpu6000 imu_mpu_spi;
void imu_impl_init(void)
{
imu_mpu_spi.accel_valid = FALSE;
imu_mpu_spi.gyro_valid = FALSE;
mpu60x0_spi_init(&imu_mpu_spi.mpu, &IMU_MPU_SPI_DEV, IMU_MPU_SPI_SLAVE_IDX);
// change the default configuration
imu_mpu_spi.mpu.config.smplrt_div = IMU_MPU_SMPLRT_DIV;
@@ -92,10 +89,13 @@ void imu_mpu_spi_event(void)
{
mpu60x0_spi_event(&imu_mpu_spi.mpu);
if (imu_mpu_spi.mpu.data_available) {
uint32_t now_ts = get_sys_time_usec();
RATES_COPY(imu.gyro_unscaled, imu_mpu_spi.mpu.data_rates.rates);
VECT3_COPY(imu.accel_unscaled, imu_mpu_spi.mpu.data_accel.vect);
imu_mpu_spi.mpu.data_available = FALSE;
imu_mpu_spi.gyro_valid = TRUE;
imu_mpu_spi.accel_valid = TRUE;
imu_scale_gyro(&imu);
imu_scale_accel(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_MPU6000_ID, now_ts, &imu.gyro);
AbiSendMsgIMU_ACCEL_INT32(IMU_MPU6000_ID, now_ts, &imu.accel);
}
}
sw/airborne/subsystems/imu/imu_mpu6000.h
+1 -14
View File
@@ -36,8 +36,6 @@
struct ImuMpu6000 {
volatile bool_t gyro_valid;
volatile bool_t accel_valid;
struct Mpu60x0_Spi mpu;
};
@@ -46,17 +44,6 @@ extern struct ImuMpu6000 imu_mpu_spi;
extern void imu_mpu_spi_event(void);
static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void) __attribute__((unused)))
{
imu_mpu_spi_event();
if (imu_mpu_spi.gyro_valid) {
imu_mpu_spi.gyro_valid = FALSE;
_gyro_handler();
}
if (imu_mpu_spi.accel_valid) {
imu_mpu_spi.accel_valid = FALSE;
_accel_handler();
}
}
#define ImuEvent imu_mpu_spi_event
#endif /* IMU_MPU6000_H */
sw/airborne/subsystems/imu/imu_mpu6000_hmc5883.c
+9 -8
View File
@@ -25,7 +25,7 @@
*/
#include "subsystems/imu.h"
#include "subsystems/abi.h"
#include "mcu_periph/spi.h"
#include "peripherals/hmc58xx_regs.h"
@@ -74,10 +74,6 @@ struct ImuMpu6000Hmc5883 imu_mpu_hmc;
void imu_impl_init(void)
{
imu_mpu_hmc.accel_valid = FALSE;
imu_mpu_hmc.gyro_valid = FALSE;
imu_mpu_hmc.mag_valid = FALSE;
mpu60x0_spi_init(&imu_mpu_hmc.mpu, &IMU_MPU_SPI_DEV, IMU_MPU_SPI_SLAVE_IDX);
// change the default configuration
imu_mpu_hmc.mpu.config.smplrt_div = IMU_MPU_SMPLRT_DIV;
@@ -102,13 +98,17 @@ void imu_periodic(void)
void imu_mpu_hmc_event(void)
{
uint32_t now_ts = get_sys_time_usec();
mpu60x0_spi_event(&imu_mpu_hmc.mpu);
if (imu_mpu_hmc.mpu.data_available) {
RATES_COPY(imu.gyro_unscaled, imu_mpu_hmc.mpu.data_rates.rates);
VECT3_COPY(imu.accel_unscaled, imu_mpu_hmc.mpu.data_accel.vect);
imu_mpu_hmc.mpu.data_available = FALSE;
imu_mpu_hmc.gyro_valid = TRUE;
imu_mpu_hmc.accel_valid = TRUE;
imu_scale_gyro(&imu);
imu_scale_accel(&imu);
AbiSendMsgIMU_GYRO_INT32(IMU_MPU6000_HMC_ID, now_ts, &imu.gyro);
AbiSendMsgIMU_ACCEL_INT32(IMU_MPU6000_HMC_ID, now_ts, &imu.accel);
}
/* HMC58XX event task */
@@ -119,6 +119,7 @@ void imu_mpu_hmc_event(void)
imu.mag_unscaled.y = -imu_mpu_hmc.hmc.data.vect.x;
imu.mag_unscaled.z = imu_mpu_hmc.hmc.data.vect.z;
imu_mpu_hmc.hmc.data_available = FALSE;
imu_mpu_hmc.mag_valid = TRUE;
imu_scale_mag(&imu);
AbiSendMsgIMU_MAG_INT32(IMU_MPU6000_HMC_ID, now_ts, &imu.mag);
}
}
sw/airborne/subsystems/imu/imu_mpu6000_hmc5883.h
+1 -20
View File
@@ -38,9 +38,6 @@
struct ImuMpu6000Hmc5883 {
volatile bool_t gyro_valid;
volatile bool_t accel_valid;
volatile bool_t mag_valid;
struct Mpu60x0_Spi mpu;
struct Hmc58xx hmc;
};
@@ -49,22 +46,6 @@ extern struct ImuMpu6000Hmc5883 imu_mpu_hmc;
extern void imu_mpu_hmc_event(void);
static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void))
{
imu_mpu_hmc_event();
if (imu_mpu_hmc.gyro_valid) {
imu_mpu_hmc.gyro_valid = FALSE;
_gyro_handler();
}
if (imu_mpu_hmc.accel_valid) {
imu_mpu_hmc.accel_valid = FALSE;
_accel_handler();
}
if (imu_mpu_hmc.mag_valid) {
imu_mpu_hmc.mag_valid = FALSE;
_mag_handler();
}
}
#define ImuEvent imu_mpu_hmc_event
#endif /* IMU_MPU6000_HMC5883_H */

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