|
|
|
@ -785,28 +785,9 @@ private:
@@ -785,28 +785,9 @@ private:
|
|
|
|
|
// soaring mode-change timer
|
|
|
|
|
uint32_t soaring_mode_timer; |
|
|
|
|
|
|
|
|
|
// Attitude.cpp
|
|
|
|
|
void adjust_nav_pitch_throttle(void); |
|
|
|
|
void update_load_factor(void); |
|
|
|
|
void send_fence_status(mavlink_channel_t chan); |
|
|
|
|
void send_servo_out(mavlink_channel_t chan); |
|
|
|
|
|
|
|
|
|
void Log_Write_Fast(void); |
|
|
|
|
void Log_Write_Attitude(void); |
|
|
|
|
void Log_Write_Performance(); |
|
|
|
|
void Log_Write_Startup(uint8_t type); |
|
|
|
|
void Log_Write_Control_Tuning(); |
|
|
|
|
void Log_Write_OFG_Guided(); |
|
|
|
|
void Log_Write_Guided(void); |
|
|
|
|
void Log_Write_Nav_Tuning(); |
|
|
|
|
void Log_Write_Status(); |
|
|
|
|
void Log_Write_RC(void); |
|
|
|
|
void Log_Write_Vehicle_Startup_Messages(); |
|
|
|
|
void Log_Write_AOA_SSA(); |
|
|
|
|
void Log_Write_AETR(); |
|
|
|
|
void Log_Write_MavCmdI(const mavlink_command_int_t &packet); |
|
|
|
|
|
|
|
|
|
void load_parameters(void) override; |
|
|
|
|
void convert_mixers(void); |
|
|
|
|
void adjust_altitude_target(); |
|
|
|
|
void setup_glide_slope(void); |
|
|
|
|
int32_t get_RTL_altitude(); |
|
|
|
@ -832,11 +813,50 @@ private:
@@ -832,11 +813,50 @@ private:
|
|
|
|
|
float rangefinder_correction(void); |
|
|
|
|
void rangefinder_height_update(void); |
|
|
|
|
void rangefinder_terrain_correction(float &height); |
|
|
|
|
void stabilize(); |
|
|
|
|
void calc_throttle(); |
|
|
|
|
void calc_nav_roll(); |
|
|
|
|
void calc_nav_pitch(); |
|
|
|
|
float get_speed_scaler(void); |
|
|
|
|
bool stick_mixing_enabled(void); |
|
|
|
|
void stabilize_roll(float speed_scaler); |
|
|
|
|
void stabilize_pitch(float speed_scaler); |
|
|
|
|
void stabilize_stick_mixing_direct(); |
|
|
|
|
void stabilize_stick_mixing_fbw(); |
|
|
|
|
void stabilize_yaw(float speed_scaler); |
|
|
|
|
void stabilize_training(float speed_scaler); |
|
|
|
|
void stabilize_acro(float speed_scaler); |
|
|
|
|
void calc_nav_yaw_coordinated(float speed_scaler); |
|
|
|
|
void calc_nav_yaw_course(void); |
|
|
|
|
void calc_nav_yaw_ground(void); |
|
|
|
|
|
|
|
|
|
// GCS_Mavlink.cpp
|
|
|
|
|
void send_fence_status(mavlink_channel_t chan); |
|
|
|
|
void send_servo_out(mavlink_channel_t chan); |
|
|
|
|
|
|
|
|
|
// Log.cpp
|
|
|
|
|
void Log_Write_Fast(void); |
|
|
|
|
void Log_Write_Attitude(void); |
|
|
|
|
void Log_Write_Performance(); |
|
|
|
|
void Log_Write_Startup(uint8_t type); |
|
|
|
|
void Log_Write_Control_Tuning(); |
|
|
|
|
void Log_Write_OFG_Guided(); |
|
|
|
|
void Log_Write_Guided(void); |
|
|
|
|
void Log_Write_Nav_Tuning(); |
|
|
|
|
void Log_Write_Status(); |
|
|
|
|
void Log_Write_RC(void); |
|
|
|
|
void Log_Write_Vehicle_Startup_Messages(); |
|
|
|
|
void Log_Write_AOA_SSA(); |
|
|
|
|
void Log_Write_AETR(); |
|
|
|
|
void Log_Write_MavCmdI(const mavlink_command_int_t &packet); |
|
|
|
|
void log_init(); |
|
|
|
|
|
|
|
|
|
// Parameters.cpp
|
|
|
|
|
void load_parameters(void) override; |
|
|
|
|
void convert_mixers(void); |
|
|
|
|
|
|
|
|
|
// commands_logic.cpp
|
|
|
|
|
void set_next_WP(const struct Location &loc); |
|
|
|
|
void set_guided_WP(void); |
|
|
|
|
void update_home(); |
|
|
|
|
// set home location and store it persistently:
|
|
|
|
|
bool set_home_persistently(const Location &loc) WARN_IF_UNUSED; |
|
|
|
|
void do_RTL(int32_t alt); |
|
|
|
|
bool verify_takeoff(); |
|
|
|
|
bool verify_loiter_unlim(const AP_Mission::Mission_Command &cmd); |
|
|
|
@ -848,12 +868,44 @@ private:
@@ -848,12 +868,44 @@ private:
|
|
|
|
|
bool verify_wait_delay(); |
|
|
|
|
bool verify_within_distance(); |
|
|
|
|
bool verify_altitude_wait(const AP_Mission::Mission_Command &cmd); |
|
|
|
|
bool verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd); |
|
|
|
|
bool verify_vtol_land(const AP_Mission::Mission_Command &cmd); |
|
|
|
|
void do_loiter_at_location(); |
|
|
|
|
bool verify_loiter_heading(bool init); |
|
|
|
|
void exit_mission_callback(); |
|
|
|
|
void mavlink_delay(uint32_t ms); |
|
|
|
|
bool start_command(const AP_Mission::Mission_Command& cmd); |
|
|
|
|
bool verify_command(const AP_Mission::Mission_Command& cmd); |
|
|
|
|
void do_takeoff(const AP_Mission::Mission_Command& cmd); |
|
|
|
|
void do_nav_wp(const AP_Mission::Mission_Command& cmd); |
|
|
|
|
void do_land(const AP_Mission::Mission_Command& cmd); |
|
|
|
|
void do_landing_vtol_approach(const AP_Mission::Mission_Command& cmd); |
|
|
|
|
void loiter_set_direction_wp(const AP_Mission::Mission_Command& cmd); |
|
|
|
|
void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd); |
|
|
|
|
void do_loiter_turns(const AP_Mission::Mission_Command& cmd); |
|
|
|
|
void do_loiter_time(const AP_Mission::Mission_Command& cmd); |
|
|
|
|
void do_altitude_wait(const AP_Mission::Mission_Command& cmd); |
|
|
|
|
void do_continue_and_change_alt(const AP_Mission::Mission_Command& cmd); |
|
|
|
|
void do_loiter_to_alt(const AP_Mission::Mission_Command& cmd); |
|
|
|
|
void do_vtol_takeoff(const AP_Mission::Mission_Command& cmd); |
|
|
|
|
void do_vtol_land(const AP_Mission::Mission_Command& cmd); |
|
|
|
|
bool verify_nav_wp(const AP_Mission::Mission_Command& cmd); |
|
|
|
|
bool verify_landing_vtol_approach(const AP_Mission::Mission_Command& cmd); |
|
|
|
|
void do_wait_delay(const AP_Mission::Mission_Command& cmd); |
|
|
|
|
void do_within_distance(const AP_Mission::Mission_Command& cmd); |
|
|
|
|
void do_change_speed(const AP_Mission::Mission_Command& cmd); |
|
|
|
|
void do_set_home(const AP_Mission::Mission_Command& cmd); |
|
|
|
|
bool start_command_callback(const AP_Mission::Mission_Command &cmd); |
|
|
|
|
bool verify_command_callback(const AP_Mission::Mission_Command& cmd); |
|
|
|
|
|
|
|
|
|
// commands.cpp
|
|
|
|
|
void set_guided_WP(void); |
|
|
|
|
void update_home(); |
|
|
|
|
// set home location and store it persistently:
|
|
|
|
|
bool set_home_persistently(const Location &loc) WARN_IF_UNUSED; |
|
|
|
|
|
|
|
|
|
// quadplane.cpp
|
|
|
|
|
bool verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd); |
|
|
|
|
bool verify_vtol_land(const AP_Mission::Mission_Command &cmd); |
|
|
|
|
|
|
|
|
|
// control_modes.cpp
|
|
|
|
|
void read_control_switch(); |
|
|
|
|
uint8_t readSwitch(void); |
|
|
|
|
void reset_control_switch(); |
|
|
|
@ -861,11 +913,17 @@ private:
@@ -861,11 +913,17 @@ private:
|
|
|
|
|
void autotune_restore(void); |
|
|
|
|
void autotune_enable(bool enable); |
|
|
|
|
bool fly_inverted(void); |
|
|
|
|
uint8_t get_mode() const override { return (uint8_t)control_mode->mode_number(); } |
|
|
|
|
Mode *mode_from_mode_num(const enum Mode::Number num); |
|
|
|
|
|
|
|
|
|
// events.cpp
|
|
|
|
|
void failsafe_short_on_event(enum failsafe_state fstype, ModeReason reason); |
|
|
|
|
void failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason); |
|
|
|
|
void failsafe_short_off_event(ModeReason reason); |
|
|
|
|
void failsafe_long_off_event(ModeReason reason); |
|
|
|
|
void handle_battery_failsafe(const char* type_str, const int8_t action); |
|
|
|
|
|
|
|
|
|
// geofence.cpp
|
|
|
|
|
uint8_t max_fencepoints(void) const; |
|
|
|
|
Vector2l get_fence_point_with_index(uint8_t i) const; |
|
|
|
|
void set_fence_point_with_index(const Vector2l &point, unsigned i); |
|
|
|
@ -883,8 +941,37 @@ private:
@@ -883,8 +941,37 @@ private:
|
|
|
|
|
void geofence_send_status(mavlink_channel_t chan); |
|
|
|
|
bool geofence_breached(void); |
|
|
|
|
void geofence_disable_and_send_error_msg(const char *errorMsg); |
|
|
|
|
|
|
|
|
|
// ArduPlane.cpp
|
|
|
|
|
void disarm_if_autoland_complete(); |
|
|
|
|
float tecs_hgt_afe(void); |
|
|
|
|
void efi_update(void); |
|
|
|
|
void get_scheduler_tasks(const AP_Scheduler::Task *&tasks, |
|
|
|
|
uint8_t &task_count, |
|
|
|
|
uint32_t &log_bit) override; |
|
|
|
|
void ahrs_update(); |
|
|
|
|
void update_speed_height(void); |
|
|
|
|
void update_GPS_50Hz(void); |
|
|
|
|
void update_GPS_10Hz(void); |
|
|
|
|
void update_compass(void); |
|
|
|
|
void update_alt(void); |
|
|
|
|
#if ADVANCED_FAILSAFE == ENABLED |
|
|
|
|
void afs_fs_check(void); |
|
|
|
|
#endif |
|
|
|
|
void one_second_loop(void); |
|
|
|
|
void airspeed_ratio_update(void); |
|
|
|
|
void compass_save(void); |
|
|
|
|
void update_logging1(void); |
|
|
|
|
void update_logging2(void); |
|
|
|
|
void update_control_mode(void); |
|
|
|
|
void update_flight_stage(); |
|
|
|
|
void update_navigation(); |
|
|
|
|
void set_flight_stage(AP_Vehicle::FixedWing::FlightStage fs); |
|
|
|
|
#if OSD_ENABLED == ENABLED |
|
|
|
|
void publish_osd_info(); |
|
|
|
|
#endif |
|
|
|
|
|
|
|
|
|
// navigation.cpp
|
|
|
|
|
void set_nav_controller(void); |
|
|
|
|
void loiter_angle_reset(void); |
|
|
|
|
void loiter_angle_update(void); |
|
|
|
@ -896,6 +983,8 @@ private:
@@ -896,6 +983,8 @@ private:
|
|
|
|
|
void update_fbwb_speed_height(void); |
|
|
|
|
void setup_turn_angle(void); |
|
|
|
|
bool reached_loiter_target(void); |
|
|
|
|
|
|
|
|
|
// radio.cpp
|
|
|
|
|
void set_control_channels(void) override; |
|
|
|
|
void init_rc_in(); |
|
|
|
|
void init_rc_out_main(); |
|
|
|
@ -907,26 +996,28 @@ private:
@@ -907,26 +996,28 @@ private:
|
|
|
|
|
bool trim_radio(); |
|
|
|
|
bool rc_throttle_value_ok(void) const; |
|
|
|
|
bool rc_failsafe_active(void) const; |
|
|
|
|
|
|
|
|
|
// sensors.cpp
|
|
|
|
|
void read_rangefinder(void); |
|
|
|
|
void read_airspeed(void); |
|
|
|
|
void rpm_update(void); |
|
|
|
|
void efi_update(void); |
|
|
|
|
void accel_cal_update(void); |
|
|
|
|
|
|
|
|
|
// system.cpp
|
|
|
|
|
void init_ardupilot() override; |
|
|
|
|
void get_scheduler_tasks(const AP_Scheduler::Task *&tasks, |
|
|
|
|
uint8_t &task_count, |
|
|
|
|
uint32_t &log_bit) override; |
|
|
|
|
void startup_ground(void); |
|
|
|
|
bool set_mode(Mode& new_mode, const ModeReason reason); |
|
|
|
|
bool set_mode(const uint8_t mode, const ModeReason reason) override; |
|
|
|
|
bool set_mode_by_number(const Mode::Number new_mode_number, const ModeReason reason); |
|
|
|
|
uint8_t get_mode() const override { return (uint8_t)control_mode->mode_number(); } |
|
|
|
|
Mode *mode_from_mode_num(const enum Mode::Number num); |
|
|
|
|
void check_long_failsafe(); |
|
|
|
|
void check_short_failsafe(); |
|
|
|
|
void startup_INS_ground(void); |
|
|
|
|
bool should_log(uint32_t mask); |
|
|
|
|
int8_t throttle_percentage(void); |
|
|
|
|
void update_dynamic_notch(); |
|
|
|
|
void notify_mode(const Mode& mode); |
|
|
|
|
|
|
|
|
|
// takeoff.cpp
|
|
|
|
|
bool auto_takeoff_check(void); |
|
|
|
|
void takeoff_calc_roll(void); |
|
|
|
|
void takeoff_calc_pitch(void); |
|
|
|
@ -934,24 +1025,11 @@ private:
@@ -934,24 +1025,11 @@ private:
|
|
|
|
|
int16_t get_takeoff_pitch_min_cd(void); |
|
|
|
|
void landing_gear_update(void); |
|
|
|
|
void complete_auto_takeoff(void); |
|
|
|
|
void ahrs_update(); |
|
|
|
|
void update_speed_height(void); |
|
|
|
|
void update_GPS_50Hz(void); |
|
|
|
|
void update_GPS_10Hz(void); |
|
|
|
|
void update_compass(void); |
|
|
|
|
void update_alt(void); |
|
|
|
|
#if ADVANCED_FAILSAFE == ENABLED |
|
|
|
|
void afs_fs_check(void); |
|
|
|
|
#endif |
|
|
|
|
void update_optical_flow(void); |
|
|
|
|
void one_second_loop(void); |
|
|
|
|
void airspeed_ratio_update(void); |
|
|
|
|
void compass_save(void); |
|
|
|
|
void update_logging1(void); |
|
|
|
|
void update_logging2(void); |
|
|
|
|
|
|
|
|
|
// avoidance_adsb.cpp
|
|
|
|
|
void avoidance_adsb_update(void); |
|
|
|
|
void update_control_mode(void); |
|
|
|
|
void stabilize(); |
|
|
|
|
|
|
|
|
|
// servos.cpp
|
|
|
|
|
void set_servos_idle(void); |
|
|
|
|
void set_servos(); |
|
|
|
|
void set_servos_manual_passthrough(void); |
|
|
|
@ -966,83 +1044,39 @@ private:
@@ -966,83 +1044,39 @@ private:
|
|
|
|
|
void servos_twin_engine_mix(); |
|
|
|
|
void throttle_voltage_comp(); |
|
|
|
|
void throttle_watt_limiter(int8_t &min_throttle, int8_t &max_throttle); |
|
|
|
|
void update_is_flying_5Hz(void); |
|
|
|
|
void crash_detection_update(void); |
|
|
|
|
bool in_preLaunch_flight_stage(void); |
|
|
|
|
void calc_throttle(); |
|
|
|
|
void calc_nav_roll(); |
|
|
|
|
void calc_nav_pitch(); |
|
|
|
|
void update_flight_stage(); |
|
|
|
|
void update_navigation(); |
|
|
|
|
void set_flight_stage(AP_Vehicle::FixedWing::FlightStage fs); |
|
|
|
|
bool is_flying(void); |
|
|
|
|
float get_speed_scaler(void); |
|
|
|
|
bool stick_mixing_enabled(void); |
|
|
|
|
void stabilize_roll(float speed_scaler); |
|
|
|
|
void stabilize_pitch(float speed_scaler); |
|
|
|
|
void stabilize_stick_mixing_direct(); |
|
|
|
|
void stabilize_stick_mixing_fbw(); |
|
|
|
|
void stabilize_yaw(float speed_scaler); |
|
|
|
|
void stabilize_training(float speed_scaler); |
|
|
|
|
void stabilize_acro(float speed_scaler); |
|
|
|
|
void calc_nav_yaw_coordinated(float speed_scaler); |
|
|
|
|
void calc_nav_yaw_course(void); |
|
|
|
|
void calc_nav_yaw_ground(void); |
|
|
|
|
void throttle_slew_limit(SRV_Channel::Aux_servo_function_t func); |
|
|
|
|
bool suppress_throttle(void); |
|
|
|
|
void update_throttle_hover(); |
|
|
|
|
void channel_function_mixer(SRV_Channel::Aux_servo_function_t func1_in, SRV_Channel::Aux_servo_function_t func2_in, |
|
|
|
|
SRV_Channel::Aux_servo_function_t func1_out, SRV_Channel::Aux_servo_function_t func2_out); |
|
|
|
|
void flaperon_update(int8_t flap_percent); |
|
|
|
|
bool start_command(const AP_Mission::Mission_Command& cmd); |
|
|
|
|
bool verify_command(const AP_Mission::Mission_Command& cmd); |
|
|
|
|
void do_takeoff(const AP_Mission::Mission_Command& cmd); |
|
|
|
|
void do_nav_wp(const AP_Mission::Mission_Command& cmd); |
|
|
|
|
void do_land(const AP_Mission::Mission_Command& cmd); |
|
|
|
|
void do_landing_vtol_approach(const AP_Mission::Mission_Command& cmd); |
|
|
|
|
void loiter_set_direction_wp(const AP_Mission::Mission_Command& cmd); |
|
|
|
|
void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd); |
|
|
|
|
void do_loiter_turns(const AP_Mission::Mission_Command& cmd); |
|
|
|
|
void do_loiter_time(const AP_Mission::Mission_Command& cmd); |
|
|
|
|
void do_altitude_wait(const AP_Mission::Mission_Command& cmd); |
|
|
|
|
void do_continue_and_change_alt(const AP_Mission::Mission_Command& cmd); |
|
|
|
|
void do_loiter_to_alt(const AP_Mission::Mission_Command& cmd); |
|
|
|
|
void do_vtol_takeoff(const AP_Mission::Mission_Command& cmd); |
|
|
|
|
void do_vtol_land(const AP_Mission::Mission_Command& cmd); |
|
|
|
|
bool verify_nav_wp(const AP_Mission::Mission_Command& cmd); |
|
|
|
|
bool verify_landing_vtol_approach(const AP_Mission::Mission_Command& cmd); |
|
|
|
|
void do_wait_delay(const AP_Mission::Mission_Command& cmd); |
|
|
|
|
void do_within_distance(const AP_Mission::Mission_Command& cmd); |
|
|
|
|
void do_change_speed(const AP_Mission::Mission_Command& cmd); |
|
|
|
|
void do_set_home(const AP_Mission::Mission_Command& cmd); |
|
|
|
|
bool start_command_callback(const AP_Mission::Mission_Command &cmd); |
|
|
|
|
bool verify_command_callback(const AP_Mission::Mission_Command& cmd); |
|
|
|
|
void notify_mode(const Mode& mode); |
|
|
|
|
void log_init(); |
|
|
|
|
|
|
|
|
|
// is_flying.cpp
|
|
|
|
|
void update_is_flying_5Hz(void); |
|
|
|
|
void crash_detection_update(void); |
|
|
|
|
bool in_preLaunch_flight_stage(void); |
|
|
|
|
bool is_flying(void); |
|
|
|
|
|
|
|
|
|
// parachute.cpp
|
|
|
|
|
void parachute_check(); |
|
|
|
|
#if PARACHUTE == ENABLED |
|
|
|
|
void do_parachute(const AP_Mission::Mission_Command& cmd); |
|
|
|
|
void parachute_release(); |
|
|
|
|
bool parachute_manual_release(); |
|
|
|
|
#endif |
|
|
|
|
#if OSD_ENABLED == ENABLED |
|
|
|
|
void publish_osd_info(); |
|
|
|
|
#endif |
|
|
|
|
void accel_cal_update(void); |
|
|
|
|
|
|
|
|
|
// soaring.cpp
|
|
|
|
|
#if SOARING_ENABLED == ENABLED |
|
|
|
|
void update_soaring(); |
|
|
|
|
#endif |
|
|
|
|
|
|
|
|
|
// reverse_thrust.cpp
|
|
|
|
|
bool reversed_throttle; |
|
|
|
|
bool have_reverse_throttle_rc_option; |
|
|
|
|
bool allow_reverse_thrust(void) const; |
|
|
|
|
bool have_reverse_thrust(void) const; |
|
|
|
|
int16_t get_throttle_input(bool no_deadzone=false) const; |
|
|
|
|
|
|
|
|
|
// support for AP_Avoidance custom flight mode, AVOID_ADSB
|
|
|
|
|
bool avoid_adsb_init(bool ignore_checks); |
|
|
|
|
void avoid_adsb_run(); |
|
|
|
|
|
|
|
|
|
enum Failsafe_Action { |
|
|
|
|
Failsafe_Action_None = 0, |
|
|
|
|
Failsafe_Action_RTL = 1, |
|
|
|
|