void initMag(I2C_TypeDef *I2Cx)
{
uint8_t I2C_Buffer_Rx[1] = { 0 };
uint8_t i;
i2cWrite(I2Cx, HMC5883_ADDRESS, HMC5883_CONFIG_REG_A, SENSOR_CONFIG | POSITIVE_BIAS_CONFIGURATION);
delay(50);
i2cWrite(I2Cx, HMC5883_ADDRESS, HMC5883_CONFIG_REG_B, SENSOR_GAIN);
delay(20);
magScaleFactor[XAXIS] = 0.0f;
magScaleFactor[YAXIS] = 0.0f;
magScaleFactor[ZAXIS] = 0.0f;
for (i = 0; i < 10; i++)
{
i2cWrite(I2Cx, HMC5883_ADDRESS, HMC5883_MODE_REG, OP_MODE_SINGLE);
delay(20);
while ( (I2C_Buffer_Rx[0] & STATUS_RDY) == 0x00 )
i2cRead(I2Cx, HMC5883_ADDRESS, HMC5883_STATUS_REG, 1, I2C_Buffer_Rx);
readMag(I2Cx);
magScaleFactor[XAXIS] += (1.16f * 1090.0f) / (float)rawMag[XAXIS].value;
magScaleFactor[YAXIS] += (1.16f * 1090.0f) / (float)rawMag[YAXIS].value;
magScaleFactor[ZAXIS] += (1.08f * 1090.0f) / (float)rawMag[ZAXIS].value;
}
magScaleFactor[XAXIS] = fabs(magScaleFactor[XAXIS] / 10.0f);
magScaleFactor[YAXIS] = fabs(magScaleFactor[YAXIS] / 10.0f);
magScaleFactor[ZAXIS] = fabs(magScaleFactor[ZAXIS] / 10.0f);
i2cWrite(I2Cx, HMC5883_ADDRESS, HMC5883_CONFIG_REG_A, SENSOR_CONFIG | NORMAL_MEASUREMENT_CONFIGURATION);
delay(50);
i2cWrite(I2Cx, HMC5883_ADDRESS, HMC5883_MODE_REG, OP_MODE_CONTINUOUS);
delay(20);
readMag(I2Cx);
delay(20);
}
//szenzor kiolvasas
static void prvSensorReadTask (void* pvParameters)
{
portTickType xLastWakeTime;
static uint8_t data[6];
static uint8_t newData;
AccAxesRaw_t AccAxes;
uint8_t status;
int16_t x_acc,y_acc,z_acc;
int16_t x_mag,y_mag,z_mag;
int16_t x_gyr,y_gyr,z_gyr;
xLastWakeTime=xTaskGetTickCount();
while(1)
{
GetSatusReg(&status);
if (status&0b0001000) //new data received
{
newData|=1;
// GetAccAxesRaw(&AccAxes);
readACC(data);
//allithato endiannes miatt olvasható így is
x_acc=((int16_t*)data)[0];
y_acc=((int16_t*)data)[1];
z_acc=((int16_t*)data)[2];
}
ReadStatusM(&status);
if (status&0x01)
{
readMag(data);
x_mag=((int16_t)data[0])<<8;
x_mag|=data[1];
y_mag=((int16_t)data[2])<<8;
y_mag|=data[3];
z_mag=((int16_t)data[4])<<8;
z_mag|=data[5];
}
L3G4200D_GetSatusReg(&status);
if (status&0b00001000)
{
newData|=2;
x_gyr=((int16_t*)data)[0];
y_gyr=((int16_t*)data)[1];
z_gyr=((int16_t*)data)[2];
}
if (newData==3)
{
newData=0;
}
vTaskDelayUntil(&xLastWakeTime,100);
}
}
void magCalibration(void)
{
uint16_t calibrationCounter = 0;
uint16_t population[2][3];
float d[600][3]; // 600 Samples = 60 seconds of data at 10 Hz
float sphereOrigin[3];
float sphereRadius;
magCalibrating = true;
cliPrintF("\nMagnetometer Calibration:\n\n");
cliPrintF("Rotate magnetometer around all axes multiple times\n");
cliPrintF("Must complete within 60 seconds....\n\n");
cliPrintF(" Send a character when ready to begin and another when complete\n\n");
while (cliAvailable() == false);
cliPrintF(" Start rotations.....\n");
getChar();
while ((cliAvailable() == false) && (calibrationCounter < 600))
{
if (readMag() == true)
{
d[calibrationCounter][XAXIS] = (float)rawMag[XAXIS].value * magScaleFactor[XAXIS];
d[calibrationCounter][YAXIS] = (float)rawMag[YAXIS].value * magScaleFactor[YAXIS];
d[calibrationCounter][ZAXIS] = (float)rawMag[ZAXIS].value * magScaleFactor[ZAXIS];
calibrationCounter++;
}
delay(100);
}
cliPrintF("\n\nMagnetometer Bias Calculation, %3ld samples collected out of 600 max)\n", calibrationCounter);
sphereFit(d, calibrationCounter, 100, 0.0f, population, sphereOrigin, &sphereRadius);
eepromConfig.magBias[XAXIS] = sphereOrigin[XAXIS];
eepromConfig.magBias[YAXIS] = sphereOrigin[YAXIS];
eepromConfig.magBias[ZAXIS] = sphereOrigin[ZAXIS];
magCalibrating = false;
}
static void cmd_magdata(BaseSequentialStream *chp, int argc, char *argv[]) {
float magdata[3];
uint32_t times = 5;
uint32_t delay = 200;
if (argc >= 1) {
times = atoi(argv[0]);
}
if (argc >= 2) {
delay = atoi(argv[1]);
}
uint32_t i = 0;
chprintf(chp, " Number 1\t Number 2\t Number 3\t #\r\n");
for (i = 0; i < times; i++) {
chThdSleepMilliseconds(delay);
if (readMag(magdata)) {
chprintf(chp, " %f\t %f\t %f\t %d\r\n", magdata[0], magdata[1], magdata[2], i);
}
}
}
//force a read and update the global values
float updateHeading( void ){
neinAxis_semaphore = LOCKED;
readMag(global_mag);
readAcc(global_acc);
readGyr(global_gyr);
int acc[3];
acc[xaxis] = global_acc[xaxis];
acc[yaxis] = global_acc[yaxis];
acc[zaxis] = global_acc[zaxis];
int mag[3];
mag[xaxis] = global_mag[xaxis];
mag[yaxis] = global_mag[yaxis];
mag[zaxis] = global_mag[zaxis];
float acc_norm[3];
float mag_norm[3];
//readAcc(acc);
//readMag(mag);
acc_norm[xaxis]=maxAccG*((float) acc[xaxis]/maxAccGRaw);
acc_norm[yaxis]=maxAccG*((float) acc[yaxis]/maxAccGRaw);
acc_norm[zaxis]=maxAccG*((float) acc[zaxis]/maxAccGRaw);
mag_norm[xaxis]=maxMag*((float) mag[xaxis]/maxMagRaw);
mag_norm[yaxis]=maxMag*((float) mag[yaxis]/maxMagRaw);
mag_norm[zaxis]=maxMag*((float) mag[zaxis]/maxMagRaw);
float pitch = asin(acc_norm[xaxis]);
float roll = asin(acc_norm[yaxis]);
float cosRoll = cos(roll);
float sinRoll = sin(roll);
float cosPitch = cos(pitch);
float sinPitch = sin(pitch);
float Xh = mag_norm[xaxis] * cosPitch + mag_norm[zaxis] * sinPitch;
float Yh = mag_norm[xaxis] * sinRoll * sinPitch + mag_norm[yaxis] * cosRoll - mag_norm[zaxis] * sinRoll * cosPitch;
neinAxis_semaphore = UNLOCKED;
return atan2f(Yh, Xh);
}
void __attribute__((__interrupt__, no_auto_psv)) _MI2C1Interrupt(void){
switch (i2c1State) {
case CONFIG_START:
if (!I2C1CONbits.SEN){
// change the state
i2c1State = CONFIG_IDTX;
// Send the address to write
i2c1Write(MAG_WRITE);
}
break;
case CONFIG_IDTX:
if (!I2C1STATbits.ACKSTAT){
// change the state
i2c1State = CONFIG_REG_TX;
// send the register address
i2c1Write(reg2Config);
}
break;
case CONFIG_REG_TX:
if (!I2C1STATbits.ACKSTAT){
i2c1State = CONFIG_VAL_TX;
if (reg2Config==REGISTER_A){
// send the actual configuration for 50Hz
i2c1Write(MODE_50_HZ);
} else {
// send the configuration for continuous sampling
i2c1Write(MODE_CONTINUOS);
}
}
break;
case CONFIG_VAL_TX:
if (!I2C1STATbits.ACKSTAT){
i2c1State = CONFIG_DONE;
// Send a bus Stop
i2c1Stop();
}
break;
case CONFIG_STOP:
if (!I2C1CONbits.PEN){
if (reg2Config!=MODE_REGISTER){
// Set the Config to idle, wait for next config
i2c1State = CONFIG_IDLE;
} else {
// Ready to read magnetometer data
i2c1State = READ_IDLE;
}
}
break;
case READ_START:
if (!I2C1CONbits.SEN){
// change the state
i2c1State = READ_IDTX;
// Send the address to write
i2c1Write(MAG_READ);
}
break;
case READ_IDTX:
//if (!I2C1CONbits.RSEN){
if (!I2C1STATbits.ACKSTAT){
// change the state
i2c1State = READ_DATA;
// Start Clocking Data
I2C1CONbits.RCEN = 1;
}
break;
case READ_DATA:
if (I2C1STATbits.RBF){
//printToUart2("br:%d wr:%d dc:%d\n\r",byteRead, wordRead,byteCount);
byteCount++;
// read the received Data
if (byteCount < 7) {
currentMag.chData[byteRead] = (unsigned char)I2C1RCV;
//printToUart2("byte:%d\n\r", I2C1RCV);
} else {
wordRead= (unsigned char)I2C1RCV;
byteRead = 1;
//printToUart2("ignored:%d\n\r", I2C1RCV);
}
// if we just read a word (i.e. byteRead == 1)
if(byteRead == 0)
{
//printToUart2("%u\n\r",currentMag.usData);
readMag(wordRead,currentMag.shData);
wordRead++;
}
// flip the byte
byteRead = byteRead == 0? 1 : 0;
//if (wordRead >= 3){
if (byteCount >= 7 ){
// Done Generate a NACK
I2C1CONbits.ACKDT = 1;
I2C1CONbits.ACKEN = 1;
} else {
// Generate the AKN
I2C1CONbits.ACKDT = 0;
I2C1CONbits.ACKEN = 1;
}
} else if (!I2C1STATbits.ACKSTAT){
//if (wordRead < 3){
if (byteCount < 7){
// Enable reading
I2C1CONbits.RCEN = 1;
} else {
// change the state
i2c1State = READ_DONE;
i2c1Stop();
}
}
break;
case READ_STOP:
if (!I2C1CONbits.PEN){
// Ready to read next magnetometer data
i2c1State = READ_IDLE;
}
break;
}
//
IFS1bits.MI2C1IF = 0; // Clear the master interrupt
I2C1STATbits.IWCOL = 0; // Clear the collision flag just in case
}
// Reads all 6 channels of the LSM303 and stores them in the object variables
void LSM303_read(LSM303 *compass)
{
//readAcc();
readMag(compass);
}
void SysTick_Handler(void)
{
uint8_t index;
uint32_t currentTime;
sysTickCycleCounter = *DWT_CYCCNT;
sysTickUptime++;
#if defined(USE_MADGWICK_AHRS) | defined(USE_MARG_AHRS)
if ((systemReady == true) &&
(accelCalibrating == false) &&
(gyroCalibrating == false) &&
(magCalibrating == false))
#endif
#if defined(USE_CHR6DM_AHRS)
if ((systemReady == true) &&
(accelCalibrating == false) &&
(gyroCalibrating == false) &&
(magCalibrating == false) &&
(ahrsCalibrating == false))
#endif
{
frameCounter++;
if (frameCounter > FRAME_COUNT)
frameCounter = 1;
///////////////////////////////
currentTime = micros();
deltaTime1000Hz = currentTime - previous1000HzTime;
previous1000HzTime = currentTime;
readAccel();
#if defined(USE_MADGWICK_AHRS) | defined(USE_MARG_AHRS)
accelSum200Hz[XAXIS] += rawAccel[XAXIS].value;
accelSum200Hz[YAXIS] += rawAccel[YAXIS].value;
accelSum200Hz[ZAXIS] += rawAccel[ZAXIS].value;
#endif
accelSum100Hz[XAXIS] += rawAccel[XAXIS].value;
accelSum100Hz[YAXIS] += rawAccel[YAXIS].value;
accelSum100Hz[ZAXIS] += rawAccel[ZAXIS].value;
readGyro();
gyroSum200Hz[ROLL] += rawGyro[ROLL].value;
gyroSum200Hz[PITCH] += rawGyro[PITCH].value;
gyroSum200Hz[YAW] += rawGyro[YAW].value;
gyroSum100Hz[ROLL] += rawGyro[ROLL].value;
gyroSum100Hz[PITCH] += rawGyro[PITCH].value;
gyroSum100Hz[YAW] += rawGyro[YAW].value;
gyroSum500Hz[ROLL] += rawGyro[ROLL].value;
gyroSum500Hz[PITCH] += rawGyro[PITCH].value;
gyroSum500Hz[YAW] += rawGyro[YAW].value;
///////////////////////////////
if ((frameCounter % COUNT_500HZ) == 0) {
frame_500Hz = true;
for (index = 0; index < 3; index++) {
gyroSummedSamples500Hz[index] = gyroSum500Hz[index];
gyroSum500Hz[index] = 0.0f;
}
}
///////////////////////////////
if ((frameCounter % COUNT_200HZ) == 0) {
frame_200Hz = true;
for (index = 0; index < 3; index++) {
#if defined(USE_MADGWICK_AHRS) | defined(USE_MARG_AHRS)
accelSummedSamples200Hz[index] = accelSum200Hz[index];
accelSum200Hz[index] = 0.0f;
#endif
gyroSummedSamples200Hz[index] = gyroSum200Hz[index];
gyroSum200Hz[index] = 0.0f;
}
}
///////////////////////////////
if ((frameCounter % COUNT_100HZ) == 0) {
frame_100Hz = true;
for (index = 0; index < 3; index++) {
accelSummedSamples100Hz[index] = accelSum100Hz[index];
accelSum100Hz[index] = 0.0f;
gyroSummedSamples100Hz[index] = gyroSum100Hz[index];
gyroSum100Hz[index] = 0.0f;
}
if (frameCounter == COUNT_100HZ) {
readTemperatureRequestPressure();
} else if (frameCounter == FRAME_COUNT) {
readPressureRequestTemperature();
} else {
readPressureRequestPressure();
}
pressureSum += uncompensatedPressure.value;
}
///////////////////////////////
if (((frameCounter + 1) % COUNT_50HZ) == 0) {
readMag();
magSum[XAXIS] += rawMag[XAXIS].value;
magSum[YAXIS] += rawMag[YAXIS].value;
magSum[ZAXIS] += rawMag[ZAXIS].value;
}
if ((frameCounter % COUNT_50HZ) == 0) {
frame_50Hz = true;
}
///////////////////////////////
if ((frameCounter % COUNT_10HZ) == 0)
frame_10Hz = true;
///////////////////////////////
if ((frameCounter % COUNT_5HZ) == 0)
frame_5Hz = true;
///////////////////////////////
if ((frameCounter % COUNT_1HZ) == 0)
frame_1Hz = true;
///////////////////////////////////
executionTime1000Hz = micros() - currentTime;
///////////////////////////////
}
}
void readSensors()
{
readGyro();
readAccel();
readMag();
}
float LSM303C::readMagX()
{
return readMag(xAxis);
}
float LSM303C::readMagZ()
{
return readMag(zAxis);
}
float LSM303C::readMagY()
{
return readMag(yAxis);
}
void initMag(void)
{
uint8_t I2C_Buffer_Rx[1] = { 0 };
uint8_t i;
if (eepromConfig.externalHMC5883 == true)
{
hmc5883I2C = I2C2;
hmc5883Address = 0x1E;
}
else
{
hmc5883I2C = I2C1;
hmc5883Address = 0x1E;
}
i2cWrite(hmc5883I2C, hmc5883Address, HMC5883_CONFIG_REG_A, SENSOR_CONFIG | POSITIVE_BIAS_CONFIGURATION);
delay(50);
i2cWrite(hmc5883I2C, hmc5883Address, HMC5883_CONFIG_REG_B, SENSOR_GAIN);
delay(20);
magScaleFactor[XAXIS + eepromConfig.externalHMC5883] = 0.0f;
magScaleFactor[YAXIS + eepromConfig.externalHMC5883] = 0.0f;
magScaleFactor[ZAXIS + eepromConfig.externalHMC5883] = 0.0f;
for (i = 0; i < 10; i++)
{
i2cWrite(hmc5883I2C, hmc5883Address, HMC5883_MODE_REG, OP_MODE_SINGLE);
delay(50);
do
{
delay(1);
i2cRead(hmc5883I2C, hmc5883Address, HMC5883_STATUS_REG, 1, I2C_Buffer_Rx);
}
while ((I2C_Buffer_Rx[0] & STATUS_RDY) == 0x00);
if (readMag())
{
magScaleFactor[XAXIS + eepromConfig.externalHMC5883] += (1.16f * 1090.0f) / (float)rawMag[XAXIS].value;
magScaleFactor[YAXIS + eepromConfig.externalHMC5883] += (1.16f * 1090.0f) / (float)rawMag[YAXIS].value;
magScaleFactor[ZAXIS + eepromConfig.externalHMC5883] += (1.08f * 1090.0f) / (float)rawMag[ZAXIS].value;
I2C_Buffer_Rx[0] = 0;
}
else
{
i--;
}
}
magScaleFactor[XAXIS + eepromConfig.externalHMC5883] = fabsf(magScaleFactor[XAXIS + eepromConfig.externalHMC5883] / 10.0f);
magScaleFactor[YAXIS + eepromConfig.externalHMC5883] = fabsf(magScaleFactor[YAXIS + eepromConfig.externalHMC5883] / 10.0f);
magScaleFactor[ZAXIS + eepromConfig.externalHMC5883] = fabsf(magScaleFactor[ZAXIS + eepromConfig.externalHMC5883] / 10.0f);
i2cWrite(hmc5883I2C, hmc5883Address, HMC5883_CONFIG_REG_A, SENSOR_CONFIG | NORMAL_MEASUREMENT_CONFIGURATION);
delay(50);
i2cWrite(hmc5883I2C, hmc5883Address, HMC5883_MODE_REG, OP_MODE_CONTINUOUS);
delay(20);
readMag();
delay(20);
}
///< Reads all 6 channels of the MagneticSensorLsm303 and stores them in the object variables
void MagneticSensorLsm303::read(void)
{
readAcc();
readMag();
}
// Reads all 6 channels of the LSM303 and stores them in the object variables
void LSM303::read(void)
{
readAcc();
readMag();
}
void SysTick_Handler(void)
{
uint8_t index;
uint32_t currentTime;
float mxrTemp[3];
sysTickCycleCounter = *DWT_CYCCNT;
sysTickUptime++;
watchDogsTick();
if ((systemReady == true) &&
(cliBusy == false) &&
(accelCalibrating == false) &&
(escCalibrating == false) &&
(magCalibrating == false) &&
(mpu6000Calibrating == false))
{
#ifdef _DTIMING
// LA2_ENABLE;
#endif
frameCounter++;
if (frameCounter > FRAME_COUNT)
frameCounter = 1;
///////////////////////////////
currentTime = micros();
deltaTime1000Hz = currentTime - previous1000HzTime;
previous1000HzTime = currentTime;
readMPU6000();
accelSum500Hz[XAXIS] += rawAccel[XAXIS].value;
accelSum500Hz[YAXIS] += rawAccel[YAXIS].value;
accelSum500Hz[ZAXIS] += rawAccel[ZAXIS].value;
accelSum100Hz[XAXIS] += rawAccel[XAXIS].value;
accelSum100Hz[YAXIS] += rawAccel[YAXIS].value;
accelSum100Hz[ZAXIS] += rawAccel[ZAXIS].value;
gyroSum500Hz[ROLL ] += rawGyro[ROLL ].value;
gyroSum500Hz[PITCH] += rawGyro[PITCH].value;
gyroSum500Hz[YAW ] += rawGyro[YAW ].value;
mxrTemp[XAXIS] = mxr9150XAxis();
mxrTemp[YAXIS] = mxr9150YAxis();
mxrTemp[ZAXIS] = mxr9150ZAxis();
accelSum500HzMXR[XAXIS] += mxrTemp[XAXIS];
accelSum500HzMXR[YAXIS] += mxrTemp[YAXIS];
accelSum500HzMXR[ZAXIS] += mxrTemp[ZAXIS];
accelSum100HzMXR[XAXIS] += mxrTemp[XAXIS];
accelSum100HzMXR[YAXIS] += mxrTemp[YAXIS];
accelSum100HzMXR[ZAXIS] += mxrTemp[ZAXIS];
///////////////////////////////
if ((frameCounter % COUNT_500HZ) == 0)
{
frame_500Hz = true;
for (index = 0; index < 3; index++)
{
accelSummedSamples500Hz[index] = accelSum500Hz[index];
accelSum500Hz[index] = 0;
accelSummedSamples500HzMXR[index] = accelSum500HzMXR[index];
accelSum500HzMXR[index] = 0.0f;
gyroSummedSamples500Hz[index] = gyroSum500Hz[index];
gyroSum500Hz[index] = 0;
}
}
///////////////////////////////
if ((frameCounter % COUNT_100HZ) == 0)
{
frame_100Hz = true;
for (index = 0; index < 3; index++)
{
accelSummedSamples100Hz[index] = accelSum100Hz[index];
accelSum100Hz[index] = 0;
accelSummedSamples100HzMXR[index] = accelSum100HzMXR[index];
accelSum100HzMXR[index] = 0.0f;
}
if (!newTemperatureReading)
{
readTemperatureRequestPressure(MS5611_I2C);
newTemperatureReading = true;
}
else
{
readPressureRequestTemperature(MS5611_I2C);
newPressureReading = true;
}
}
///////////////////////////////
if ((frameCounter % COUNT_50HZ) == 0)
frame_50Hz = true;
///////////////////////////////
if (((frameCounter + 1) % COUNT_10HZ) == 0)
newMagData = readMag(HMC5883L_I2C);
if ((frameCounter % COUNT_10HZ) == 0)
frame_10Hz = true;
///////////////////////////////
if ((frameCounter % COUNT_5HZ) == 0)
frame_5Hz = true;
///////////////////////////////
if ((frameCounter % COUNT_1HZ) == 0)
frame_1Hz = true;
///////////////////////////////////
executionTime1000Hz = micros() - currentTime;
///////////////////////////////
#ifdef _DTIMING
// LA2_DISABLE;
#endif
}
}
void SysTick_Handler(void)
{
uint8_t index;
uint32_t currentTime;
sysTickCycleCounter = *DWT_CYCCNT;
sysTickUptime++;
if ((systemReady == true ) &&
(cliBusy == false) &&
(accelCalibrating == false) &&
(escCalibrating == false) &&
(gyroCalibrating == false) &&
(magCalibrating == false))
{
frameCounter++;
if (frameCounter > FRAME_COUNT)
frameCounter = 1;
///////////////////////////////
currentTime = micros();
deltaTime1000Hz = currentTime - previous1000HzTime;
previous1000HzTime = currentTime;
readAccel();
accelSum200Hz[XAXIS] += rawAccel[XAXIS].value;
accelSum200Hz[YAXIS] += rawAccel[YAXIS].value;
accelSum200Hz[ZAXIS] += rawAccel[ZAXIS].value;
accelSum100Hz[XAXIS] += rawAccel[XAXIS].value;
accelSum100Hz[YAXIS] += rawAccel[YAXIS].value;
accelSum100Hz[ZAXIS] += rawAccel[ZAXIS].value;
readGyro();
gyroSum200Hz[ROLL ] += rawGyro[ROLL ].value;
gyroSum200Hz[PITCH] += rawGyro[PITCH].value;
gyroSum200Hz[YAW ] += rawGyro[YAW ].value;
///////////////////////////////
if ((frameCounter % COUNT_200HZ) == 0)
{
frame_200Hz = true;
for (index = 0; index < 3; index++)
{
accelSummedSamples200Hz[index] = accelSum200Hz[index];
accelSum200Hz[index] = 0.0f;
gyroSummedSamples200Hz[index] = gyroSum200Hz[index];
gyroSum200Hz[index] = 0.0f;
}
}
///////////////////////////////
if ((frameCounter % COUNT_100HZ) == 0)
{
frame_100Hz = true;
for (index = 0; index < 3; index++)
{
accelSummedSamples100Hz[index] = accelSum100Hz[index];
accelSum100Hz[index] = 0.0f;
}
if (frameCounter == COUNT_100HZ)
readTemperatureRequestPressure();
else if (frameCounter == FRAME_COUNT)
readPressureRequestTemperature();
else
readPressureRequestPressure();
pressureSum += uncompensatedPressure.value;
}
///////////////////////////////
if ((frameCounter % COUNT_50HZ) == 0)
{
frame_50Hz = true;
}
///////////////////////////////
if (((frameCounter + 1) % COUNT_10HZ) == 0)
newMagData = readMag();
if ((frameCounter % COUNT_10HZ) == 0)
frame_10Hz = true;
///////////////////////////////
if ((frameCounter % COUNT_5HZ) == 0)
frame_5Hz = true;
///////////////////////////////
if ((frameCounter % COUNT_1HZ) == 0)
frame_1Hz = true;
///////////////////////////////////
executionTime1000Hz = micros() - currentTime;
///////////////////////////////
}
}
int main()
{
int32_t bemf_vals[4] = {0,0,0,0};
int32_t bemf_vals_filt[4] = {0,0,0,0};
init();
// set up DMA/SPI buffers
init_rx_buffer();
init_tx_buffer();
// set up pid structs
pid_struct pid_structs[4];
{ uint8_t i;
for (i = 0; i < 4; ++i) init_pid_struct(&(pid_structs[i]), i);
}
update_dig_pin_configs();
config_adc_in_from_regs();
//debug_printf("starting\n");
int low_volt_alarmed = 0;
// Loop until button is pressed
uint32_t count = 0;
while (1)
{
count += 1;
{
// only sample motor backemf 1/4 of the time
const uint8_t bemf_update_time = (count % 4 == 1);
if (bemf_update_time)
{
// idle breaking
MOT0_DIR1_PORT->BSRRH |= MOT0_DIR1;
MOT0_DIR2_PORT->BSRRH |= MOT0_DIR2;
MOT1_DIR1_PORT->BSRRH |= MOT1_DIR1;
MOT1_DIR2_PORT->BSRRH |= MOT1_DIR2;
MOT2_DIR1_PORT->BSRRH |= MOT2_DIR1;
MOT2_DIR2_PORT->BSRRH |= MOT2_DIR2;
MOT3_DIR1_PORT->BSRRH |= MOT3_DIR1;
MOT3_DIR2_PORT->BSRRH |= MOT3_DIR2;
}
// let the motor coast
// delay_us(700);
// now I squeeze in most sensor updates instead of just sleeping
uint32_t before = usCount;
update_dig_pins();
int16_t batt = adc_update();
readAccel();
readMag();
readGyro();
if (batt < 636) // about 5.75 volts
{
configDigitalOutPin(LED1_PIN, LED1_PORT);
low_volt_alarmed = 1;
if (count % 50 < 10) // low duty cycle to save battery
{
LED1_PORT->BSRRL |= LED1_PIN; // ON
}
else
{
LED1_PORT->BSRRH |= LED1_PIN; // OFF
}
}
else if (low_volt_alarmed)
{
// make sure led is off coming out of the low voltage alarm
configDigitalOutPin(LED1_PIN, LED1_PORT);
LED1_PORT->BSRRH |= LED1_PIN; // OFF
low_volt_alarmed = 0;
}
if (adc_dirty)
{
adc_dirty = 0;
config_adc_in_from_regs();
}
if (dig_dirty)
{
dig_dirty = 0;
update_dig_pin_configs();
}
uint32_t sensor_update_time = usCount - before;
const uint16_t us_delay_needed = 700;
const uint8_t got_time_to_burn = sensor_update_time < us_delay_needed;
if (got_time_to_burn)
{
// sleep the remainder of the coasting period before sampling bemf
delay_us(us_delay_needed-sensor_update_time);
}
if (bemf_update_time)
{
update_bemfs(bemf_vals, bemf_vals_filt);
// set the motor directions back up
update_motor_modes();
uint8_t channel;
for (channel = 0; channel < 4; ++channel)
{
uint8_t shift = 2*channel;
uint8_t motor_mode = (aTxBuffer[REG_RW_MOT_MODES] & (0b11 << shift)) >> shift;
motor_update(bemf_vals[channel], bemf_vals_filt[channel], &pid_structs[channel], channel, motor_mode);
}
}
else
{
// updating sensors doesnt' take as long as all of the adc for backemf updates
// sleep a bit so that our time through the loop is consistent for PID control
delay_us(222);
}
}
}