static inline void main_periodic_task( void ) {
servos[0]+=10;
servos[1]+=10;
servos[2]+=10;
servos[3]+=10;
if ((can_error_warning = CAN_GetFlagStatus(CAN1, CAN_FLAG_EWG)) == SET) {
LED_ON(2);
} else {
LED_OFF(2);
}
if ((can_error_passive = CAN_GetFlagStatus(CAN1, CAN_FLAG_EPV)) == SET) {
LED_ON(3);
} else {
LED_OFF(3);
}
if ((can_bus_off = CAN_GetFlagStatus(CAN1, CAN_FLAG_BOF)) == SET) {
LED_ON(0);
} else {
LED_OFF(0);
}
cscp_transmit(0, 0, (uint8_t *)servos, 8);
LED_PERIODIC();
DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);
}
void handle_periodic_tasks_ap(void) {
if (sys_time_check_and_ack_timer(sensors_tid))
sensors_task();
if (sys_time_check_and_ack_timer(navigation_tid))
navigation_task();
#ifndef AHRS_TRIGGERED_ATTITUDE_LOOP
if (sys_time_check_and_ack_timer(attitude_tid))
attitude_loop();
#endif
if (sys_time_check_and_ack_timer(modules_tid))
modules_periodic_task();
if (sys_time_check_and_ack_timer(monitor_tid))
monitor_task();
if (sys_time_check_and_ack_timer(telemetry_tid)) {
reporting_task();
LED_PERIODIC();
}
}
STATIC_INLINE void main_periodic(void)
{
#if INTER_MCU_AP
/* Inter-MCU watchdog */
intermcu_periodic();
#endif
/* run control loops */
autopilot_periodic();
/* set actuators */
//actuators_set(autopilot_motors_on);
#ifndef INTER_MCU_AP
SetActuatorsFromCommands(commands, autopilot_mode);
#else
intermcu_set_actuators(commands, autopilot_mode);
#endif
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)
{
#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)
{
SetActuatorsFromCommands(commands, 0);
LED_PERIODIC();
RunOnceEvery(512, {DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);});
void handle_periodic_tasks_ap(void)
{
if (sys_time_check_and_ack_timer(sensors_tid))
sensors_task();//imu数据读取
if (sys_time_check_and_ack_timer(navigation_tid))
navigation_task();//估算期望的航线，故障保护
#ifndef AHRS_TRIGGERED_ATTITUDE_LOOP
if (sys_time_check_and_ack_timer(attitude_tid))
attitude_loop();//姿态循环
#endif
if (sys_time_check_and_ack_timer(modules_tid))
modules_periodic_task();//没有该函数说明
if (sys_time_check_and_ack_timer(monitor_tid))
monitor_task();//监听
if (sys_time_check_and_ack_timer(telemetry_tid))
{
reporting_task();//汇报
LED_PERIODIC();
}
}
static inline void main_periodic_task( void ) {
// LED_TOGGLE(6);
max1168_read();
RunOnceEvery(10,
{
DOWNLINK_SEND_BOOT(DefaultChannel, DefaultDevice, &sys_time.nb_sec);
LED_PERIODIC();
});
static inline void main_periodic_task(void)
{
tx_data[0] += 1;
ppz_can_transmit(0, tx_data, 8);
LED_PERIODIC();
DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);
}
static inline void main_periodic_task( void ) {
trans.type = I2CTransTx;
trans.buf[0] = 0x04;
trans.len_w = 1;
trans.slave_addr = 0x58;
i2c_submit(&ACTUATORS_MKK_DEV,&trans);
LED_PERIODIC();
}
static inline void main_periodic( void ) {
static float foo = 0.;
foo += 0.0025;
int32_t bar = 1500 + 500. * sin(foo);
for (int i = 0; i < ACTUATORS_PWM_NB; i++) {
actuators_pwm_values[i] = bar;
}
actuators_pwm_commit();
LED_PERIODIC();
}
static inline void main_periodic( void ) {
static float foo = 0.;
foo += 0.0025;
int32_t bar = 1500 + 500. * sin(foo);
for (int i = 0; i < ACTUATORS_PWM_NB; i++) {
ActuatorPwmSet(i, bar);
}
ActuatorsPwmCommit();
LED_PERIODIC();
}
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_task( void ) {
trans.type = I2CTransTx;
trans.slave_addr = 0x02;
trans.len_w = 4;
trans.buf[0] = 100;
trans.buf[1] = 100;
trans.buf[2] = 100;
trans.buf[3] = 1;
i2c_submit(&i2c1,&trans);
LED_PERIODIC();
}
/* Sets the actual actuator commands */
STATIC_INLINE void main_periodic(void)
{
/* Inter-MCU watchdog */
intermcu_periodic();
/* Safety check and set FBW mode */
fbw_safety_check();
#ifdef BOARD_PX4IO
//due to a baud rate issue on PX4, for a few seconds the baud is 1500000 however this may result in package loss, causing the motors to spin at random
//to prevent this situation:
if (intermcu.stable_px4_baud != PPRZ_BAUD) {
fbw_mode = FBW_MODE_FAILSAFE;
fbw_motors_on = false;
//signal to user whether fbw can be flashed:
#ifdef FBW_MODE_LED
LED_OFF(FBW_MODE_LED); // causes really fast blinking
#endif
}
#endif
// TODO make module out of led blink?
#ifdef FBW_MODE_LED
static uint16_t dv = 0;
if (fbw_mode == FBW_MODE_FAILSAFE) {
if (!(dv++ % (PERIODIC_FREQUENCY / 20))) { LED_TOGGLE(FBW_MODE_LED);}
} else if (fbw_mode == FBW_MODE_MANUAL) {
if (!(dv++ % (PERIODIC_FREQUENCY))) { LED_TOGGLE(FBW_MODE_LED);}
} else if (fbw_mode == FBW_MODE_AUTO) {
LED_ON(FBW_MODE_LED);
}
#endif // FWB_MODE_LED
/* Set failsafe commands */
if (fbw_mode == FBW_MODE_FAILSAFE) {
fbw_motors_on = false;
SetCommands(commands_failsafe);
}
/* If in auto copy autopilot motors on */
if (fbw_mode == FBW_MODE_AUTO) {
fbw_motors_on = autopilot_motors_on;
}
/* Set actuators */
SetActuatorsFromCommands(commands, autopilot_mode);
/* Periodic blinking */
RunOnceEvery(10, LED_PERIODIC());
}
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 ) {
static float foo = 0.;
foo += 0.0025;
int32_t bar = 1500 + 500. * sin(foo);
// int32_t bar = 1450;
// int32_t bar = 1950;
actuators_pwm_values[0] = bar;
actuators_pwm_values[1] = bar;
actuators_pwm_values[2] = bar;
actuators_pwm_values[3] = bar;
actuators_pwm_values[4] = bar;
actuators_pwm_values[5] = bar;
actuators_pwm_commit();
LED_PERIODIC();
}
int main(void) {
int i = 0;
mcu_init();
while (1) {
for (i=0; i< LED_PROGRAM_SIZE; i++)
{
if (LED_PROG_ON[i] >= 0)
led_on(LED_PROG_ON[i]);
LED_PERIODIC();
Delay(2000000);
if (LED_PROG_OFF[i] >= 0)
led_off(LED_PROG_OFF[i]);
}
};
return 0;
}
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);
});
void handle_periodic_tasks_ap(void)
{
if (sys_time_check_and_ack_timer(sensors_tid)) {
sensors_task();
}
#if USE_BARO_BOARD
if (sys_time_check_and_ack_timer(baro_tid)) {
baro_periodic();
}
#endif
#if USE_GENERATED_AUTOPILOT
if (sys_time_check_and_ack_timer(attitude_tid)) {
autopilot_periodic();
}
#else
// static autopilot
if (sys_time_check_and_ack_timer(navigation_tid)) {
navigation_task();
}
#ifndef AHRS_TRIGGERED_ATTITUDE_LOOP
if (sys_time_check_and_ack_timer(attitude_tid)) {
attitude_loop();
}
#endif
#endif
if (sys_time_check_and_ack_timer(modules_tid)) {
modules_periodic_task();
}
if (sys_time_check_and_ack_timer(monitor_tid)) {
monitor_task();
}
if (sys_time_check_and_ack_timer(telemetry_tid)) {
reporting_task();
LED_PERIODIC();
}
}
void main_periodic(void)
{
/* run control loops */
autopilot_periodic();
/* set actuators */
//actuators_set(autopilot_get_motors_on());
SetActuatorsFromCommands(commands, autopilot_get_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_task(void)
{
if (sys_time.nb_sec > 1) {
imu_periodic();
}
RunOnceEvery(10, { LED_PERIODIC();});
static inline void main_periodic( void ) {
OveroLinkPeriodic(on_overo_link_lost);
RunOnceEvery(10,{ LED_PERIODIC();});
static inline void main_periodic_task( void ) {
if (cpu_time_sec > 1) imu_periodic();
RunOnceEvery(10, { LED_PERIODIC();});
STATIC_INLINE void main_periodic(void)
{
/* Inter-MCU watchdog */
intermcu_periodic();
/* Safety logic */
bool ap_lost = (inter_mcu.status == INTERMCU_LOST);
bool rc_lost = (radio_control.status == RC_REALLY_LOST);
if (rc_lost) {
if (ap_lost) {
// Both are lost
fbw_mode = FBW_MODE_FAILSAFE;
} else {
if (fbw_mode == FBW_MODE_MANUAL) {
fbw_mode = RC_LOST_FBW_MODE;
} else {
if (fbw_mode == FBW_MODE_FAILSAFE) {
// No change: failsafe stays failsafe
} else {
// Lost RC while in working Auto mode
fbw_mode = RC_LOST_IN_AUTO_FBW_MODE;
}
}
}
} else { // rc_is_good
if (fbw_mode == FBW_MODE_AUTO) {
if (ap_lost) {
fbw_mode = AP_LOST_FBW_MODE;
}
}
}
#ifdef BOARD_PX4IO
//due to a baud rate issue on PX4, for a few seconds the baud is 1500000 however this may result in package loss, causing the motors to spin at random
//to prevent this situation:
if (inter_mcu.stable_px4_baud != PPRZ_BAUD) {
fbw_mode = FBW_MODE_FAILSAFE;
autopilot_motors_on = false;
//signal to user whether fbw can be flashed:
#ifdef FBW_MODE_LED
LED_OFF(FBW_MODE_LED); // causes really fast blinking
#endif
}
#endif
static uint16_t dv = 0;
// TODO make module out of led blink?
/* set failsafe commands */
if (fbw_mode == FBW_MODE_FAILSAFE) {
autopilot_motors_on = false;
SetCommands(commands_failsafe);
#ifdef FBW_MODE_LED
if (!(dv++ % (PERIODIC_FREQUENCY / 20))) { LED_TOGGLE(FBW_MODE_LED);}
} else if (fbw_mode == FBW_MODE_MANUAL) {
if (!(dv++ % (PERIODIC_FREQUENCY))) { LED_TOGGLE(FBW_MODE_LED);}
} else if (fbw_mode == FBW_MODE_AUTO) {
intermcu_blink_fbw_led(dv++);
#endif // FWB_MODE_LED
}
/* set actuators */
SetActuatorsFromCommands(commands, autopilot_mode);
/* blink */
RunOnceEvery(10, LED_PERIODIC());
}
static inline void main_periodic(void)
{
LED_PERIODIC();
}
static inline void main_periodic_task( void ) {
main_test_send();
LED_PERIODIC();
}
#if USE_GX3
#else
imu_periodic();
#endif
/* 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++; });
}
RunOnceEvery(10, LED_PERIODIC());
}
STATIC_INLINE void telemetry_periodic(void) {
PeriodicSendMain(DefaultChannel,DefaultDevice);
}
STATIC_INLINE void failsafe_check( void ) {
if (radio_control.status != RC_OK &&
autopilot_mode != AP_MODE_KILL &&
autopilot_mode != AP_MODE_NAV)
{
autopilot_set_mode(AP_MODE_FAILSAFE);
}
#if USE_GPS
static inline void main_periodic(void)
{
ActuatorsPwmCommit();
LED_PERIODIC();
RunOnceEvery(100, {DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);});
