Skip to content

Commit

Permalink
Add external IMU support (SIMU).
Browse files Browse the repository at this point in the history
  • Loading branch information
@mpaperno
mpaperno committed Apr 14, 2016
1 parent 3eb036e commit e8227b5
Show file tree
Hide file tree
Showing 5 changed files with 117 additions and 69 deletions.
4 changes: 2 additions & 2 deletions src/d_imu.c
View file
Expand Up @@ -337,7 +337,7 @@ static void dIMUTaskCode(void *unused) {
max21100DrateDecode();
#endif

imuDImuDRateReady();
imuSensorReady(IMU_TYPE_DIMU, IMU_UPDT_DRATE);

// full sensor loop
if (!(loops % (DIMU_OUTER_PERIOD/DIMU_INNER_PERIOD))) {
Expand All @@ -354,7 +354,7 @@ static void dIMUTaskCode(void *unused) {
ms5611Decode();
#endif
dImuData.lastUpdate = timerMicros();
imuDImuSensorReady();
imuSensorReady(IMU_TYPE_DIMU, IMU_UPDT_FULL);
dIMUCalcTempDiff();
}
}
Expand Down
6 changes: 4 additions & 2 deletions src/drivers/adc.c
View file
Expand Up @@ -113,7 +113,7 @@ void adcTaskCode(void *unused) {
// adcData.dRateZ = c;

// notify IMU of double rate GYO readings are ready
imuAdcDRateReady();
imuSensorReady(IMU_TYPE_AIMU, IMU_UPDT_DRATE);

// mags
// we need to discard frames after mag bias flips
Expand Down Expand Up @@ -252,7 +252,7 @@ void adcTaskCode(void *unused) {
adcData.dt = (float)(adcData.sampleTime * 2) * (1.0f / (float)AQ_US_PER_SEC);
adcData.lastUpdate = adcData.lastSample;

imuAdcSensorReady();
imuSensorReady(IMU_TYPE_AIMU, IMU_UPDT_FULL);
}

// flip our mag sign and try to refine bridge bias estimate
Expand Down Expand Up @@ -326,6 +326,8 @@ void adcInit(void) {

memset((void *)&adcData, 0, sizeof(adcData));

adcData.magEnabled = 1;

// energize mag's set/reset circuit
adcData.magSetReset = digitalInit(GPIOE, GPIO_Pin_10, 1);

Expand Down
10 changes: 7 additions & 3 deletions src/drivers/adc.h
View file
Expand Up @@ -19,6 +19,7 @@
#ifndef _adc_h
#define _adc_h

#include "aq.h"
#include "digital.h"
#include <CoOS.h>

Expand Down Expand Up @@ -96,9 +97,6 @@
#define ADC_VOLTS_TEMP3 14

typedef struct {
OS_TID adcTask;
OS_FlagID adcFlag;

unsigned long interrupt123Sums[ADC_SENSORS];

unsigned long volatile adcSums[ADC_SENSORS];
Expand Down Expand Up @@ -130,7 +128,13 @@ typedef struct {
volatile unsigned long lastSample;
volatile unsigned long sampleTime;
float dt;

int8_t magSign;
bool magEnabled;

OS_TID adcTask;
OS_FlagID adcFlag;

} adcStruct_t __attribute__((aligned));

extern adcStruct_t adcData;
Expand Down
67 changes: 45 additions & 22 deletions src/imu.c
View file
Expand Up @@ -18,7 +18,9 @@

#include "imu.h"
#include "arm_math.h"
#include "aq_timer.h"
#include "config.h"
#include "supervisor.h"
#include <string.h>

imuStruct_t imuData __attribute__((section(".ccm")));
Expand Down Expand Up @@ -63,12 +65,30 @@ void imuCalcRot(void) {
imuData.cosRot = cosf(rotAngle);
}

void imuInitData(void) {

#ifdef USE_DIGITAL_IMU

imuData.magEnabled = &hmc5983Data.enabled;
imuData.lastUpdate = &dImuData.lastUpdate;

#else // AIMU

imuData.magEnabled = &adcData.magEnabled;
imuData.lastUpdate = &adcData.lastUpdate;

#endif

}

void imuInit(void) {
memset((void *)&imuData, 0, sizeof(imuData));

imuData.dRateFlag = CoCreateFlag(1, 0);
imuData.sensorFlag = CoCreateFlag(1, 0);

// set up data pointers
imuInitData();
// calculate IMU rotation
imuCalcRot();

Expand All @@ -81,30 +101,33 @@ void imuInit(void) {
#endif // HAS_DIGITAL_IMU
}

void imuAdcDRateReady(void) {
#ifndef USE_DIGITAL_IMU
imuData.halfUpdates++;
CoSetFlag(imuData.dRateFlag);
#endif
}
void imuSensorReady(int8_t imuType, uint8_t updtType) {

void imuAdcSensorReady(void) {
switch (imuType) {
case IMU_TYPE_DIMU :
#ifndef USE_DIGITAL_IMU
imuData.fullUpdates++;
CoSetFlag(imuData.sensorFlag);
return;
#endif
}
break;

void imuDImuDRateReady(void) {
#ifdef USE_DIGITAL_IMU
imuData.halfUpdates++;
CoSetFlag(imuData.dRateFlag);
#endif // USE_DIGITAL_IMU
}

void imuDImuSensorReady(void) {
#ifdef USE_DIGITAL_IMU
imuData.fullUpdates++;
CoSetFlag(imuData.sensorFlag);
#endif // USE_DIGITAL_IMU
case IMU_TYPE_AIMU :
#if !defined(HAS_AIMU) || defined(USE_DIGITAL_IMU)
return;
#endif
break;

case IMU_TYPE_SIMU :
if (!(supervisorData.state & STATE_SIM_ENABLED))
return;
break;
}

if (updtType == IMU_UPDT_FULL) {
imuData.fullUpdates++;
CoSetFlag(imuData.sensorFlag);
}
else {
imuData.halfUpdates++;
CoSetFlag(imuData.dRateFlag);
}
}
99 changes: 59 additions & 40 deletions src/imu.h
View file
Expand Up @@ -33,101 +33,120 @@
#define AQ_ROLL navUkfData.roll
#define AQ_PRES_ADJ UKF_PRES_ALT

#define IMU_LASTUPD (*imuData.lastUpdate)
#define AQ_MAG_ENABLED (*imuData.magEnabled)

#ifdef USE_DIGITAL_IMU
// using the Digital IMU as IMU

#ifdef DIMU_HAVE_MAX21100

#define IMU_DRATEX max21100Data.dRateGyo[0]
#define IMU_DRATEY max21100Data.dRateGyo[1]
#define IMU_DRATEZ max21100Data.dRateGyo[2]
#define IMU_RATEX max21100Data.gyo[0]
#define IMU_RATEY max21100Data.gyo[1]
#define IMU_RATEZ max21100Data.gyo[2]
#define IMU_RAW_RATEX max21100Data.rawGyo[0]
#define IMU_RAW_RATEY max21100Data.rawGyo[1]
#define IMU_RAW_RATEZ max21100Data.rawGyo[2]
#define IMU_RAW_RATEX max21100Data.rawGyo[0]
#define IMU_RAW_RATEY max21100Data.rawGyo[1]
#define IMU_RAW_RATEZ max21100Data.rawGyo[2]
#define IMU_ACCX max21100Data.acc[0]
#define IMU_ACCY max21100Data.acc[1]
#define IMU_ACCZ max21100Data.acc[2]
#define IMU_RAW_ACCX max21100Data.rawAcc[0]
#define IMU_RAW_ACCY max21100Data.rawAcc[1]
#define IMU_RAW_ACCZ max21100Data.rawAcc[2]
#else
#define IMU_RAW_ACCX max21100Data.rawAcc[0]
#define IMU_RAW_ACCY max21100Data.rawAcc[1]
#define IMU_RAW_ACCZ max21100Data.rawAcc[2]

#elif defined(DIMU_HAVE_MPU6000)

#define IMU_DRATEX mpu6000Data.dRateGyo[0]
#define IMU_DRATEY mpu6000Data.dRateGyo[1]
#define IMU_DRATEZ mpu6000Data.dRateGyo[2]
#define IMU_RATEX mpu6000Data.gyo[0]
#define IMU_RATEY mpu6000Data.gyo[1]
#define IMU_RATEZ mpu6000Data.gyo[2]
#define IMU_RAW_RATEX mpu6000Data.rawGyo[0]
#define IMU_RAW_RATEY mpu6000Data.rawGyo[1]
#define IMU_RAW_RATEZ mpu6000Data.rawGyo[2]
#define IMU_RAW_RATEX mpu6000Data.rawGyo[0]
#define IMU_RAW_RATEY mpu6000Data.rawGyo[1]
#define IMU_RAW_RATEZ mpu6000Data.rawGyo[2]
#define IMU_ACCX mpu6000Data.acc[0]
#define IMU_ACCY mpu6000Data.acc[1]
#define IMU_ACCZ mpu6000Data.acc[2]
#define IMU_RAW_ACCX mpu6000Data.rawAcc[0]
#define IMU_RAW_ACCY mpu6000Data.rawAcc[1]
#define IMU_RAW_ACCZ mpu6000Data.rawAcc[2]
#endif
#define IMU_RAW_ACCX mpu6000Data.rawAcc[0]
#define IMU_RAW_ACCY mpu6000Data.rawAcc[1]
#define IMU_RAW_ACCZ mpu6000Data.rawAcc[2]

#endif // DIMU_HAVE_MPU6000

#define IMU_MAGX hmc5983Data.mag[0]
#define IMU_MAGY hmc5983Data.mag[1]
#define IMU_MAGZ hmc5983Data.mag[2]
#define IMU_RAW_MAGX hmc5983Data.rawMag[0]
#define IMU_RAW_MAGY hmc5983Data.rawMag[1]
#define IMU_RAW_MAGZ hmc5983Data.rawMag[2]
#define IMU_RAW_MAGX hmc5983Data.rawMag[0]
#define IMU_RAW_MAGY hmc5983Data.rawMag[1]
#define IMU_RAW_MAGZ hmc5983Data.rawMag[2]
#define IMU_TEMP dImuData.temp
#define IMU_LASTUPD dImuData.lastUpdate
#define AQ_OUTER_TIMESTEP DIMU_OUTER_DT
#define AQ_INNER_TIMESTEP DIMU_INNER_DT
#define AQ_PRESSURE ms5611Data.pres
#define AQ_MAG_ENABLED hmc5983Data.enabled
#endif // USE_DIGITAL_IMU

#ifndef USE_DIGITAL_IMU
#elif defined(HAS_AIMU)

// using ADC as IMU
#define IMU_DRATEX adcData.dRateX
#define IMU_DRATEY adcData.dRateY
#define IMU_DRATEZ adcData.dRateZ
#define IMU_RATEX adcData.rateX
#define IMU_RATEY adcData.rateY
#define IMU_RATEZ adcData.rateZ
#define IMU_RAW_RATEX adcData.voltages[ADC_VOLTS_RATEX]
#define IMU_RAW_RATEY adcData.voltages[ADC_VOLTS_RATEY]
#define IMU_RAW_RATEZ adcData.voltages[ADC_VOLTS_RATEZ]
#define IMU_RAW_RATEX adcData.voltages[ADC_VOLTS_RATEX]
#define IMU_RAW_RATEY adcData.voltages[ADC_VOLTS_RATEY]
#define IMU_RAW_RATEZ adcData.voltages[ADC_VOLTS_RATEZ]
#define IMU_ACCX adcData.accX
#define IMU_ACCY adcData.accY
#define IMU_ACCZ adcData.accZ
#define IMU_RAW_ACCX adcData.voltages[ADC_VOLTS_ACCX]
#define IMU_RAW_ACCY adcData.voltages[ADC_VOLTS_ACCY]
#define IMU_RAW_ACCZ adcData.voltages[ADC_VOLTS_ACCZ]
#define IMU_RAW_ACCX adcData.voltages[ADC_VOLTS_ACCX]
#define IMU_RAW_ACCY adcData.voltages[ADC_VOLTS_ACCY]
#define IMU_RAW_ACCZ adcData.voltages[ADC_VOLTS_ACCZ]
#define IMU_MAGX adcData.magX
#define IMU_MAGY adcData.magY
#define IMU_MAGZ adcData.magZ
#define IMU_RAW_MAGX adcData.voltages[ADC_VOLTS_MAGX]
#define IMU_RAW_MAGY adcData.voltages[ADC_VOLTS_MAGY]
#define IMU_RAW_MAGZ adcData.voltages[ADC_VOLTS_MAGZ]
#define IMU_RAW_MAGX adcData.voltages[ADC_VOLTS_MAGX]
#define IMU_RAW_MAGY adcData.voltages[ADC_VOLTS_MAGY]
#define IMU_RAW_MAGZ adcData.voltages[ADC_VOLTS_MAGZ]
#define IMU_TEMP adcData.temperature
#define IMU_LASTUPD adcData.lastUpdate
#define AQ_OUTER_TIMESTEP adcData.dt
#define AQ_INNER_TIMESTEP (adcData.dt * 0.5f)
#define AQ_PRESSURE adcData.pressure
#define AQ_MAG_ENABLED 1
#endif

#endif // HAS_AIMU && !USE_DIGITAL_IMU

enum imuTypes {
IMU_TYPE_AIMU,
IMU_TYPE_DIMU,
IMU_TYPE_SIMU
};

enum imuUpdateTypes {
IMU_UPDT_DRATE,
IMU_UPDT_FULL,
};

typedef struct {
OS_FlagID dRateFlag;
OS_FlagID sensorFlag;
volatile uint32_t *lastUpdate;
bool *magEnabled;

float sinRot, cosRot;

uint32_t fullUpdates;
uint32_t halfUpdates;
OS_FlagID dRateFlag;
OS_FlagID sensorFlag;

} imuStruct_t __attribute__((aligned));

extern imuStruct_t imuData;

extern void imuInitData(void);
extern void imuInit(void);
extern void imuQuasiStatic(int n);
extern void imuAdcDRateReady(void);
extern void imuAdcSensorReady(void);
extern void imuDImuDRateReady(void);
extern void imuDImuSensorReady(void);
extern void imuSensorReady(int8_t imuType, uint8_t updtType);

#endif

0 comments on commit e8227b5

Please sign in to comment.