void pwm_meas_init( void ) { pwm_input_init(); }
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; } } }