//------------Read magnetometer when ready-------------//
uint8_t LSM9DS1::read_mag_DRDY(uint32_t timeout){
uint32_t now = micros();
while((micros() - now) < timeout){
if (digitalRead(_DRDY_pin_M)){
read_raw_mag();
return 1;
}
if ((micros() - now) < 0){
now = 0L;
}
}
return 0;
}
//---------Read data when ready (STATUS register)-----------//
uint8_t LSM9DS1::read_mag_STATUS(uint32_t timeout){
uint32_t now = micros();
while((micros() - now) < timeout){
uint8_t STATUS_val = readRegister(_chipSelectPin_M, LSM9DS1_STATUS_REG_M);
if (STATUS_val & (1 << 3)){
read_raw_mag();
return 1;
}
if ((micros() - now) < 0){
now = 0L;
}
}
return 0;
}
void gyro_data_ready_irq(void)
{
static uint32_t last_us = 0;
uint32_t cur_us = current_us();
static __IO uint32_t d_us;
static __IO uint32_t d_us2;
d_us = cur_us - last_us;
last_us = cur_us;
if(current_mode == DT_ATT)
{
int16_t gyro[3],acc[3],mag[3];
read_raw_gyro(gyro);
read_raw_acc(acc);
read_raw_mag(mag);
gyro_hungry = 0;
sensors.gyroSum[ROLL] += gyro[1];
sensors.gyroSum[PITCH] += gyro[0];
sensors.gyroSum[YAW] += gyro[2];
sensors.accSum[XAXIS] += acc[0];
sensors.accSum[YAXIS] += acc[1];
sensors.accSum[ZAXIS] += acc[2];
sensors.magSum[XAXIS] += mag[0];
sensors.magSum[YAXIS] += mag[1];
sensors.magSum[ZAXIS] += mag[2];
sensors.countSum++;
if( sensors.countSum>= SUM_COUNT ){
uint32_t cur_us = current_us();
memcpy(sensors.gyroSumed,sensors.gyroSum,4*3*3);
memset(sensors.gyroSum,0,4*3*3);
sensors.countSumed = sensors.countSum;
sensors.countSum = 0;
sensor_data_ready = 1;
if(sensors.lastSumTime_us){
sensors.sumTime_us = cur_us - sensors.lastSumTime_us;
}
sensors.lastSumTime_us = cur_us;
}
}
d_us2 = current_us() - last_us;
d_us2++;
}
//-------------------Discard measures----------------------//
uint8_t LSM9DS1::discard_measures_mag(uint8_t number_of_measures, uint32_t timeout){
uint8_t count = 0;
uint32_t now = micros();
while (count < (number_of_measures * 0.5)){
uint8_t STATUS_value = status_mag();
if (STATUS_value & (1 << 7)){
read_raw_mag();
now = micros();
count++;
}
if ((micros() - now) > timeout){
return 0;
}
else if ((micros() - now) < 0){
now = 0L;
}
}
return 1;
}
int main(void)
{
/* 2 bit for pre-emption priority, 2 bits for subpriority */
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
setup_systick();
enable_tick_count();
setup_io_leds();
setup_io_usb();
init_sensor_config();
GYRO_INIT();
ACC_INIT();
MAG_INIT();
nrf_init();
nrf_detect();
nrf_rx_mode_dual(nrf_addr, 5, 40);
{
uint8_t status = nrf_read_reg(NRF_STATUS);
nrf_write_reg(NRF_FLUSH_RX, 0xff);
nrf_write_reg(NRF_FLUSH_TX, 0xff);
nrf_write_reg(NRF_WRITE_REG|NRF_STATUS,status); // clear IRQ flags
}
pwm_input_init();
USB_Init();
acc_scale_factor = calc_acc_scale(200);
compute_gyro_runtime_bias(sensors.gyro_rt_bias, 1000);
// wait usb ready
//while ((bDeviceState != CONFIGURED)&&(USBConnectTimeOut != 0))
//{}
current_mode = DT_ATT;
// endless loop
while(1)
{
uint8_t buf[64];
if(frame_100Hz){
frame_100Hz = 0;
buf[0] = 0;
if(current_mode == DT_RCDATA){
prepare_rc_data(buf);
usb_send_data(buf,64);
}else if(current_mode == DT_SENSOR){
buf[0] = DT_SENSOR;
buf[1] = 9;
read_raw_gyro((int16_t*)(buf+2));
read_raw_acc((int16_t*)(buf+8));
read_raw_mag((int16_t*)(buf+14));
usb_send_data(buf,64);
}
if(buf[0]){
usb_send_data(buf,64);
}
}
if(sensor_data_ready){
sensor_data_ready = 0;
if(sensors.sumTime_us){
update_AHRS();
if(current_mode == DT_ATT){
buf[0] = DT_ATT;
buf[1] = 3;
sensors.height = 0.0;
memcpy(buf+2,sensors.attitude,sizeof(sensors.attitude) + 4);
usb_send_data(buf,64);
}
LED4_TOGGLE;
LED5_TOGGLE;
LED10_TOGGLE;
}
// process sensor data
}
if(frame_200Hz){
frame_200Hz = 0;
// if L3GD20 already contains gyro data, rising edge will not occur
if( (current_mode == DT_ATT) && (gyro_hungry>1) ){
if(L3GD20_INT2){
int16_t gyro[3];
read_raw_gyro(gyro);
}
}
if(gyro_hungry < 10)
gyro_hungry++;
}
if(frame_1Hz){
frame_1Hz = 0;
LED3_TOGGLE;
}
}
}