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;
}
}
}
