void loop() { long t = millis(); // put your main code here, to run repeatedly: droid.exec(t); #ifdef hal_beeps beeps.exec(t); #endif #ifdef hal_ir ir_update(); #endif #ifdef hal_tft // read joystick int ana[2]; ana[0] = 1024 - analogRead(A1); ana[1] = analogRead(A2); int b0 = (digitalRead(A3) == LOW); droid.update_joystick(ana,b0,t); droid.update_display(t); #endif #ifdef hal_compass int v[3]; compass.get_vector(v); for(byte i=0; i<3; i++) droid.set_signal(124+i, v[i]); /* Serial.print(v[0]); Serial.print(','); Serial.print(v[1]); Serial.print(','); Serial.print(v[2]); Serial.print("\n"); delay(100); */ #endif }
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 #if CONTROL_RATE == 20 if (!_20Hz) #endif { #ifdef USE_GYRO gyro_update(); #endif #ifdef USE_INFRARED ir_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(); }
void vTask_6() { ir_update(); estimator_update_state_infrared(); roll_pitch_pid_run(); }