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_static_init(void)
{
/* Mode is finally set by autopilot_static_set_mode if MODE_STARTUP is not KILL.
* For autopilot_static_set_mode to do anything, the requested mode needs to differ
* from previous mode, so we set it to a safe KILL first.
*/
autopilot.mode = AP_MODE_KILL;
/* set startup mode, propagates through to guidance h/v */
autopilot_static_set_mode(MODE_STARTUP);
/* init arming */
autopilot_arming_init();
}
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
}
void autopilot_init(void) {
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_detect_ground = FALSE;
autopilot_detect_ground_once = FALSE;
autopilot_flight_time = 0;
autopilot_rc = TRUE;
autopilot_power_switch = FALSE;
#ifdef POWER_SWITCH_LED
LED_ON(POWER_SWITCH_LED); // POWER OFF
#endif
autopilot_arming_init();
}
/*飞控的初始化*/
void autopilot_init(void) {
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;//自动飞行模式2
autopilot_detect_ground = FALSE;//地面检测关闭
autopilot_detect_ground_once = FALSE;
autopilot_flight_time = 0;//飞行时间初始为0
autopilot_rc = TRUE;//RC遥控使能
autopilot_power_switch = FALSE；//电源开关关闭
#ifdef POWER_SWITCH_LED
LED_ON(POWER_SWITCH_LED); // POWER OFF
#endif
autopilot_arming_init();//飞控的初始化：开关，节流阀，偏航
}
void autopilot_init(void) {
autopilot_mode = AP_MODE_KILL;// Orignal value is AP_MODE_KILL
autopilot_motors_on = FALSE; // Original value is FALSE
kill_throttle = ! autopilot_motors_on;
autopilot_in_flight = FALSE;//Original value is FALSE
autopilot_in_flight_counter = 0;
autopilot_mode_auto2 = MODE_AUTO2;
autopilot_detect_ground = FALSE;
autopilot_detect_ground_once = FALSE;
autopilot_flight_time = 0;
autopilot_rc = TRUE;
autopilot_power_switch = FALSE; //Original value is FALSE
//stabilization_cmd[COMMAND_THRUST] = 2400;//THIS LINE OF CODE IS ADDED BY EDWARD FOR TESTING.
#ifdef POWER_SWITCH_LED
LED_ON(POWER_SWITCH_LED); // POWER OFF
#endif
autopilot_arming_init();
}
