static inline void main_periodic_task( void ) {
uint8_t i;
static uint16_t cnt = 0;
if (++cnt == SERVO_SPEED)
{
/* Move all servos forward or backward by dval */
for (i = 0; i < actuators_get_num(ACTUATOR_BANK_SERVOS); i++)
{
Servo_t *servo = &servos[i];
/* reverse servos when they get to the end */
if (servo->val == SERVO_MAX && servo->dval == 1)
servo->dval = -1;
else if (servo->val == SERVO_MIN)
servo->dval = 1;
/* move and set servo */
servo->val += servo->dval;
#if SERVO_FIXED_VALUE > 0
actuators_set(ACTUATOR_BANK_SERVOS | i, SERVO_FIXED_VALUE);
#else
actuators_set(ACTUATOR_BANK_SERVOS | i, servo->val);
#endif
}
actuators_commit(ACTUATOR_BANK_SERVOS);
cnt = 0;
}
}
static inline void main_periodic_task( void ) {
commands[COMMAND_ROLL]=0;
commands[COMMAND_PITCH]=0;
commands[COMMAND_YAW]=0;
commands[COMMAND_THRUST]=1;
actuators_set(TRUE);
LED_PERIODIC();
}
STATIC_INLINE void main_periodic( void ) {
imu_periodic();
/* run control loops */
autopilot_periodic();
/* set actuators */
actuators_set(autopilot_motors_on);
modules_periodic_task();
if (autopilot_in_flight) {
RunOnceEvery(PERIODIC_FREQUENCY, { autopilot_flight_time++; datalink_time++; });
}
STATIC_INLINE void main_periodic( void ) {
imu_periodic();
/* run control loops */
autopilot_periodic();
/* set actuators */
actuators_set(autopilot_motors_on);
PeriodicPrescaleBy10( \
{ \
radio_control_periodic_task(); \
if (radio_control.status != RC_OK && \
autopilot_mode != AP_MODE_KILL && \
autopilot_mode != AP_MODE_NAV) \
autopilot_set_mode(AP_MODE_FAILSAFE); \
}, \
{ \
/* booz_fms_periodic(); FIXME */ \
}, \
{ \
