void stabilization_init(void) {
#ifndef STABILIZATION_SKIP_RATE
stabilization_none_init();
stabilization_rate_init();
#endif
stabilization_attitude_init();
}
void autopilot_init(void)
{
autopilot_motors_on = false;
kill_throttle = ! autopilot_motors_on;
autopilot_in_flight = false;
autopilot_in_flight_counter = 0;
autopilot_mode_auto2 = MODE_AUTO2;
autopilot_ground_detected = false;
autopilot_detect_ground_once = false;
autopilot_flight_time = 0;
autopilot_rc = true;
autopilot_power_switch = false;
#ifdef POWER_SWITCH_GPIO
gpio_setup_output(POWER_SWITCH_GPIO);
gpio_clear(POWER_SWITCH_GPIO); // POWER OFF
#endif
// init GNC stack
// TODO this should be done in modules init
nav_init();
guidance_h_init();
guidance_v_init();
stabilization_init();
stabilization_none_init();
#if USE_STABILIZATION_RATE
stabilization_rate_init();
#endif
stabilization_attitude_init();
// call implementation init
// it will set startup mode
#if USE_GENERATED_AUTOPILOT
autopilot_generated_init();
#else
autopilot_static_init();
#endif
// register messages
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AUTOPILOT_VERSION, send_autopilot_version);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ALIVE, send_alive);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_STATUS, send_status);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ATTITUDE, send_attitude);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ENERGY, send_energy);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_FP, send_fp);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_FP_MIN, send_fp_min);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_CMD, send_rotorcraft_cmd);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_DL_VALUE, send_dl_value);
#ifdef ACTUATORS
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ACTUATORS, send_actuators);
#endif
#ifdef RADIO_CONTROL
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_RC, send_rc);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_RADIO_CONTROL, send_rotorcraft_rc);
#endif
}
void autopilot_init(void)
{
/* mode is finally set at end of init if MODE_STARTUP is not KILL */
autopilot_mode = AP_MODE_KILL;
autopilot_motors_on = false;
kill_throttle = ! autopilot_motors_on;
autopilot_in_flight = false;
autopilot_in_flight_counter = 0;
autopilot_mode_auto2 = MODE_AUTO2;
autopilot_ground_detected = false;
autopilot_detect_ground_once = false;
autopilot_flight_time = 0;
autopilot_rc = true;
autopilot_power_switch = false;
#ifdef POWER_SWITCH_GPIO
gpio_setup_output(POWER_SWITCH_GPIO);
gpio_clear(POWER_SWITCH_GPIO); // POWER OFF
#endif
autopilot_arming_init();
nav_init();
guidance_h_init();
guidance_v_init();
stabilization_init();
stabilization_none_init();
#if USE_STABILIZATION_RATE
stabilization_rate_init();
#endif
stabilization_attitude_init();
/* set startup mode, propagates through to guidance h/v */
autopilot_set_mode(MODE_STARTUP);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AUTOPILOT_VERSION, send_autopilot_version);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ALIVE, send_alive);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_STATUS, send_status);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ATTITUDE, send_attitude);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ENERGY, send_energy);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_FP, send_fp);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_FP_MIN, send_fp_min);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_CMD, send_rotorcraft_cmd);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_DL_VALUE, send_dl_value);
#ifdef ACTUATORS
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ACTUATORS, send_actuators);
#endif
#ifdef RADIO_CONTROL
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_RC, send_rc);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTORCRAFT_RADIO_CONTROL, send_rotorcraft_rc);
#endif
}
void autopilot_init(void)
{
/* mode is finally set at end of init if MODE_STARTUP is not KILL */
autopilot_mode = AP_MODE_KILL;
autopilot_motors_on = FALSE;
kill_throttle = ! autopilot_motors_on;
autopilot_in_flight = FALSE;
autopilot_in_flight_counter = 0;
autopilot_mode_auto2 = MODE_AUTO2;
autopilot_ground_detected = FALSE;
autopilot_detect_ground_once = FALSE;
autopilot_flight_time = 0;
autopilot_rc = TRUE;
autopilot_power_switch = FALSE;
#ifdef POWER_SWITCH_GPIO
gpio_setup_output(POWER_SWITCH_GPIO);
gpio_clear(POWER_SWITCH_GPIO); // POWER OFF
#endif
autopilot_arming_init();
nav_init();
guidance_h_init();
guidance_v_init();
stabilization_init();
stabilization_none_init();
stabilization_rate_init();
stabilization_attitude_init();
/* set startup mode, propagates through to guidance h/v */
autopilot_set_mode(MODE_STARTUP);
register_periodic_telemetry(DefaultPeriodic, "AUTOPILOT_VERSION", send_autopilot_version);
register_periodic_telemetry(DefaultPeriodic, "ALIVE", send_alive);
register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_STATUS", send_status);
register_periodic_telemetry(DefaultPeriodic, "ATTITUDE", send_attitude);
register_periodic_telemetry(DefaultPeriodic, "ENERGY", send_energy);
register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_FP", send_fp);
register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_CMD", send_rotorcraft_cmd);
register_periodic_telemetry(DefaultPeriodic, "DL_VALUE", send_dl_value);
#ifdef ACTUATORS
register_periodic_telemetry(DefaultPeriodic, "ACTUATORS", send_actuators);
#endif
#ifdef RADIO_CONTROL
register_periodic_telemetry(DefaultPeriodic, "RC", send_rc);
register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_RADIO_CONTROL", send_rotorcraft_rc);
#endif
}
