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 }