/**
* @brief Handler for interrupts on port A
*/
void PORTA_Handler()
{
#if ENABLE_MMA8451Q
register uint32_t isfr_mma = MMA8451Q_INT_PORT->ISFR;
/* check MMA8451Q */
register uint32_t fromMMA8451Q = (isfr_mma & ((1 << MMA8451Q_INT1_PIN) | (1 << MMA8451Q_INT2_PIN)));
if (fromMMA8451Q || fromMPU6050)
{
poll_mma8451q = 1;
LED_RedOn();
/* clear interrupts using BME decorated logical OR store
* PORTA->ISFR |= (1 << MMA8451Q_INT1_PIN) | (1 << MMA8451Q_INT2_PIN);
*/
BME_OR_W(&MMA8451Q_INT_PORT->ISFR, (1 << MMA8451Q_INT1_PIN) | (1 << MMA8451Q_INT2_PIN));
}
#endif
/* check MPU6050 */
register uint32_t isfr_mpu = MPU6050_INT_PORT->ISFR;
register uint32_t fromMPU6050 = (isfr_mpu & (1 << MPU6050_INT_PIN));
if (fromMPU6050)
{
poll_mpu6050 = 1;
LED_BlueOn();
/* clear interrupts using BME decorated logical OR store
* PORTA->ISFR |= (1 << MPU6050_INT_PIN);
*/
BME_OR_W(&MMA8451Q_INT_PORT->ISFR, (1 << MPU6050_INT_PIN));
}
}
static void USB_CCID_GetDataFromUSB (void)
{
int i;
int USB_Datalen_s32;
// Bool cbw_error;
Usb_reset_endpoint_fifo_access (EP_CCID_OUT);
// Get all data from USB
USB_CCID_data_st.CCID_datalen = 0;
while (Is_usb_out_received (EP_CCID_OUT))
{
Usb_reset_endpoint_fifo_access (EP_CCID_OUT);
USB_Datalen_s32 = Usb_byte_count (EP_CCID_OUT);
USB_Log_st.CCID_WriteCalls_u32++;
USB_Log_st.CCID_BytesWrite_u32 += USB_Datalen_s32;
// CI_TickLocalPrintf ("Get CCID USB block %3d byte - %3d\n",USB_Datalen_s32,USB_CCID_data_st.CCID_datalen);
if (CCID_MAX_XFER_LENGTH <= USB_Datalen_s32 + USB_CCID_data_st.CCID_datalen) // Check for oversize
{
CI_LocalPrintf ("*** CCID buffer to small %d ***\n", CCID_MAX_XFER_LENGTH);
Usb_ack_out_received_free (EP_CCID_OUT);
return;
}
for (i = 0; i < USB_Datalen_s32; i++)
{
USB_CCID_data_st.USB_data[USB_CCID_data_st.CCID_datalen] = Usb_read_endpoint_data (EP_CCID_OUT, 8);
USB_CCID_data_st.CCID_datalen++;
}
Usb_ack_out_received_free (EP_CCID_OUT);
DelayMs (1);
}
LED_RedOn ();
// USB_CCID_Datalen_s32 = USB_CCID_data_st.CCID_datalen;
USB_to_CRD_DispatchUSBMessage_v (&USB_CCID_data_st);
// memset (USB_CCID_data_st.USB_data,0,USB_Datalen_s32);
LED_RedOff ();
// Usb_ack_out_received_free(EP_CCID_OUT);
}
int main(void)
{
/* initialize the core clock and the systick timer */
InitClock();
InitSysTick();
/* initialize the RGB led */
LED_Init();
/* Initialize UART0 */
InitUart0();
/* double rainbow all across the sky */
DoubleFlash();
/* initialize the I2C bus */
I2C_Init();
#if DATA_FUSE_MODE
/* signaling for fusion */
FusionSignal_Init();
#endif // DATA_FUSE_MODE
/* initialize UART fifos */
RingBuffer_Init(&uartInputFifo, &uartInputData, UART_RX_BUFFER_SIZE);
RingBuffer_Init(&uartOutputFifo, &uartOutputData, UART_TX_BUFFER_SIZE);
/* initialize UART0 interrupts */
Uart0_InitializeIrq(&uartInputFifo, &uartOutputFifo);
Uart0_EnableReceiveIrq();
/* initialize I2C arbiter */
InitI2CArbiter();
/* initialize the IMUs */
InitHMC5883L();
InitMPU6050();
// InitMPU6050();
#if ENABLE_MMA8451Q
InitMMA8451Q();
#endif
/* Wait for the config messages to get flushed */
//TrafficLight();
DoubleFlash();
RingBuffer_BlockWhileNotEmpty(&uartOutputFifo);
#if ENABLE_MMA8451Q
/* initialize the MMA8451Q data structure for accelerometer data fetching */
mma8451q_acc_t acc;
MMA8451Q_InitializeData(&acc);
#endif
/* initialize the MPU6050 data structure */
mpu6050_sensor_t accgyrotemp, previous_accgyrotemp;
MPU6050_InitializeData(&accgyrotemp);
MPU6050_InitializeData(&previous_accgyrotemp);
/* initialize the HMC5883L data structure */
hmc5883l_data_t compass, previous_compass;
HMC5883L_InitializeData(&compass);
HMC5883L_InitializeData(&previous_compass);
/* initialize HMC5883L reading */
uint32_t lastHMCRead = 0;
const uint32_t readHMCEvery = 1000 / 75; /* at 75Hz, data come every (1000/75Hz) ms. */
/************************************************************************/
/* Fetch scaler values */
/************************************************************************/
#if DATA_FUSE_MODE
const fix16_t mpu6050_accelerometer_scaler = mpu6050_accelerometer_get_scaler();
const fix16_t mpu6050_gyroscope_scaler = mpu6050_gyroscope_get_scaler();
const fix16_t hmc5883l_magnetometer_scaler = hmc5883l_magnetometer_get_scaler();
#endif // DATA_FUSE_MODE
/************************************************************************/
/* Prepare data fusion */
/************************************************************************/
#if DATA_FUSE_MODE
uint32_t last_transmit_time = 0;
uint32_t last_fusion_time = systemTime();
fusion_initialize();
#endif // DATA_FUSE_MODE
/************************************************************************/
/* Main loop */
/************************************************************************/
for(;;)
{
/* helper variables to track data freshness */
uint_fast8_t have_gyro_data = 0;
uint_fast8_t have_acc_data = 0;
uint_fast8_t have_mag_data = 0;
/************************************************************************/
/* Determine if sensor data fetching is required */
/************************************************************************/
/* helper variables for event processing */
int eventsProcessed = 0;
int readMPU, readHMC;
#if ENABLE_MMA8451Q
int readMMA;
#endif
/* atomic detection of fresh data */
__disable_irq();
#if ENABLE_MMA8451Q
readMMA = poll_mma8451q;
#endif
readMPU = poll_mpu6050;
poll_mma8451q = 0;
poll_mpu6050 = 0;
__enable_irq();
/* detection of HMC read */
/*
* TODO: read synchronized with MPU
*/
readHMC = 0;
uint32_t time = systemTime();
if ((time - lastHMCRead) >= readHMCEvery)
{
readHMC = 1;
lastHMCRead = time;
}
/************************************************************************/
/* Fetching MPU6050 sensor data if required */
/************************************************************************/
/* read accelerometer/gyro */
if (readMPU)
{
LED_BlueOff();
I2CArbiter_Select(MPU6050_I2CADDR);
MPU6050_ReadData(&accgyrotemp);
/* mark event as detected */
eventsProcessed = 1;
/* check for data freshness */
have_acc_data = (accgyrotemp.accel.x != previous_accgyrotemp.accel.x)
|| (accgyrotemp.accel.y != previous_accgyrotemp.accel.y)
|| (accgyrotemp.accel.z != previous_accgyrotemp.accel.z);
have_gyro_data = (accgyrotemp.gyro.x != previous_accgyrotemp.gyro.x)
|| (accgyrotemp.gyro.y != previous_accgyrotemp.gyro.y)
|| (accgyrotemp.gyro.z != previous_accgyrotemp.gyro.z);
/* loop current data --> previous data */
previous_accgyrotemp = accgyrotemp;
}
/************************************************************************/
/* Fetching HMC5883L sensor data if required */
/************************************************************************/
/* read compass data */
if (readHMC)
{
I2CArbiter_Select(HMC5883L_I2CADDR);
HMC5883L_ReadData(&compass);
/* mark event as detected */
eventsProcessed = 1;
/* check for data freshness */
have_mag_data = (compass.x != previous_compass.x)
|| (compass.y != previous_compass.y)
|| (compass.z != previous_compass.z);
/* loop current data --> previous data */
previous_compass = compass;
}
/************************************************************************/
/* Fetching MMA8451Q sensor data if required */
/************************************************************************/
#if ENABLE_MMA8451Q
/* read accelerometer */
if (readMMA)
{
LED_RedOff();
I2CArbiter_Select(MMA8451Q_I2CADDR);
MMA8451Q_ReadAcceleration14bitNoFifo(&acc);
/* mark event as detected */
eventsProcessed = 1;
}
#endif
/************************************************************************/
/* Raw sensor data output over serial */
/************************************************************************/
#if DATA_FETCH_MODE
/* data availability + sanity check
* This sent me on a long bug hunt: Sometimes the interrupt would be raised
* even if not all data registers were written. This always resulted in a
* z data register not being fully written which, in turn, resulted in
* extremely jumpy measurements.
*/
if (readMPU && accgyrotemp.status != 0)
{
/* write data */
uint8_t type = 0x02;
P2PPE_TransmissionPrefixed(&type, 1, (uint8_t*)accgyrotemp.data, sizeof(accgyrotemp.data), IO_SendByte);
}
/* data availability + sanity check */
if (readHMC && (compass.status & HMC5883L_SR_RDY_MASK) != 0) /* TODO: check if not in lock state */
{
uint8_t type = 0x03;
P2PPE_TransmissionPrefixed(&type, 1, (uint8_t*)compass.xyz, sizeof(compass.xyz), IO_SendByte);
}
#if ENABLE_MMA8451Q
/* data availability + sanity check */
if (readMMA && acc.status != 0)
{
uint8_t type = 0x01;
P2PPE_TransmissionPrefixed(&type, 1, (uint8_t*)acc.xyz, sizeof(acc.xyz), IO_SendByte);
}
#endif
#endif // DATA_FETCH_MODE
/************************************************************************/
/* Sensor data fusion */
/************************************************************************/
#if DATA_FUSE_MODE
// if there were sensor data ...
if (eventsProcessed)
{
v3d gyro, acc, mag;
// convert, calibrate and store gyroscope data
if (have_gyro_data)
{
sensor_prepare_mpu6050_gyroscope_data(&gyro, accgyrotemp.gyro.x, accgyrotemp.gyro.y, accgyrotemp.gyro.z, mpu6050_gyroscope_scaler);
fusion_set_gyroscope_v3d(&gyro);
}
// convert, calibrate and store accelerometer data
if (have_acc_data)
{
sensor_prepare_mpu6050_accelerometer_data(&acc, accgyrotemp.accel.x, accgyrotemp.accel.y, accgyrotemp.accel.z, mpu6050_accelerometer_scaler);
fusion_set_accelerometer_v3d(&acc);
}
// convert, calibrate and store magnetometer data
if (have_mag_data)
{
sensor_prepare_hmc5883l_data(&mag, compass.x, compass.y, compass.z, hmc5883l_magnetometer_scaler);
fusion_set_magnetometer_v3d(&mag);
}
// get the time differential
const uint32_t current_time = systemTime();
const fix16_t deltaT_ms = fix16_from_int(current_time - last_fusion_time);
const fix16_t deltaT = fix16_mul(deltaT_ms, F16(0.001));
last_fusion_time = current_time;
FusionSignal_Predict();
// predict the current measurements
fusion_predict(deltaT);
FusionSignal_Update();
// correct the measurements
fusion_update(deltaT);
FusionSignal_Clear();
#if 0
fix16_t yaw, pitch, roll;
fusion_fetch_angles(&roll, &pitch, &yaw);
#if 0
float yawf = fix16_to_float(yaw),
pitchf = fix16_to_float(pitch),
rollf = fix16_to_float(roll);
IO_SendInt16((int16_t)yawf);
IO_SendInt16((int16_t)pitchf);
IO_SendInt16((int16_t)rollf);
IO_SendByteUncommited('\r');
IO_SendByte('\n');
#else
if (current_time - last_transmit_time >= 100)
{
/* write data */
uint8_t type = 42;
fix16_t buffer[3] = { roll, pitch, yaw };
P2PPE_TransmissionPrefixed(&type, 1, (uint8_t*)buffer, sizeof(buffer), IO_SendByte);
last_transmit_time = current_time;
}
#endif
#else
if (current_time - last_transmit_time >= 100)
{
/* write data */
switch (output_mode)
{
case RPY:
{
fix16_t roll, pitch, yaw;
fusion_fetch_angles(&roll, &pitch, &yaw);
/* write data */
uint8_t type = 42;
fix16_t buffer[3] = { roll, pitch, yaw };
P2PPE_TransmissionPrefixed(&type, 1, (uint8_t*)buffer, sizeof(buffer), IO_SendByte);
break;
}
case QUATERNION:
{
qf16 orientation;
fusion_fetch_quaternion(&orientation);
uint8_t type = 43;
fix16_t buffer[4] = { orientation.a, orientation.b, orientation.c, orientation.d };
P2PPE_TransmissionPrefixed(&type, 1, (uint8_t*)buffer, sizeof(buffer), IO_SendByte);
break;
}
case QUATERNION_RPY:
{
fix16_t roll, pitch, yaw;
fusion_fetch_angles(&roll, &pitch, &yaw);
qf16 orientation;
fusion_fetch_quaternion(&orientation);
uint8_t type = 44;
fix16_t buffer[7] = { orientation.a, orientation.b, orientation.c, orientation.d, roll, pitch, yaw };
P2PPE_TransmissionPrefixed(&type, 1, (uint8_t*)buffer, sizeof(buffer), IO_SendByte);
break;
}
case SENSORS_RAW:
{
uint8_t type = 0;
fix16_t buffer[6] = { acc.x, acc.y, acc.z, mag.x, mag.y, mag.z };
P2PPE_TransmissionPrefixed(&type, 1, (uint8_t*)buffer, sizeof(buffer), IO_SendByte);
break;
}
}
last_transmit_time = current_time;
}
#endif
}
#endif // DATA_FUSE_MODE
/************************************************************************/
/* Read user data input */
/************************************************************************/
/* as long as there is data in the buffer */
while(!RingBuffer_Empty(&uartInputFifo))
{
/* light one led */
LED_RedOn();
/* fetch byte */
uint8_t data = IO_ReadByte();
output_mode = (output_mode_t)data;
LED_RedOff();
#if 0
/* echo to output */
IO_SendByte(data);
/* mark event as detected */
eventsProcessed = 1;
#endif
}
/************************************************************************/
/* Save energy if you like to */
/************************************************************************/
/* in case of no events, allow a sleep */
if (!eventsProcessed)
{
/*
* Care must be taken with this instruction here, as it can lead
* to a condition where after being woken up (e.g. by the SysTick)
* and looping through, immediately before entering WFI again
* an interrupt would yield a true condition for the branches below.
* In this case this loop would be blocked until the next IRQ,
* which, in case of a 1ms SysTick timer, could be too late.
*
* To counter this behaviour, SysTick has been speed up by factor
* four (0.25ms).
*/
#if 0
__WFI();
#endif
}
}
return 0;
}