diff --git a/AntennaTracker/Tracker.h b/AntennaTracker/Tracker.h index 2dbf8f3a972b2..e4528e6489843 100644 --- a/AntennaTracker/Tracker.h +++ b/AntennaTracker/Tracker.h @@ -221,6 +221,7 @@ class Tracker : public AP_Vehicle { bool start_command_callback(const AP_Mission::Mission_Command& cmd) { return false; } void exit_mission_callback() { return; } bool verify_command_callback(const AP_Mission::Mission_Command& cmd) { return false; } + AC_PID *get_AC_PID(AC_PID_TYPE type) override; // tracking.cpp void update_vehicle_pos_estimate(); diff --git a/AntennaTracker/system.cpp b/AntennaTracker/system.cpp index 9e650a749857e..47d34688fda2b 100644 --- a/AntennaTracker/system.cpp +++ b/AntennaTracker/system.cpp @@ -239,6 +239,27 @@ bool Tracker::should_log(uint32_t mask) return true; } +AC_PID* Tracker::get_AC_PID(AC_PID_TYPE type) +{ + AC_PID *ret = nullptr; + + switch(type) { + case AC_PID_TYPE::TRACKER_PITCH: + ret = &g.pidPitch2Srv; + break; + + case AC_PID_TYPE::TRACKER_YAW: + ret = &g.pidYaw2Srv; + break; + + default: + break; + } + + return ret; +} + + #include #include diff --git a/ArduCopter/AP_Vehicle.cpp b/ArduCopter/AP_Vehicle.cpp new file mode 100644 index 0000000000000..2573b05037def --- /dev/null +++ b/ArduCopter/AP_Vehicle.cpp @@ -0,0 +1,78 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +#include "Copter.h" + +// start takeoff to given altitude (for use by scripting) +bool Copter::start_takeoff(float alt) +{ + // exit if vehicle is not in Guided mode or Auto-Guided mode + if (!flightmode->in_guided_mode()) { + return false; + } + + if (mode_guided.do_user_takeoff_start(alt * 100.0f)) { + copter.set_auto_armed(true); + return true; + } + return false; +} + +// set target location (for use by scripting) +bool Copter::set_target_location(const Location& target_loc) +{ + // exit if vehicle is not in Guided mode or Auto-Guided mode + if (!flightmode->in_guided_mode()) { + return false; + } + + return mode_guided.set_destination(target_loc); +} + +bool Copter::set_target_velocity_NED(const Vector3f& vel_ned) +{ + // exit if vehicle is not in Guided mode or Auto-Guided mode + if (!flightmode->in_guided_mode()) { + return false; + } + + // convert vector to neu in cm + const Vector3f vel_neu_cms(vel_ned.x * 100.0f, vel_ned.y * 100.0f, -vel_ned.z * 100.0f); + mode_guided.set_velocity(vel_neu_cms); + return true; +} + +AC_PID* Copter::get_AC_PID(AC_PID_TYPE type) +{ + AC_PID *ret = nullptr; + + switch(type) { + case AC_PID_TYPE::RATE_ROLL: + ret = &copter.attitude_control->get_rate_roll_pid(); + break; + + case AC_PID_TYPE::RATE_PITCH: + ret = &copter.attitude_control->get_rate_pitch_pid(); + break; + + case AC_PID_TYPE::RATE_YAW: + ret = &copter.attitude_control->get_rate_yaw_pid(); + break; + + default: + break; + } + + return ret; +} diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index cd5a7cf790f6c..353c43a55e0cf 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -270,45 +270,6 @@ void Copter::fast_loop() AP_Vehicle::fast_loop(); } -// start takeoff to given altitude (for use by scripting) -bool Copter::start_takeoff(float alt) -{ - // exit if vehicle is not in Guided mode or Auto-Guided mode - if (!flightmode->in_guided_mode()) { - return false; - } - - if (mode_guided.do_user_takeoff_start(alt * 100.0f)) { - copter.set_auto_armed(true); - return true; - } - return false; -} - -// set target location (for use by scripting) -bool Copter::set_target_location(const Location& target_loc) -{ - // exit if vehicle is not in Guided mode or Auto-Guided mode - if (!flightmode->in_guided_mode()) { - return false; - } - - return mode_guided.set_destination(target_loc); -} - -bool Copter::set_target_velocity_NED(const Vector3f& vel_ned) -{ - // exit if vehicle is not in Guided mode or Auto-Guided mode - if (!flightmode->in_guided_mode()) { - return false; - } - - // convert vector to neu in cm - const Vector3f vel_neu_cms(vel_ned.x * 100.0f, vel_ned.y * 100.0f, -vel_ned.z * 100.0f); - mode_guided.set_velocity(vel_neu_cms); - return true; -} - // rc_loops - reads user input from transmitter/receiver // called at 100hz void Copter::rc_loop() diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 0fa5781d3cb54..bb7e582f52436 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -626,14 +626,17 @@ class Copter : public AP_Vehicle { void set_failsafe_gcs(bool b); void update_using_interlock(); + //AP_Vehicle.cpp + bool start_takeoff(float alt) override; + bool set_target_location(const Location& target_loc) override; + bool set_target_velocity_NED(const Vector3f& vel_ned) override; + AC_PID *get_AC_PID(AC_PID_TYPE type) override; + // Copter.cpp void get_scheduler_tasks(const AP_Scheduler::Task *&tasks, uint8_t &task_count, uint32_t &log_bit) override; void fast_loop() override; - bool start_takeoff(float alt) override; - bool set_target_location(const Location& target_loc) override; - bool set_target_velocity_NED(const Vector3f& vel_ned) override; void rc_loop(); void throttle_loop(); void update_batt_compass(void); diff --git a/ArduCopter/tuning.cpp b/ArduCopter/tuning.cpp index 43fc191bdedd3..efda48c6aa6a3 100644 --- a/ArduCopter/tuning.cpp +++ b/ArduCopter/tuning.cpp @@ -39,18 +39,18 @@ void Copter::tuning() break; case TUNING_RATE_ROLL_PITCH_KP: - attitude_control->get_rate_roll_pid().kP(tuning_value); - attitude_control->get_rate_pitch_pid().kP(tuning_value); + attitude_control->get_rate_roll_pid().set_kP(tuning_value); + attitude_control->get_rate_pitch_pid().set_kP(tuning_value); break; case TUNING_RATE_ROLL_PITCH_KI: - attitude_control->get_rate_roll_pid().kI(tuning_value); - attitude_control->get_rate_pitch_pid().kI(tuning_value); + attitude_control->get_rate_roll_pid().set_kI(tuning_value); + attitude_control->get_rate_pitch_pid().set_kI(tuning_value); break; case TUNING_RATE_ROLL_PITCH_KD: - attitude_control->get_rate_roll_pid().kD(tuning_value); - attitude_control->get_rate_pitch_pid().kD(tuning_value); + attitude_control->get_rate_roll_pid().set_kD(tuning_value); + attitude_control->get_rate_pitch_pid().set_kD(tuning_value); break; // Yaw tuning @@ -59,11 +59,11 @@ void Copter::tuning() break; case TUNING_YAW_RATE_KP: - attitude_control->get_rate_yaw_pid().kP(tuning_value); + attitude_control->get_rate_yaw_pid().set_kP(tuning_value); break; case TUNING_YAW_RATE_KD: - attitude_control->get_rate_yaw_pid().kD(tuning_value); + attitude_control->get_rate_yaw_pid().set_kD(tuning_value); break; // Altitude and throttle tuning @@ -76,15 +76,15 @@ void Copter::tuning() break; case TUNING_ACCEL_Z_KP: - pos_control->get_accel_z_pid().kP(tuning_value); + pos_control->get_accel_z_pid().set_kP(tuning_value); break; case TUNING_ACCEL_Z_KI: - pos_control->get_accel_z_pid().kI(tuning_value); + pos_control->get_accel_z_pid().set_kI(tuning_value); break; case TUNING_ACCEL_Z_KD: - pos_control->get_accel_z_pid().kD(tuning_value); + pos_control->get_accel_z_pid().set_kD(tuning_value); break; // Loiter and navigation tuning @@ -120,15 +120,15 @@ void Copter::tuning() break; case TUNING_RATE_PITCH_FF: - attitude_control->get_rate_pitch_pid().ff(tuning_value); + attitude_control->get_rate_pitch_pid().set_ff(tuning_value); break; case TUNING_RATE_ROLL_FF: - attitude_control->get_rate_roll_pid().ff(tuning_value); + attitude_control->get_rate_roll_pid().set_ff(tuning_value); break; case TUNING_RATE_YAW_FF: - attitude_control->get_rate_yaw_pid().ff(tuning_value); + attitude_control->get_rate_yaw_pid().set_ff(tuning_value); break; #endif @@ -154,27 +154,27 @@ void Copter::tuning() break; case TUNING_RATE_PITCH_KP: - attitude_control->get_rate_pitch_pid().kP(tuning_value); + attitude_control->get_rate_pitch_pid().set_kP(tuning_value); break; case TUNING_RATE_PITCH_KI: - attitude_control->get_rate_pitch_pid().kI(tuning_value); + attitude_control->get_rate_pitch_pid().set_kI(tuning_value); break; case TUNING_RATE_PITCH_KD: - attitude_control->get_rate_pitch_pid().kD(tuning_value); + attitude_control->get_rate_pitch_pid().set_kD(tuning_value); break; case TUNING_RATE_ROLL_KP: - attitude_control->get_rate_roll_pid().kP(tuning_value); + attitude_control->get_rate_roll_pid().set_kP(tuning_value); break; case TUNING_RATE_ROLL_KI: - attitude_control->get_rate_roll_pid().kI(tuning_value); + attitude_control->get_rate_roll_pid().set_kI(tuning_value); break; case TUNING_RATE_ROLL_KD: - attitude_control->get_rate_roll_pid().kD(tuning_value); + attitude_control->get_rate_roll_pid().set_kD(tuning_value); break; #if FRAME_CONFIG != HELI_FRAME @@ -184,7 +184,7 @@ void Copter::tuning() #endif case TUNING_RATE_YAW_FILT: - attitude_control->get_rate_yaw_pid().filt_E_hz(tuning_value); + attitude_control->get_rate_yaw_pid().set_filt_E_hz(tuning_value); break; #if WINCH_ENABLED == ENABLED diff --git a/ArduPlane/AP_Vehicle.cpp b/ArduPlane/AP_Vehicle.cpp new file mode 100644 index 0000000000000..6fb2524eede89 --- /dev/null +++ b/ArduPlane/AP_Vehicle.cpp @@ -0,0 +1,79 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +#include "Plane.h" + +// set target location (for use by scripting) +bool Plane::set_target_location(const Location& target_loc) +{ + if (plane.control_mode != &plane.mode_guided) { + // only accept position updates when in GUIDED mode + return false; + } + plane.guided_WP_loc = target_loc; + // add home alt if needed + if (plane.guided_WP_loc.relative_alt) { + plane.guided_WP_loc.alt += plane.home.alt; + plane.guided_WP_loc.relative_alt = 0; + } + plane.set_guided_WP(); + return true; +} + +// set target location (for use by scripting) +bool Plane::get_target_location(Location& target_loc) +{ + switch (control_mode->mode_number()) { + case Mode::Number::RTL: + case Mode::Number::AVOID_ADSB: + case Mode::Number::GUIDED: + case Mode::Number::AUTO: + case Mode::Number::LOITER: + case Mode::Number::QLOITER: + case Mode::Number::QLAND: + case Mode::Number::QRTL: + target_loc = next_WP_loc; + return true; + break; + default: + break; + } + return false; +} + +AC_PID* Plane::get_AC_PID(AC_PID_TYPE type) +{ + AC_PID *ret = nullptr; + + if (plane.quadplane.enabled()) { + switch(type) { + case AC_PID_TYPE::RATE_ROLL: + ret = &quadplane.attitude_control->get_rate_roll_pid(); + break; + + case AC_PID_TYPE::RATE_PITCH: + ret = &quadplane.attitude_control->get_rate_pitch_pid(); + break; + + case AC_PID_TYPE::RATE_YAW: + ret = &quadplane.attitude_control->get_rate_yaw_pid(); + break; + + default: + break; + } + } + + return ret; +} diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 3302dba700e5e..930cacfdb6daa 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -705,42 +705,4 @@ void Plane::publish_osd_info() } #endif -// set target location (for use by scripting) -bool Plane::set_target_location(const Location& target_loc) -{ - if (plane.control_mode != &plane.mode_guided) { - // only accept position updates when in GUIDED mode - return false; - } - plane.guided_WP_loc = target_loc; - // add home alt if needed - if (plane.guided_WP_loc.relative_alt) { - plane.guided_WP_loc.alt += plane.home.alt; - plane.guided_WP_loc.relative_alt = 0; - } - plane.set_guided_WP(); - return true; -} - -// set target location (for use by scripting) -bool Plane::get_target_location(Location& target_loc) -{ - switch (control_mode->mode_number()) { - case Mode::Number::RTL: - case Mode::Number::AVOID_ADSB: - case Mode::Number::GUIDED: - case Mode::Number::AUTO: - case Mode::Number::LOITER: - case Mode::Number::QLOITER: - case Mode::Number::QLAND: - case Mode::Number::QRTL: - target_loc = next_WP_loc; - return true; - break; - default: - break; - } - return false; -} - AP_HAL_MAIN_CALLBACKS(&plane); diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 5f28014e889cb..3778d92f5e511 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -785,28 +785,9 @@ class Plane : public AP_Vehicle { // 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 @@ class Plane : public AP_Vehicle { 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 @@ class Plane : public AP_Vehicle { 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 @@ class Plane : public AP_Vehicle { 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 @@ class Plane : public AP_Vehicle { 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 @@ class Plane : public AP_Vehicle { 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 @@ class Plane : public AP_Vehicle { 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 @@ class Plane : public AP_Vehicle { 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,82 +1044,45 @@ class Plane : public AP_Vehicle { 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; + // UNUSED // support for AP_Avoidance custom flight mode, AVOID_ADSB bool avoid_adsb_init(bool ignore_checks); void avoid_adsb_run(); + void mavlink_delay(uint32_t ms); + void update_optical_flow(void); enum Failsafe_Action { Failsafe_Action_None = 0, @@ -1074,8 +1115,12 @@ class Plane : public AP_Vehicle { public: void failsafe_check(void); + + // AP_Vehicle.h bool set_target_location(const Location& target_loc) override; bool get_target_location(Location& target_loc) override; + AC_PID *get_AC_PID(AC_PID_TYPE type) override; + }; extern Plane plane; diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 894cd0b34e058..4705c0ccec796 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -1,7 +1,6 @@ #pragma once #include -#include #include // Attitude control library #include #include diff --git a/Rover/AP_Vehicle.cpp b/Rover/AP_Vehicle.cpp new file mode 100644 index 0000000000000..c4e22c2b64a7e --- /dev/null +++ b/Rover/AP_Vehicle.cpp @@ -0,0 +1,74 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +#include "Rover.h" + +// set target location (for use by scripting) +bool Rover::set_target_location(const Location& target_loc) +{ + // exit if vehicle is not in Guided mode or Auto-Guided mode + if (!control_mode->in_guided_mode()) { + return false; + } + + return control_mode->set_desired_location(target_loc); +} + +// set target velocity (for use by scripting) +bool Rover::set_target_velocity_NED(const Vector3f& vel_ned) +{ + // exit if vehicle is not in Guided mode or Auto-Guided mode + if (!control_mode->in_guided_mode()) { + return false; + } + + // convert vector length into speed + const float target_speed_m = safe_sqrt(sq(vel_ned.x) + sq(vel_ned.y)); + + // convert vector direction to target yaw + const float target_yaw_cd = degrees(atan2f(vel_ned.y, vel_ned.x)) * 100.0f; + + // send target heading and speed + mode_guided.set_desired_heading_and_speed(target_yaw_cd, target_speed_m); + + return true; +} + +AC_PID* Rover::get_AC_PID(AC_PID_TYPE type) +{ + AC_PID *ret = nullptr; + + switch(type) { + case AC_PID_TYPE::RATE_STEER: + ret = &g2.attitude_control.get_steering_rate_pid(); + break; + + case AC_PID_TYPE::THROTTLE_SPEED: + ret = &g2.attitude_control.get_throttle_speed_pid(); + break; + + case AC_PID_TYPE::PITCH_THROTTLE: + ret = &g2.attitude_control.get_pitch_to_throttle_pid(); + break; + + case AC_PID_TYPE::SAILBOAT_HEEL: + ret = &g2.attitude_control.get_sailboat_heel_pid(); + break; + + default: + break; + } + + return ret; +} diff --git a/Rover/Rover.cpp b/Rover/Rover.cpp index 4c89582d337dd..fae2b05522330 100644 --- a/Rover/Rover.cpp +++ b/Rover/Rover.cpp @@ -136,37 +136,6 @@ Rover::Rover(void) : { } -// set target location (for use by scripting) -bool Rover::set_target_location(const Location& target_loc) -{ - // exit if vehicle is not in Guided mode or Auto-Guided mode - if (!control_mode->in_guided_mode()) { - return false; - } - - return control_mode->set_desired_location(target_loc); -} - -// set target velocity (for use by scripting) -bool Rover::set_target_velocity_NED(const Vector3f& vel_ned) -{ - // exit if vehicle is not in Guided mode or Auto-Guided mode - if (!control_mode->in_guided_mode()) { - return false; - } - - // convert vector length into speed - const float target_speed_m = safe_sqrt(sq(vel_ned.x) + sq(vel_ned.y)); - - // convert vector direction to target yaw - const float target_yaw_cd = degrees(atan2f(vel_ned.y, vel_ned.x)) * 100.0f; - - // send target heading and speed - mode_guided.set_desired_heading_and_speed(target_yaw_cd, target_speed_m); - - return true; -} - #if STATS_ENABLED == ENABLED /* update AP_Stats @@ -178,7 +147,6 @@ void Rover::stats_update(void) } #endif - // update AHRS system void Rover::ahrs_update() { diff --git a/Rover/Rover.h b/Rover/Rover.h index 4f204ac8802ff..1c6f4ed70b789 100644 --- a/Rover/Rover.h +++ b/Rover/Rover.h @@ -24,7 +24,6 @@ #include #include #include -#include #include // interface and maths for accelerometer calibration #include // ArduPilot Mega DCM Library #include // needed for AHRS build @@ -275,9 +274,12 @@ class Rover : public AP_Vehicle { private: - // Rover.cpp + // AP_Vehicle.cpp bool set_target_location(const Location& target_loc) override; bool set_target_velocity_NED(const Vector3f& vel_ned) override; + AC_PID *get_AC_PID(AC_PID_TYPE type) override; + + // Rover.cpp void stats_update(); void ahrs_update(); void gcs_failsafe_check(void); diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 7e5a61941f52f..769d0c47fba80 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -629,7 +629,7 @@ void AC_PosControl::run_z_controller() // ensure imax is always large enough to overpower hover throttle if (_motors.get_throttle_hover() * 1000.0f > _pid_accel_z.imax()) { - _pid_accel_z.imax(_motors.get_throttle_hover() * 1000.0f); + _pid_accel_z.set_imax(_motors.get_throttle_hover() * 1000.0f); } float thr_out; diff --git a/libraries/AC_AutoTune/AC_AutoTune.cpp b/libraries/AC_AutoTune/AC_AutoTune.cpp index 24bb3743c96af..f80570b003a57 100644 --- a/libraries/AC_AutoTune/AC_AutoTune.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune.cpp @@ -965,31 +965,31 @@ void AC_AutoTune::load_orig_gains() attitude_control->bf_feedforward(orig_bf_feedforward); if (roll_enabled()) { if (!is_zero(orig_roll_rp)) { - attitude_control->get_rate_roll_pid().kP(orig_roll_rp); - attitude_control->get_rate_roll_pid().kI(orig_roll_ri); - attitude_control->get_rate_roll_pid().kD(orig_roll_rd); - attitude_control->get_rate_roll_pid().ff(orig_roll_rff); + attitude_control->get_rate_roll_pid().set_kP(orig_roll_rp); + attitude_control->get_rate_roll_pid().set_kI(orig_roll_ri); + attitude_control->get_rate_roll_pid().set_kD(orig_roll_rd); + attitude_control->get_rate_roll_pid().set_ff(orig_roll_rff); attitude_control->get_angle_roll_p().kP(orig_roll_sp); attitude_control->set_accel_roll_max(orig_roll_accel); } } if (pitch_enabled()) { if (!is_zero(orig_pitch_rp)) { - attitude_control->get_rate_pitch_pid().kP(orig_pitch_rp); - attitude_control->get_rate_pitch_pid().kI(orig_pitch_ri); - attitude_control->get_rate_pitch_pid().kD(orig_pitch_rd); - attitude_control->get_rate_pitch_pid().ff(orig_pitch_rff); + attitude_control->get_rate_pitch_pid().set_kP(orig_pitch_rp); + attitude_control->get_rate_pitch_pid().set_kI(orig_pitch_ri); + attitude_control->get_rate_pitch_pid().set_kD(orig_pitch_rd); + attitude_control->get_rate_pitch_pid().set_ff(orig_pitch_rff); attitude_control->get_angle_pitch_p().kP(orig_pitch_sp); attitude_control->set_accel_pitch_max(orig_pitch_accel); } } if (yaw_enabled()) { if (!is_zero(orig_yaw_rp)) { - attitude_control->get_rate_yaw_pid().kP(orig_yaw_rp); - attitude_control->get_rate_yaw_pid().kI(orig_yaw_ri); - attitude_control->get_rate_yaw_pid().kD(orig_yaw_rd); - attitude_control->get_rate_yaw_pid().ff(orig_yaw_rff); - attitude_control->get_rate_yaw_pid().filt_E_hz(orig_yaw_rLPF); + attitude_control->get_rate_yaw_pid().set_kP(orig_yaw_rp); + attitude_control->get_rate_yaw_pid().set_kI(orig_yaw_ri); + attitude_control->get_rate_yaw_pid().set_kD(orig_yaw_rd); + attitude_control->get_rate_yaw_pid().set_ff(orig_yaw_rff); + attitude_control->get_rate_yaw_pid().set_filt_E_hz(orig_yaw_rLPF); attitude_control->get_angle_yaw_p().kP(orig_yaw_sp); attitude_control->set_accel_yaw_max(orig_yaw_accel); } @@ -1006,31 +1006,31 @@ void AC_AutoTune::load_tuned_gains() } if (roll_enabled()) { if (!is_zero(tune_roll_rp)) { - attitude_control->get_rate_roll_pid().kP(tune_roll_rp); - attitude_control->get_rate_roll_pid().kI(tune_roll_rp*AUTOTUNE_PI_RATIO_FINAL); - attitude_control->get_rate_roll_pid().kD(tune_roll_rd); - attitude_control->get_rate_roll_pid().ff(orig_roll_rff); + attitude_control->get_rate_roll_pid().set_kP(tune_roll_rp); + attitude_control->get_rate_roll_pid().set_kI(tune_roll_rp*AUTOTUNE_PI_RATIO_FINAL); + attitude_control->get_rate_roll_pid().set_kD(tune_roll_rd); + attitude_control->get_rate_roll_pid().set_ff(orig_roll_rff); attitude_control->get_angle_roll_p().kP(tune_roll_sp); attitude_control->set_accel_roll_max(tune_roll_accel); } } if (pitch_enabled()) { if (!is_zero(tune_pitch_rp)) { - attitude_control->get_rate_pitch_pid().kP(tune_pitch_rp); - attitude_control->get_rate_pitch_pid().kI(tune_pitch_rp*AUTOTUNE_PI_RATIO_FINAL); - attitude_control->get_rate_pitch_pid().kD(tune_pitch_rd); - attitude_control->get_rate_pitch_pid().ff(orig_pitch_rff); + attitude_control->get_rate_pitch_pid().set_kP(tune_pitch_rp); + attitude_control->get_rate_pitch_pid().set_kI(tune_pitch_rp*AUTOTUNE_PI_RATIO_FINAL); + attitude_control->get_rate_pitch_pid().set_kD(tune_pitch_rd); + attitude_control->get_rate_pitch_pid().set_ff(orig_pitch_rff); attitude_control->get_angle_pitch_p().kP(tune_pitch_sp); attitude_control->set_accel_pitch_max(tune_pitch_accel); } } if (yaw_enabled()) { if (!is_zero(tune_yaw_rp)) { - attitude_control->get_rate_yaw_pid().kP(tune_yaw_rp); - attitude_control->get_rate_yaw_pid().kI(tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL); - attitude_control->get_rate_yaw_pid().kD(0.0f); - attitude_control->get_rate_yaw_pid().ff(orig_yaw_rff); - attitude_control->get_rate_yaw_pid().filt_E_hz(tune_yaw_rLPF); + attitude_control->get_rate_yaw_pid().set_kP(tune_yaw_rp); + attitude_control->get_rate_yaw_pid().set_kI(tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL); + attitude_control->get_rate_yaw_pid().set_kD(0.0f); + attitude_control->get_rate_yaw_pid().set_ff(orig_yaw_rff); + attitude_control->get_rate_yaw_pid().set_filt_E_hz(tune_yaw_rLPF); attitude_control->get_angle_yaw_p().kP(tune_yaw_sp); attitude_control->set_accel_yaw_max(tune_yaw_accel); } @@ -1045,25 +1045,25 @@ void AC_AutoTune::load_intra_test_gains() // sanity check the gains attitude_control->bf_feedforward(true); if (roll_enabled()) { - attitude_control->get_rate_roll_pid().kP(orig_roll_rp); - attitude_control->get_rate_roll_pid().kI(orig_roll_rp*AUTOTUNE_PI_RATIO_FOR_TESTING); - attitude_control->get_rate_roll_pid().kD(orig_roll_rd); - attitude_control->get_rate_roll_pid().ff(orig_roll_rff); + attitude_control->get_rate_roll_pid().set_kP(orig_roll_rp); + attitude_control->get_rate_roll_pid().set_kI(orig_roll_rp*AUTOTUNE_PI_RATIO_FOR_TESTING); + attitude_control->get_rate_roll_pid().set_kD(orig_roll_rd); + attitude_control->get_rate_roll_pid().set_ff(orig_roll_rff); attitude_control->get_angle_roll_p().kP(orig_roll_sp); } if (pitch_enabled()) { - attitude_control->get_rate_pitch_pid().kP(orig_pitch_rp); - attitude_control->get_rate_pitch_pid().kI(orig_pitch_rp*AUTOTUNE_PI_RATIO_FOR_TESTING); - attitude_control->get_rate_pitch_pid().kD(orig_pitch_rd); - attitude_control->get_rate_pitch_pid().ff(orig_pitch_rff); + attitude_control->get_rate_pitch_pid().set_kP(orig_pitch_rp); + attitude_control->get_rate_pitch_pid().set_kI(orig_pitch_rp*AUTOTUNE_PI_RATIO_FOR_TESTING); + attitude_control->get_rate_pitch_pid().set_kD(orig_pitch_rd); + attitude_control->get_rate_pitch_pid().set_ff(orig_pitch_rff); attitude_control->get_angle_pitch_p().kP(orig_pitch_sp); } if (yaw_enabled()) { - attitude_control->get_rate_yaw_pid().kP(orig_yaw_rp); - attitude_control->get_rate_yaw_pid().kI(orig_yaw_rp*AUTOTUNE_PI_RATIO_FOR_TESTING); - attitude_control->get_rate_yaw_pid().kD(orig_yaw_rd); - attitude_control->get_rate_yaw_pid().ff(orig_yaw_rff); - attitude_control->get_rate_yaw_pid().filt_E_hz(orig_yaw_rLPF); + attitude_control->get_rate_yaw_pid().set_kP(orig_yaw_rp); + attitude_control->get_rate_yaw_pid().set_kI(orig_yaw_rp*AUTOTUNE_PI_RATIO_FOR_TESTING); + attitude_control->get_rate_yaw_pid().set_kD(orig_yaw_rd); + attitude_control->get_rate_yaw_pid().set_ff(orig_yaw_rff); + attitude_control->get_rate_yaw_pid().set_filt_E_hz(orig_yaw_rLPF); attitude_control->get_angle_yaw_p().kP(orig_yaw_sp); } } @@ -1074,25 +1074,25 @@ void AC_AutoTune::load_twitch_gains() { switch (axis) { case ROLL: - attitude_control->get_rate_roll_pid().kP(tune_roll_rp); - attitude_control->get_rate_roll_pid().kI(tune_roll_rp*0.01f); - attitude_control->get_rate_roll_pid().kD(tune_roll_rd); - attitude_control->get_rate_roll_pid().ff(0.0f); + attitude_control->get_rate_roll_pid().set_kP(tune_roll_rp); + attitude_control->get_rate_roll_pid().set_kI(tune_roll_rp*0.01f); + attitude_control->get_rate_roll_pid().set_kD(tune_roll_rd); + attitude_control->get_rate_roll_pid().set_ff(0.0f); attitude_control->get_angle_roll_p().kP(tune_roll_sp); break; case PITCH: - attitude_control->get_rate_pitch_pid().kP(tune_pitch_rp); - attitude_control->get_rate_pitch_pid().kI(tune_pitch_rp*0.01f); - attitude_control->get_rate_pitch_pid().kD(tune_pitch_rd); - attitude_control->get_rate_pitch_pid().ff(0.0f); + attitude_control->get_rate_pitch_pid().set_kP(tune_pitch_rp); + attitude_control->get_rate_pitch_pid().set_kI(tune_pitch_rp*0.01f); + attitude_control->get_rate_pitch_pid().set_kD(tune_pitch_rd); + attitude_control->get_rate_pitch_pid().set_ff(0.0f); attitude_control->get_angle_pitch_p().kP(tune_pitch_sp); break; case YAW: - attitude_control->get_rate_yaw_pid().kP(tune_yaw_rp); - attitude_control->get_rate_yaw_pid().kI(tune_yaw_rp*0.01f); - attitude_control->get_rate_yaw_pid().kD(0.0f); - attitude_control->get_rate_yaw_pid().ff(0.0f); - attitude_control->get_rate_yaw_pid().filt_E_hz(tune_yaw_rLPF); + attitude_control->get_rate_yaw_pid().set_kP(tune_yaw_rp); + attitude_control->get_rate_yaw_pid().set_kI(tune_yaw_rp*0.01f); + attitude_control->get_rate_yaw_pid().set_kD(0.0f); + attitude_control->get_rate_yaw_pid().set_ff(0.0f); + attitude_control->get_rate_yaw_pid().set_filt_E_hz(tune_yaw_rLPF); attitude_control->get_angle_yaw_p().kP(tune_yaw_sp); break; } @@ -1137,10 +1137,10 @@ void AC_AutoTune::save_tuning_gains() // sanity check the rate P values if ((axes_completed & AUTOTUNE_AXIS_BITMASK_ROLL) && roll_enabled() && !is_zero(tune_roll_rp)) { // rate roll gains - attitude_control->get_rate_roll_pid().kP(tune_roll_rp); - attitude_control->get_rate_roll_pid().kI(tune_roll_rp*AUTOTUNE_PI_RATIO_FINAL); - attitude_control->get_rate_roll_pid().kD(tune_roll_rd); - attitude_control->get_rate_roll_pid().ff(orig_roll_rff); + attitude_control->get_rate_roll_pid().set_kP(tune_roll_rp); + attitude_control->get_rate_roll_pid().set_kI(tune_roll_rp*AUTOTUNE_PI_RATIO_FINAL); + attitude_control->get_rate_roll_pid().set_kD(tune_roll_rd); + attitude_control->get_rate_roll_pid().set_ff(orig_roll_rff); attitude_control->get_rate_roll_pid().save_gains(); // stabilize roll @@ -1161,10 +1161,10 @@ void AC_AutoTune::save_tuning_gains() if ((axes_completed & AUTOTUNE_AXIS_BITMASK_PITCH) && pitch_enabled() && !is_zero(tune_pitch_rp)) { // rate pitch gains - attitude_control->get_rate_pitch_pid().kP(tune_pitch_rp); - attitude_control->get_rate_pitch_pid().kI(tune_pitch_rp*AUTOTUNE_PI_RATIO_FINAL); - attitude_control->get_rate_pitch_pid().kD(tune_pitch_rd); - attitude_control->get_rate_pitch_pid().ff(orig_pitch_rff); + attitude_control->get_rate_pitch_pid().set_kP(tune_pitch_rp); + attitude_control->get_rate_pitch_pid().set_kI(tune_pitch_rp*AUTOTUNE_PI_RATIO_FINAL); + attitude_control->get_rate_pitch_pid().set_kD(tune_pitch_rd); + attitude_control->get_rate_pitch_pid().set_ff(orig_pitch_rff); attitude_control->get_rate_pitch_pid().save_gains(); // stabilize pitch @@ -1185,11 +1185,11 @@ void AC_AutoTune::save_tuning_gains() if ((axes_completed & AUTOTUNE_AXIS_BITMASK_YAW) && yaw_enabled() && !is_zero(tune_yaw_rp)) { // rate yaw gains - attitude_control->get_rate_yaw_pid().kP(tune_yaw_rp); - attitude_control->get_rate_yaw_pid().kI(tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL); - attitude_control->get_rate_yaw_pid().kD(0.0f); - attitude_control->get_rate_yaw_pid().ff(orig_yaw_rff); - attitude_control->get_rate_yaw_pid().filt_E_hz(tune_yaw_rLPF); + attitude_control->get_rate_yaw_pid().set_kP(tune_yaw_rp); + attitude_control->get_rate_yaw_pid().set_kI(tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL); + attitude_control->get_rate_yaw_pid().set_kD(0.0f); + attitude_control->get_rate_yaw_pid().set_ff(orig_yaw_rff); + attitude_control->get_rate_yaw_pid().set_filt_E_hz(tune_yaw_rLPF); attitude_control->get_rate_yaw_pid().save_gains(); // stabilize yaw diff --git a/libraries/AC_PID/AC_PID.cpp b/libraries/AC_PID/AC_PID.cpp index 8ea4a5fb0968c..1d3f63a67ee33 100644 --- a/libraries/AC_PID/AC_PID.cpp +++ b/libraries/AC_PID/AC_PID.cpp @@ -74,9 +74,9 @@ AC_PID::AC_PID(float initial_p, float initial_i, float initial_d, float initial_ _kd = initial_d; _kff = initial_ff; _kimax = fabsf(initial_imax); - filt_T_hz(initial_filt_T_hz); - filt_E_hz(initial_filt_E_hz); - filt_D_hz(initial_filt_D_hz); + set_filt_T_hz(initial_filt_T_hz); + set_filt_E_hz(initial_filt_E_hz); + set_filt_D_hz(initial_filt_D_hz); // reset input filter to first value received _flags._reset_filter = true; @@ -92,19 +92,19 @@ void AC_PID::set_dt(float dt) } // filt_T_hz - set target filter hz -void AC_PID::filt_T_hz(float hz) +void AC_PID::set_filt_T_hz(float hz) { _filt_T_hz.set(fabsf(hz)); } // filt_E_hz - set error filter hz -void AC_PID::filt_E_hz(float hz) +void AC_PID::set_filt_E_hz(float hz) { _filt_E_hz.set(fabsf(hz)); } // filt_D_hz - set derivative filter hz -void AC_PID::filt_D_hz(float hz) +void AC_PID::set_filt_D_hz(float hz) { _filt_D_hz.set(fabsf(hz)); } diff --git a/libraries/AC_PID/AC_PID.h b/libraries/AC_PID/AC_PID.h index 631eb1c28fbe6..19396b1c972f8 100644 --- a/libraries/AC_PID/AC_PID.h +++ b/libraries/AC_PID/AC_PID.h @@ -85,14 +85,14 @@ class AC_PID { float get_filt_D_alpha() const; // set accessors - void kP(const float v) { _kp.set(v); } - void kI(const float v) { _ki.set(v); } - void kD(const float v) { _kd.set(v); } - void ff(const float v) { _kff.set(v); } - void imax(const float v) { _kimax.set(fabsf(v)); } - void filt_T_hz(const float v); - void filt_E_hz(const float v); - void filt_D_hz(const float v); + void set_kP(const float v) { _kp.set(v); } + void set_kI(const float v) { _ki.set(v); } + void set_kD(const float v) { _kd.set(v); } + void set_ff(const float v) { _kff.set(v); } + void set_imax(const float v) { _kimax.set(fabsf(v)); } + void set_filt_T_hz(const float v); + void set_filt_E_hz(const float v); + void set_filt_D_hz(const float v); // set the desired and actual rates (for logging purposes) void set_target_rate(float target) { _pid_info.target = target; } diff --git a/libraries/AP_Scripting/examples/AC_PID_get.lua b/libraries/AP_Scripting/examples/AC_PID_get.lua new file mode 100644 index 0000000000000..b6fb132b10f84 --- /dev/null +++ b/libraries/AP_Scripting/examples/AC_PID_get.lua @@ -0,0 +1,76 @@ +-- Example of accessing AC_PID objects, not all are available on all vehicles + +-- Copter, also used by quadplanes +local roll_pid = vehicle:get_AC_PID(vehicle.RATE_ROLL) +local pitch_pid = vehicle:get_AC_PID(vehicle.RATE_PITCH) +local yaw_pid = vehicle:get_AC_PID(vehicle.RATE_YAW) + +-- Rover +local steer_pid = vehicle:get_AC_PID(vehicle.RATE_STEER) +local throttle_pid = vehicle:get_AC_PID(vehicle.THROTTLE_SPEED) +local pitch_throttle = vehicle:get_AC_PID(vehicle.PITCH_THROTTLE) +local sail_heel = vehicle:get_AC_PID(vehicle.SAILBOAT_HEEL) + +-- Tracker +local tracker_pitch = vehicle:get_AC_PID(vehicle.TRACKER_PITCH) +local tracker_yaw = vehicle:get_AC_PID(vehicle.TRACKER_YAW) + +function print_pid(type, object) + gcs:send_text(0,string.format("%s: P %0.2f, I %0.2f, D %0.2f",type,object:kP(),object:kI(),object:kD())) +end + +function update() + -- Print the gain values from the PID controllers + if roll_pid and pitch_pid and yaw_pid then -- copter and quadplane + print_pid('Roll',roll_pid) + print_pid('Pitch',pitch_pid) + print_pid('Yaw',yaw_pid) + end + + if steer_pid and throttle_pid and pitch_throttle and sail_heel then -- rover + print_pid('Steer',steer_pid) + print_pid('Throttle',throttle_pid) + print_pid('Pitch',pitch_throttle) + print_pid('heel',sail_heel) + end + + if tracker_pitch and tracker_yaw then -- tracker + print_pid('Pitch',tracker_pitch) + print_pid('Yaw',tracker_yaw) + end + + return update, 10000 -- reschedules the loop in 10 seconds +end + + -- All methods available + --[[ + reset_I() + reset_filter() + + -- get accessors + kP() + kI() + kD() + ff() + filt_T_hz() + filt_E_hz() + filt_D_hz() + imax() + get_filt_alpha(float) + get_filt_T_alpha() + get_filt_E_alpha() + get_filt_D_alpha() + + -- set accessors, only saved permanently once save_gains() is called + set_kP(float) + set_kI(float) + set_kD(float) + set_ff(float) + set_imax(float) + set_filt_T_hz(float) + set_filt_E_hz(float) + set_filt_D_hz(float) + save_gains() + ]]-- + +return update() -- run immediately before starting to reschedule diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index 4b0538bfdbc7b..a6e5d0b61538b 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -242,3 +242,37 @@ singleton AP_RPM method get_rpm boolean uint8_t 0 RPM_MAX_INSTANCES float'Null include AP_Button/AP_Button.h singleton AP_Button alias button singleton AP_Button method get_button_state boolean uint8_t 1 AP_BUTTON_NUM_PINS + + +include AC_PID/AC_PID.h +ap_object AC_PID method reset_I void +ap_object AC_PID method reset_filter void + +-- get accessors +ap_object AC_PID method kP float +ap_object AC_PID method kI float +ap_object AC_PID method kD float +ap_object AC_PID method ff float +ap_object AC_PID method filt_T_hz float +ap_object AC_PID method filt_E_hz float +ap_object AC_PID method filt_D_hz float +ap_object AC_PID method imax float +ap_object AC_PID method get_filt_alpha float float 0 FLT_MAX +ap_object AC_PID method get_filt_T_alpha float +ap_object AC_PID method get_filt_E_alpha float +ap_object AC_PID method get_filt_D_alpha float + +-- set accessors +ap_object AC_PID method set_kP void float -FLT_MAX FLT_MAX +ap_object AC_PID method set_kI void float -FLT_MAX FLT_MAX +ap_object AC_PID method set_kD void float -FLT_MAX FLT_MAX +ap_object AC_PID method set_ff void float -FLT_MAX FLT_MAX +ap_object AC_PID method set_imax void float 0 FLT_MAX +ap_object AC_PID method set_filt_T_hz void float 0 FLT_MAX +ap_object AC_PID method set_filt_E_hz void float 0 FLT_MAX +ap_object AC_PID method set_filt_D_hz void float 0 FLT_MAX +ap_object AC_PID method save_gains void + +-- get AC_PID object from vehicle +singleton AP_Vehicle method get_AC_PID AC_PID AP_Vehicle::AC_PID_TYPE'enum AP_Vehicle::AC_PID_TYPE::RATE_ROLL AP_Vehicle::AC_PID_TYPE::TRACKER_YAW +singleton AP_Vehicle enum RATE_ROLL RATE_PITCH RATE_YAW RATE_STEER THROTTLE_SPEED PITCH_THROTTLE SAILBOAT_HEEL TRACKER_PITCH TRACKER_YAW diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index ec56e3a9dde19..eb060c0bf8a0f 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -40,6 +40,7 @@ #include #include #include +#include class AP_Vehicle : public AP_HAL::HAL::Callbacks { @@ -90,7 +91,7 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks { AP_Int8 crash_detection_enable; AP_Int16 roll_limit_cd; AP_Int16 pitch_limit_max_cd; - AP_Int16 pitch_limit_min_cd; + AP_Int16 pitch_limit_min_cd; AP_Int8 autotune_level; AP_Int8 stall_prevention; AP_Int16 loiter_radius; @@ -173,7 +174,23 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks { // get target location (for use by scripting) virtual bool get_target_location(Location& target_loc) { return false; } - + + // PID type to return with get_AC_PID + enum AC_PID_TYPE { + RATE_ROLL = 0, + RATE_PITCH = 1, + RATE_YAW = 2, + RATE_STEER = 3, + THROTTLE_SPEED = 4, + PITCH_THROTTLE = 5, + SAILBOAT_HEEL = 6, + TRACKER_PITCH = 7, + TRACKER_YAW = 8, + }; + + // get AC_PID object for use by scripting + virtual AC_PID* get_AC_PID(AC_PID_TYPE type) { return nullptr; }; + protected: virtual void init_ardupilot() = 0;