static inline void attitude_loop( void ) { #ifdef USE_GYRO gyro_update(); #endif #ifdef USE_INFRARED infrared_update(); estimator_update_state_infrared(); #endif /* USE_INFRARED */ h_ctl_attitude_loop(); /* Set h_ctl_aileron_setpoint & h_ctl_elevator_setpoint */ v_ctl_throttle_slew(); ap_state->commands[COMMAND_THROTTLE] = v_ctl_throttle_slewed; ap_state->commands[COMMAND_ROLL] = h_ctl_aileron_setpoint; ap_state->commands[COMMAND_PITCH] = h_ctl_elevator_setpoint; #if defined MCU_SPI_LINK link_mcu_send(); #elif defined INTER_MCU && defined SINGLE_MCU /**Directly set the flag indicating to FBW that shared buffer is available*/ inter_mcu_received_ap = TRUE; #endif }
mraa_result_t gyro_init(int bus) { //init bus and reset chip gyro_context = mraa_i2c_init(bus); mraa_i2c_address(gyro_context, ITG3200_I2C_ADDR); mraa_result_t result; gyro_buffer[0] = ITG3200_PWR_MGM; gyro_buffer[1] = ITG3200_RESET; result = mraa_i2c_write(gyro_context, gyro_buffer, 2); if (result != MRAA_SUCCESS) { printError("unable to write to gyro (4)"); return result; } gyro_calibrate(); gyro_update(); return MRAA_SUCCESS; }
void gyro_calibrate() { int reads = 600; int delay = 4000; // 4 milliseconds int skip = 5; // initial samples to skip int temp[3] = {0}; int i, j; for (i = 0; i < reads; i++) { gyro_update(); if (i > skip) { for (j = 0; j < 3; j++) { temp[j] += gyro_rotation[j]; } } usleep(delay); } for(i = 0; i < 3; i++) { gyro_offsets[i] = (-1) * temp[i] / (reads - skip); } }
void periodic_task_ap( void ) { static uint8_t _20Hz = 0; static uint8_t _10Hz = 0; static uint8_t _4Hz = 0; static uint8_t _1Hz = 0; _20Hz++; if (_20Hz>=3) _20Hz=0; _10Hz++; if (_10Hz>=6) _10Hz=0; _4Hz++; if (_4Hz>=15) _4Hz=0; _1Hz++; if (_1Hz>=60) _1Hz=0; reporting_task(); if (!_1Hz) { if (estimator_flight_time) estimator_flight_time++; #if defined DATALINK || defined SITL datalink_time++; #endif static uint8_t t = 0; if (vsupply < LOW_BATTERY_DECIVOLT) t++; else t = 0; kill_throttle |= (t >= LOW_BATTERY_DELAY); kill_throttle |= launch && (dist2_to_home > Square(KILL_MODE_DISTANCE)); } switch(_4Hz) { case 0: #ifdef SITL #ifdef GPS_TRIGGERED_FUNCTION GPS_TRIGGERED_FUNCTION(); #endif #endif estimator_propagate_state(); #ifdef EXTRA_DOWNLINK_DEVICE DOWNLINK_SEND_ATTITUDE(ExtraPprzTransport,&estimator_phi,&estimator_psi,&estimator_theta); #endif navigation_task(); break; case 1: if (!estimator_flight_time && estimator_hspeed_mod > MIN_SPEED_FOR_TAKEOFF) { estimator_flight_time = 1; launch = TRUE; /* Not set in non auto launch */ DOWNLINK_SEND_TAKEOFF(DefaultChannel, &cpu_time_sec); default: break; } break; #ifdef USE_GPIO case 3: GpioUpdate1(); break; #endif /* default: */ } #ifndef CONTROL_RATE #define CONTROL_RATE 20 #endif #if CONTROL_RATE != 60 && CONTROL_RATE != 20 #error "Only 20 and 60 allowed for CONTROL_RATE" #endif #ifdef USE_ANALOG_IMU if (!_20Hz) { imu_periodic(); } #endif // USE_ANALOG_IMU #if CONTROL_RATE == 20 if (!_20Hz) #endif { #ifdef USE_GYRO gyro_update(); #endif #ifdef USE_INFRARED infrared_update(); estimator_update_state_infrared(); #endif /* USE_INFRARED */ h_ctl_attitude_loop(); /* Set h_ctl_aileron_setpoint & h_ctl_elevator_setpoint */ v_ctl_throttle_slew(); ap_state->commands[COMMAND_THROTTLE] = v_ctl_throttle_slewed; ap_state->commands[COMMAND_ROLL] = h_ctl_aileron_setpoint; ap_state->commands[COMMAND_PITCH] = h_ctl_elevator_setpoint; #if defined MCU_SPI_LINK link_mcu_send(); #elif defined INTER_MCU && defined SINGLE_MCU /**Directly set the flag indicating to FBW that shared buffer is available*/ inter_mcu_received_ap = TRUE; #endif } modules_periodic_task(); }