STATIC_INLINE void main_periodic(void)
{
#if USE_IMU
imu_periodic();
#endif
//FIXME: temporary hack, remove me
#ifdef InsPeriodic
InsPeriodic();
#endif
/* run control loops */
autopilot_periodic();
/* set actuators */
//actuators_set(autopilot_motors_on);
SetActuatorsFromCommands(commands, autopilot_mode);
if (autopilot_in_flight) {
RunOnceEvery(PERIODIC_FREQUENCY, autopilot_flight_time++);
}
#if defined DATALINK || defined SITL
RunOnceEvery(PERIODIC_FREQUENCY, datalink_time++);
#endif
RunOnceEvery(10, LED_PERIODIC());
}
STATIC_INLINE void main_periodic( void ) {
imu_periodic();
/* run control loops */
autopilot_periodic();
/* set actuators */
//actuators_set(autopilot_motors_on);
SetActuatorsFromCommands(commands, autopilot_mode);
if (autopilot_in_flight) {
RunOnceEvery(PERIODIC_FREQUENCY, { autopilot_flight_time++; datalink_time++; });
}
void analog_imu_offset_set( void ) {
uint8_t i;
// read IMU
imu_periodic();
for(i = 0; i < NB_ANALOG_IMU_ADC; i++) {
analog_imu_offset[i] = analog_imu_values[i];
}
// Z channel should read
analog_imu_offset[5] += (9.81f / IMU_ACCEL_Z_SENS);
}
void analog_imu_update( void ) {
uint8_t i;
// read IMU
imu_periodic();
for(i = 0; i < NB_ANALOG_IMU_ADC; i++) {
adc_average[i] -= analog_imu_offset[i];
}
accel2ms2();
gyro2rads();
}
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_task(void)
{
/* Simply set current roll/pitch as commands.
* Scale DEMO_MAX_ROLL/PITCH to MAX_PPRZ (the max commands)
*/
commands[COMMAND_ROLL] = stateGetNedToBodyEulers_f()->phi * MAX_PPRZ / DEMO_MAX_ROLL;
commands[COMMAND_PITCH] = stateGetNedToBodyEulers_f()->theta * MAX_PPRZ / DEMO_MAX_ROLL;
/* generated macro from airframe file, seconds AP_MODE param not used */
SetActuatorsFromCommands(commands, 0);
if (sys_time.nb_sec > 1) {
imu_periodic();
}
RunOnceEvery(10, { LED_PERIODIC();});
static inline void main_periodic(void) {
uint16_t v1 = 123;
uint16_t v2 = 123;
imu_periodic();
#ifdef PASSTHROUGH_CYGNUS
autopilot_periodic();
#endif
OveroLinkPeriodic(on_overo_link_lost);
RunOnceEvery(10, {
LED_PERIODIC();
DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);
radio_control_periodic();
check_radio_lost();
DOWNLINK_SEND_BARO_RAW(DefaultChannel, &baro.absolute, &baro.differential);
});
/** Run at PERIODIC_FREQUENCY (60Hz if not defined) */
void sensors_task( void ) {
#if USE_IMU
imu_periodic();
#if USE_AHRS
if (ahrs_timeout_counter < 255)
ahrs_timeout_counter ++;
#endif // USE_AHRS
#endif // USE_IMU
//FIXME: this is just a kludge
#if USE_AHRS && defined SITL
ahrs_propagate();
#endif
#if USE_INS
ins_periodic_task();
#endif
}
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 */ \
}, \
{ \
void periodic_task_ap( void ) {
static uint8_t _60Hz = 0;
static uint8_t _20Hz = 0;
static uint8_t _10Hz = 0;
static uint8_t _4Hz = 0;
static uint8_t _1Hz = 0;
#ifdef USE_IMU
// Run at PERIODIC_FREQUENCY (60Hz if not defined)
imu_periodic();
#endif // USE_IMU
#define _check_periodic_freq_ PERIODIC_FREQUENCY % 60
#if _check_periodic_freq_
#error Using HighSpeed Periodic: PERIODIC_FREQUENCY has to be a multiple of 60!
#endif
_60Hz++;
if (_60Hz >= (PERIODIC_FREQUENCY / 60))
{
_60Hz = 0;
}
else
{
return;
}
// Rest of the periodic function still runs at 60Hz like always
_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
{
#ifndef AHRS_TRIGGERED_ATTITUDE_LOOP
attitude_loop();
#endif
}
modules_periodic_task();
}
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();
}
static inline void main_periodic_task( void ) {
if (cpu_time_sec > 1) imu_periodic();
RunOnceEvery(10, { LED_PERIODIC();});
static inline void main_periodic_task(void)
{
if (sys_time.nb_sec > 1) {
imu_periodic();
}
RunOnceEvery(10, { LED_PERIODIC();});
