From b5d22ce47c210da92c1dcc2d58721e34d24e5ccb Mon Sep 17 00:00:00 2001 From: Michelle Rossouw Date: Thu, 27 Jul 2023 15:06:32 +1000 Subject: [PATCH] Blimp: Remove extra defines and other code, fixup some formatting and naming --- Blimp/Blimp.h | 6 +- Blimp/GCS_Mavlink.cpp | 4 +- Blimp/GCS_Mavlink.h | 2 +- Blimp/Log.cpp | 7 +- Blimp/Parameters.cpp | 13 +- Blimp/Parameters.h | 5 +- Blimp/config.h | 317 ------------------------------------------ Blimp/defines.h | 108 +------------- Blimp/ekf_check.cpp | 6 +- Blimp/events.cpp | 6 +- Blimp/mode.h | 59 -------- Blimp/mode_land.cpp | 7 +- Blimp/wscript | 3 - 13 files changed, 23 insertions(+), 520 deletions(-) diff --git a/Blimp/Blimp.h b/Blimp/Blimp.h index d106c85f68..8923e6bd1c 100644 --- a/Blimp/Blimp.h +++ b/Blimp/Blimp.h @@ -364,8 +364,8 @@ private: void Log_Write_Data(LogDataID id, float value); void Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, float tune_min, float tune_max); void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target); - void Log_Write_SysID_Setup(uint8_t systemID_axis, float waveform_magnitude, float frequency_start, float frequency_stop, float time_fade_in, float time_const_freq, float time_record, float time_fade_out); - void Log_Write_SysID_Data(float waveform_time, float waveform_sample, float waveform_freq, float angle_x, float angle_y, float angle_z, float accel_x, float accel_y, float accel_z); + + void Log_Write_Vehicle_Startup_Messages(); void log_init(void); void Write_FINI(float right, float front, float down, float yaw); @@ -382,7 +382,7 @@ private: void notify_flight_mode(); // mode_land.cpp - void set_mode_land_with_pause(ModeReason reason); + void set_mode_land_failsafe(ModeReason reason); bool landing_with_GPS(); // // motors.cpp diff --git a/Blimp/GCS_Mavlink.cpp b/Blimp/GCS_Mavlink.cpp index 5099541836..9f395b4d47 100644 --- a/Blimp/GCS_Mavlink.cpp +++ b/Blimp/GCS_Mavlink.cpp @@ -102,11 +102,11 @@ int16_t GCS_MAVLINK_Blimp::vfr_hud_throttle() const */ void GCS_MAVLINK_Blimp::send_pid_tuning() { - if(blimp.control_mode == Mode::Number::MANUAL || blimp.control_mode == Mode::Number::LAND) { + if (blimp.control_mode == Mode::Number::MANUAL || blimp.control_mode == Mode::Number::LAND) { //No PIDs are used in Manual or Land mode. return; } - + static const int8_t axes[] = { PID_SEND::VELX, PID_SEND::VELY, diff --git a/Blimp/GCS_Mavlink.h b/Blimp/GCS_Mavlink.h index 036fe40666..52b9627695 100644 --- a/Blimp/GCS_Mavlink.h +++ b/Blimp/GCS_Mavlink.h @@ -73,7 +73,7 @@ private: POSZ = 7, POSYAW = 8, }; - + #if HAL_HIGH_LATENCY2_ENABLED uint8_t high_latency_wind_speed() const override; uint8_t high_latency_wind_direction() const override; diff --git a/Blimp/Log.cpp b/Blimp/Log.cpp index 6b74b028fd..5e4df3e406 100644 --- a/Blimp/Log.cpp +++ b/Blimp/Log.cpp @@ -92,7 +92,9 @@ void Blimp::Log_Write_PIDs() // Write an attitude packet void Blimp::Log_Write_Attitude() { - + //Attitude targets are all zero since Blimp doesn't have attitude control, + //but the rest of the log message is useful. + ahrs.Write_Attitude(Vector3f{0,0,0}); } // Write an EKF and POS packet @@ -232,7 +234,6 @@ tuning_max : tune_max logger.WriteBlock(&pkt_tune, sizeof(pkt_tune)); } - // type and unit information can be found in // libraries/AP_Logger/Logstructure.h; search for "log_Units" for // units and "Format characters" for field type information @@ -403,8 +404,6 @@ void Blimp::Log_Write_Data(LogDataID id, uint16_t value) {} void Blimp::Log_Write_Data(LogDataID id, float value) {} void Blimp::Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, float tune_min, float tune_max) {} void Blimp::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target) {} -void Blimp::Log_Write_SysID_Setup(uint8_t systemID_axis, float waveform_magnitude, float frequency_start, float frequency_stop, float time_fade_in, float time_const_freq, float time_record, float time_fade_out) {} -void Blimp::Log_Write_SysID_Data(float waveform_time, float waveform_sample, float waveform_freq, float angle_x, float angle_y, float angle_z, float accel_x, float accel_y, float accel_z) {} void Blimp::Log_Write_Vehicle_Startup_Messages() {} void Blimp::log_init(void) {} diff --git a/Blimp/Parameters.cpp b/Blimp/Parameters.cpp index bdb1b2d563..6956f1eda5 100644 --- a/Blimp/Parameters.cpp +++ b/Blimp/Parameters.cpp @@ -53,15 +53,6 @@ const AP_Param::Info Blimp::var_info[] = { // @Increment: .5 GSCALAR(throttle_filt, "PILOT_THR_FILT", 0), - // @Param: PILOT_TKOFF_ALT - // @DisplayName: Pilot takeoff altitude - // @Description: Altitude that altitude control modes will climb to when a takeoff is triggered with the throttle stick. - // @User: Standard - // @Units: cm - // @Range: 0.0 1000.0 - // @Increment: 10 - GSCALAR(pilot_takeoff_alt, "PILOT_TKOFF_ALT", PILOT_TKOFF_ALT_DEFAULT), - // @Param: PILOT_THR_BHV // @DisplayName: Throttle stick behavior // @Description: Bitmask containing various throttle stick options. TX with sprung throttle can set PILOT_THR_BHV to "1" so motor feedback when landed starts from mid-stick instead of bottom of stick. @@ -184,7 +175,7 @@ const AP_Param::Info Blimp::var_info[] = { // @Param: LOG_BITMASK // @DisplayName: Log bitmask // @Description: Bitmap of what log types to enable in on-board logger. This value is made up of the sum of each of the log types you want to be saved. On boards supporting microSD cards or other large block-storage devices it is usually best just to enable all basic log types by setting this to 65535. - // @Bitmask: 0:Fast Attitude,1:Medium Attitude,2:GPS,3:System Performance,4:Control Tuning,6:RC Input,7:IMU,9:Battery Monitor,10:RC Output,11:Optical Flow,12:PID,13:Compass + // @Bitmask: 0:Fast Attitude,1:Medium Attitude,2:GPS,3:System Performance,4:Control Tuning,6:RC Input,7:IMU,9:Battery Monitor,10:RC Output,12:PID,13:Compass // @User: Standard GSCALAR(log_bitmask, "LOG_BITMASK", DEFAULT_LOG_BITMASK), @@ -272,7 +263,7 @@ const AP_Param::Info Blimp::var_info[] = { // @Param: DIS_MASK // @DisplayName: Disable output mask - // @Description: Mask for disabling one or more of the 4 output axis in mode Velocity or Loiter + // @Description: Mask for disabling (setting to zero) one or more of the 4 output axis in mode Velocity or Loiter // @Values: 0:All enabled,1:Right,2:Front,4:Down,8:Yaw,3:Down and Yaw only,12:Front & Right only // @Bitmask: 0:Right,1:Front,2:Down,3:Yaw // @User: Standard diff --git a/Blimp/Parameters.h b/Blimp/Parameters.h index 614cb46e3f..8ec466d62c 100644 --- a/Blimp/Parameters.h +++ b/Blimp/Parameters.h @@ -87,7 +87,7 @@ public: k_param_log_bitmask, k_param_throttle_filt, k_param_throttle_behavior, - k_param_pilot_takeoff_alt, + k_param_pilot_takeoff_alt, //unused // AP_ADSB Library k_param_adsb, @@ -136,7 +136,7 @@ public: k_param_gcs2, k_param_serial_manager, k_param_gcs3, - k_param_gcs_pid_mask, // 126 + k_param_gcs_pid_mask, k_param_gcs4, k_param_gcs5, k_param_gcs6, @@ -214,7 +214,6 @@ public: AP_Float throttle_filt; AP_Int16 throttle_behavior; - AP_Float pilot_takeoff_alt; AP_Int8 failsafe_gcs; // ground station failsafe behavior AP_Int16 gps_hdop_good; // GPS Hdop value at or below this value represent a good position diff --git a/Blimp/config.h b/Blimp/config.h index 10cac9ea72..9c5908f65f 100644 --- a/Blimp/config.h +++ b/Blimp/config.h @@ -22,13 +22,6 @@ # define ARMING_DELAY_SEC 2.0f #endif -////////////////////////////////////////////////////////////////////////////// -// FRAME_CONFIG -// -#ifndef FRAME_CONFIG -# define FRAME_CONFIG MULTICOPTER_FRAME -#endif - ////////////////////////////////////////////////////////////////////////////// // PWM control // default RC speed in Hz @@ -36,53 +29,6 @@ # define RC_FAST_SPEED 490 #endif -////////////////////////////////////////////////////////////////////////////// -// Rangefinder -// - -#ifndef RANGEFINDER_ENABLED -# define RANGEFINDER_ENABLED ENABLED -#endif - -#ifndef RANGEFINDER_HEALTH_MAX -# define RANGEFINDER_HEALTH_MAX 3 // number of good reads that indicates a healthy rangefinder -#endif - -#ifndef RANGEFINDER_TIMEOUT_MS -# define RANGEFINDER_TIMEOUT_MS 1000 // rangefinder filter reset if no updates from sensor in 1 second -#endif - -#ifndef RANGEFINDER_GAIN_DEFAULT -# define RANGEFINDER_GAIN_DEFAULT 0.8f // gain for controlling how quickly rangefinder range adjusts target altitude (lower means slower reaction) -#endif - -#ifndef SURFACE_TRACKING_TIMEOUT_MS -# define SURFACE_TRACKING_TIMEOUT_MS 1000 // surface tracking target alt will reset to current rangefinder alt after this many milliseconds without a good rangefinder alt -#endif - -#ifndef RANGEFINDER_WPNAV_FILT_HZ -# define RANGEFINDER_WPNAV_FILT_HZ 0.25f // filter frequency for rangefinder altitude provided to waypoint navigation class -#endif - -#ifndef RANGEFINDER_TILT_CORRECTION // by disable tilt correction for use of range finder data by EKF -# define RANGEFINDER_TILT_CORRECTION ENABLED -#endif - -#ifndef RANGEFINDER_GLITCH_ALT_CM -# define RANGEFINDER_GLITCH_ALT_CM 200 // amount of rangefinder change to be considered a glitch -#endif - -#ifndef RANGEFINDER_GLITCH_NUM_SAMPLES -# define RANGEFINDER_GLITCH_NUM_SAMPLES 3 // number of rangefinder glitches in a row to take new reading -#endif - -////////////////////////////////////////////////////////////////////////////// -// Proximity sensor -// -#ifndef PROXIMITY_ENABLED -# define PROXIMITY_ENABLED ENABLED -#endif - #ifndef MAV_SYSTEM_ID # define MAV_SYSTEM_ID 1 #endif @@ -125,100 +71,9 @@ # define EKF_ORIGIN_MAX_DIST_M 50000 // EKF origin and waypoints (including home) must be within 50km #endif -////////////////////////////////////////////////////////////////////////////// -// OPTICAL_FLOW -#ifndef OPTFLOW -# define OPTFLOW ENABLED -#endif - -////////////////////////////////////////////////////////////////////////////// -// Auto Tuning -#ifndef AUTOTUNE_ENABLED -# define AUTOTUNE_ENABLED ENABLED -#endif - -////////////////////////////////////////////////////////////////////////////// -// Parachute release -#ifndef PARACHUTE -# define PARACHUTE HAL_PARACHUTE_ENABLED -#endif - -////////////////////////////////////////////////////////////////////////////// -// Acro - fly vehicle in acrobatic mode -#ifndef MODE_ACRO_ENABLED -# define MODE_ACRO_ENABLED ENABLED -#endif - -////////////////////////////////////////////////////////////////////////////// -// Auto mode - allows vehicle to trace waypoints and perform automated actions -#ifndef MODE_AUTO_ENABLED -# define MODE_AUTO_ENABLED ENABLED -#endif - -////////////////////////////////////////////////////////////////////////////// -// Brake mode - bring vehicle to stop -#ifndef MODE_BRAKE_ENABLED -# define MODE_BRAKE_ENABLED ENABLED -#endif - -////////////////////////////////////////////////////////////////////////////// -// Circle - fly vehicle around a central point -#ifndef MODE_CIRCLE_ENABLED -# define MODE_CIRCLE_ENABLED ENABLED -#endif - -////////////////////////////////////////////////////////////////////////////// -// Drift - fly vehicle in altitude-held, coordinated-turn mode -#ifndef MODE_DRIFT_ENABLED -# define MODE_DRIFT_ENABLED ENABLED -#endif - -////////////////////////////////////////////////////////////////////////////// -// flip - fly vehicle in flip in pitch and roll direction mode -#ifndef MODE_FLIP_ENABLED -# define MODE_FLIP_ENABLED ENABLED -#endif - -////////////////////////////////////////////////////////////////////////////// -// Guided mode - control vehicle's position or angles from GCS -#ifndef MODE_GUIDED_ENABLED -# define MODE_GUIDED_ENABLED ENABLED -#endif - -////////////////////////////////////////////////////////////////////////////// -// Loiter mode - allows vehicle to hold global position -#ifndef MODE_LOITER_ENABLED -# define MODE_LOITER_ENABLED ENABLED -#endif - -////////////////////////////////////////////////////////////////////////////// -// Position Hold - enable holding of global position -#ifndef MODE_POSHOLD_ENABLED -# define MODE_POSHOLD_ENABLED ENABLED -#endif - -////////////////////////////////////////////////////////////////////////////// -// RTL - Return To Launch -#ifndef MODE_RTL_ENABLED -# define MODE_RTL_ENABLED ENABLED -#endif - -////////////////////////////////////////////////////////////////////////////// -// SmartRTL - allows vehicle to retrace a (loop-eliminated) breadcrumb home -#ifndef MODE_SMARTRTL_ENABLED -# define MODE_SMARTRTL_ENABLED ENABLED -#endif - -////////////////////////////////////////////////////////////////////////////// -// RADIO CONFIGURATION -////////////////////////////////////////////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////// - - ////////////////////////////////////////////////////////////////////////////// // FLIGHT_MODE // - #ifndef FLIGHT_MODE_1 # define FLIGHT_MODE_1 Mode::Number::MANUAL #endif @@ -238,7 +93,6 @@ # define FLIGHT_MODE_6 Mode::Number::MANUAL #endif - ////////////////////////////////////////////////////////////////////////////// // Throttle Failsafe // @@ -246,188 +100,17 @@ # define FS_THR_VALUE_DEFAULT 975 #endif -////////////////////////////////////////////////////////////////////////////// -// Takeoff -// -#ifndef PILOT_TKOFF_ALT_DEFAULT -# define PILOT_TKOFF_ALT_DEFAULT 0 // default final alt above home for pilot initiated takeoff -#endif - - -////////////////////////////////////////////////////////////////////////////// -// Landing -// -#ifndef LAND_SPEED -# define LAND_SPEED 50 // the descent speed for the final stage of landing in cm/s -#endif -#ifndef LAND_REPOSITION_DEFAULT -# define LAND_REPOSITION_DEFAULT 1 // by default the pilot can override roll/pitch during landing -#endif -#ifndef LAND_WITH_DELAY_MS -# define LAND_WITH_DELAY_MS 4000 // default delay (in milliseconds) when a land-with-delay is triggered during a failsafe event -#endif -#ifndef LAND_CANCEL_TRIGGER_THR -# define LAND_CANCEL_TRIGGER_THR 700 // land is cancelled by input throttle above 700 -#endif -#ifndef LAND_RANGEFINDER_MIN_ALT_CM -#define LAND_RANGEFINDER_MIN_ALT_CM 200 -#endif - -////////////////////////////////////////////////////////////////////////////// -// Landing Detector -// -#ifndef LAND_DETECTOR_TRIGGER_SEC -# define LAND_DETECTOR_TRIGGER_SEC 1.0f // number of seconds to detect a landing -#endif -#ifndef LAND_DETECTOR_MAYBE_TRIGGER_SEC -# define LAND_DETECTOR_MAYBE_TRIGGER_SEC 0.2f // number of seconds that means we might be landed (used to reset horizontal position targets to prevent tipping over) -#endif -#ifndef LAND_DETECTOR_ACCEL_LPF_CUTOFF -# define LAND_DETECTOR_ACCEL_LPF_CUTOFF 1.0f // frequency cutoff of land detector accelerometer filter -#endif -#ifndef LAND_DETECTOR_ACCEL_MAX -# define LAND_DETECTOR_ACCEL_MAX 1.0f // vehicle acceleration must be under 1m/s/s -#endif - -////////////////////////////////////////////////////////////////////////////// -// Flight mode definitions -// - -// Acro Mode -#ifndef ACRO_RP_P -# define ACRO_RP_P 4.5f -#endif - -#ifndef ACRO_YAW_P -# define ACRO_YAW_P 4.5f -#endif - -#ifndef ACRO_LEVEL_MAX_ANGLE -# define ACRO_LEVEL_MAX_ANGLE 3000 -#endif - -#ifndef ACRO_BALANCE_ROLL -#define ACRO_BALANCE_ROLL 1.0f -#endif - -#ifndef ACRO_BALANCE_PITCH -#define ACRO_BALANCE_PITCH 1.0f -#endif - -#ifndef ACRO_RP_EXPO_DEFAULT -#define ACRO_RP_EXPO_DEFAULT 0.3f -#endif - -#ifndef ACRO_Y_EXPO_DEFAULT -#define ACRO_Y_EXPO_DEFAULT 0.0f -#endif - -#ifndef ACRO_THR_MID_DEFAULT -#define ACRO_THR_MID_DEFAULT 0.0f -#endif - -// RTL Mode -#ifndef RTL_ALT_FINAL -# define RTL_ALT_FINAL 0 // the altitude the vehicle will move to as the final stage of Returning to Launch. Set to zero to land. -#endif - -#ifndef RTL_ALT -# define RTL_ALT 1500 // default alt to return to home in cm, 0 = Maintain current altitude -#endif - -#ifndef RTL_ALT_MIN -# define RTL_ALT_MIN 200 // min height above ground for RTL (i.e 2m) -#endif - -#ifndef RTL_CLIMB_MIN_DEFAULT -# define RTL_CLIMB_MIN_DEFAULT 0 // vehicle will always climb this many cm as first stage of RTL -#endif - -#ifndef RTL_ABS_MIN_CLIMB -# define RTL_ABS_MIN_CLIMB 250 // absolute minimum initial climb -#endif - -#ifndef RTL_CONE_SLOPE_DEFAULT -# define RTL_CONE_SLOPE_DEFAULT 3.0f // slope of RTL cone (height / distance). 0 = No cone -#endif - -#ifndef RTL_MIN_CONE_SLOPE -# define RTL_MIN_CONE_SLOPE 0.5f // minimum slope of cone -#endif - -#ifndef RTL_LOITER_TIME -# define RTL_LOITER_TIME 5000 // Time (in milliseconds) to loiter above home before beginning final descent -#endif - -// AUTO Mode -#ifndef WP_YAW_BEHAVIOR_DEFAULT -# define WP_YAW_BEHAVIOR_DEFAULT WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL -#endif - -#ifndef YAW_LOOK_AHEAD_MIN_SPEED -# define YAW_LOOK_AHEAD_MIN_SPEED 100 // minimum ground speed in cm/s required before blimp is aimed at ground course -#endif - -////////////////////////////////////////////////////////////////////////////// -// Stabilize Rate Control -// -#ifndef ROLL_PITCH_YAW_INPUT_MAX -# define ROLL_PITCH_YAW_INPUT_MAX 4500 // roll, pitch and yaw input range -#endif -#ifndef DEFAULT_ANGLE_MAX -# define DEFAULT_ANGLE_MAX 4500 // ANGLE_MAX parameters default value -#endif - -////////////////////////////////////////////////////////////////////////////// -// Stop mode defaults -// -#ifndef BRAKE_MODE_SPEED_Z -# define BRAKE_MODE_SPEED_Z 250 // z-axis speed in cm/s in Brake Mode -#endif -#ifndef BRAKE_MODE_DECEL_RATE -# define BRAKE_MODE_DECEL_RATE 750 // acceleration rate in cm/s/s in Brake Mode -#endif - -////////////////////////////////////////////////////////////////////////////// -// PosHold parameter defaults -// -#ifndef POSHOLD_BRAKE_RATE_DEFAULT -# define POSHOLD_BRAKE_RATE_DEFAULT 8 // default POSHOLD_BRAKE_RATE param value. Rotation rate during braking in deg/sec -#endif -#ifndef POSHOLD_BRAKE_ANGLE_DEFAULT -# define POSHOLD_BRAKE_ANGLE_DEFAULT 3000 // default POSHOLD_BRAKE_ANGLE param value. Max lean angle during braking in centi-degrees -#endif - ////////////////////////////////////////////////////////////////////////////// // Throttle control defaults // - #ifndef THR_DZ_DEFAULT # define THR_DZ_DEFAULT 100 // the deadzone above and below mid throttle while in althold or loiter #endif -// default maximum vertical velocity and acceleration the pilot may request -#ifndef PILOT_VELZ_MAX -# define PILOT_VELZ_MAX 250 // maximum vertical velocity in cm/s -#endif -#ifndef PILOT_ACCEL_Z_DEFAULT -# define PILOT_ACCEL_Z_DEFAULT 250 // vertical acceleration in cm/s/s while altitude is under pilot control -#endif - #ifndef AUTO_DISARMING_DELAY # define AUTO_DISARMING_DELAY 10 #endif -////////////////////////////////////////////////////////////////////////////// -// Throw mode configuration -// -#ifndef THROW_HIGH_SPEED -# define THROW_HIGH_SPEED 500.0f // vehicle much reach this total 3D speed in cm/s (or be free falling) -#endif -#ifndef THROW_VERTICAL_SPEED -# define THROW_VERTICAL_SPEED 50.0f // motors start when vehicle reaches this total 3D speed in cm/s -#endif - ////////////////////////////////////////////////////////////////////////////// // Logging control // diff --git a/Blimp/defines.h b/Blimp/defines.h index f356e77600..0a73072a1a 100644 --- a/Blimp/defines.h +++ b/Blimp/defines.h @@ -10,111 +10,6 @@ #define ENABLE ENABLED #define DISABLE DISABLED -// Autopilot Yaw Mode enumeration -enum autopilot_yaw_mode { - AUTO_YAW_HOLD = 0, // pilot controls the heading - AUTO_YAW_LOOK_AT_NEXT_WP = 1, // point towards next waypoint (no pilot input accepted) - AUTO_YAW_ROI = 2, // point towards a location held in roi (no pilot input accepted) - AUTO_YAW_FIXED = 3, // point towards a particular angle (no pilot input accepted) - AUTO_YAW_LOOK_AHEAD = 4, // point in the direction the blimp is moving - AUTO_YAW_RESETTOARMEDYAW = 5, // point towards heading at time motors were armed - AUTO_YAW_RATE = 6, // turn at a specified rate (held in auto_yaw_rate) - AUTO_YAW_CIRCLE = 7, // use AC_Circle's provided yaw (used during Loiter-Turns commands) -}; - -// Frame types -#define UNDEFINED_FRAME 0 -#define MULTICOPTER_FRAME 1 -#define HELI_FRAME 2 - -// Tuning enumeration -enum tuning_func { - TUNING_NONE = 0, // - TUNING_STABILIZE_ROLL_PITCH_KP = 1, // stabilize roll/pitch angle controller's P term - TUNING_STABILIZE_YAW_KP = 3, // stabilize yaw heading controller's P term - TUNING_RATE_ROLL_PITCH_KP = 4, // body frame roll/pitch rate controller's P term - TUNING_RATE_ROLL_PITCH_KI = 5, // body frame roll/pitch rate controller's I term - TUNING_YAW_RATE_KP = 6, // body frame yaw rate controller's P term - TUNING_THROTTLE_RATE_KP = 7, // throttle rate controller's P term (desired rate to acceleration or motor output) - TUNING_WP_SPEED = 10, // maximum speed to next way point (0 to 10m/s) - TUNING_LOITER_POSITION_KP = 12, // loiter distance controller's P term (position error to speed) - TUNING_HELI_EXTERNAL_GYRO = 13, // TradHeli specific external tail gyro gain - TUNING_ALTITUDE_HOLD_KP = 14, // altitude hold controller's P term (alt error to desired rate) - TUNING_RATE_ROLL_PITCH_KD = 21, // body frame roll/pitch rate controller's D term - TUNING_VEL_XY_KP = 22, // loiter rate controller's P term (speed error to tilt angle) - TUNING_ACRO_RP_KP = 25, // acro controller's P term. converts pilot input to a desired roll, pitch or yaw rate - TUNING_YAW_RATE_KD = 26, // body frame yaw rate controller's D term - TUNING_VEL_XY_KI = 28, // loiter rate controller's I term (speed error to tilt angle) - TUNING_AHRS_YAW_KP = 30, // ahrs's compass effect on yaw angle (0 = very low, 1 = very high) - TUNING_AHRS_KP = 31, // accelerometer effect on roll/pitch angle (0=low) - TUNING_ACCEL_Z_KP = 34, // accel based throttle controller's P term - TUNING_ACCEL_Z_KI = 35, // accel based throttle controller's I term - TUNING_ACCEL_Z_KD = 36, // accel based throttle controller's D term - TUNING_DECLINATION = 38, // compass declination in radians - TUNING_CIRCLE_RATE = 39, // circle turn rate in degrees (hard coded to about 45 degrees in either direction) - TUNING_ACRO_YAW_KP = 40, // acro controller's P term. converts pilot input to a desired roll, pitch or yaw rate - TUNING_RANGEFINDER_GAIN = 41, // rangefinder gain - TUNING_EKF_VERTICAL_POS = 42, // unused - TUNING_EKF_HORIZONTAL_POS = 43, // unused - TUNING_EKF_ACCEL_NOISE = 44, // unused - TUNING_RC_FEEL_RP = 45, // roll-pitch input smoothing - TUNING_RATE_PITCH_KP = 46, // body frame pitch rate controller's P term - TUNING_RATE_PITCH_KI = 47, // body frame pitch rate controller's I term - TUNING_RATE_PITCH_KD = 48, // body frame pitch rate controller's D term - TUNING_RATE_ROLL_KP = 49, // body frame roll rate controller's P term - TUNING_RATE_ROLL_KI = 50, // body frame roll rate controller's I term - TUNING_RATE_ROLL_KD = 51, // body frame roll rate controller's D term - TUNING_RATE_PITCH_FF = 52, // body frame pitch rate controller FF term - TUNING_RATE_ROLL_FF = 53, // body frame roll rate controller FF term - TUNING_RATE_YAW_FF = 54, // body frame yaw rate controller FF term - TUNING_RATE_MOT_YAW_HEADROOM = 55, // motors yaw headroom minimum - TUNING_RATE_YAW_FILT = 56, // yaw rate input filter - UNUSED = 57, // was winch control - TUNING_SYSTEM_ID_MAGNITUDE = 58 // magnitude of the system ID signal -}; - -// Yaw behaviours during missions - possible values for WP_YAW_BEHAVIOR parameter -#define WP_YAW_BEHAVIOR_NONE 0 // auto pilot will never control yaw during missions or rtl (except for DO_CONDITIONAL_YAW command received) -#define WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP 1 // auto pilot will face next waypoint or home during rtl -#define WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL 2 // auto pilot will face next waypoint except when doing RTL at which time it will stay in it's last -#define WP_YAW_BEHAVIOR_LOOK_AHEAD 3 // auto pilot will look ahead during missions and rtl (primarily meant for traditional helicopters) - -// Auto modes -enum AutoMode { - Auto_TakeOff, - Auto_WP, - Auto_Land, - Auto_RTL, - Auto_CircleMoveToEdge, - Auto_Circle, - Auto_Spline, - Auto_NavGuided, - Auto_Loiter, - Auto_LoiterToAlt, - Auto_NavPayloadPlace, -}; - -// Guided modes -enum GuidedMode { - Guided_TakeOff, - Guided_WP, - Guided_Velocity, - Guided_PosVel, - Guided_Angle, -}; - -enum PayloadPlaceStateType { - PayloadPlaceStateType_FlyToLocation, - PayloadPlaceStateType_Descent_Start, - PayloadPlaceStateType_Descent, - PayloadPlaceStateType_Release, - PayloadPlaceStateType_Releasing, - PayloadPlaceStateType_Delay, - PayloadPlaceStateType_Ascent_Start, - PayloadPlaceStateType_Ascent, - PayloadPlaceStateType_Done, -}; - // bit options for DEV_OPTIONS parameter enum DevOptions { DevOptionADSBMAVLink = 1, @@ -184,8 +79,7 @@ enum LoggingParameters { // EKF failsafe definitions (FS_EKF_ACTION parameter) #define FS_EKF_ACTION_LAND 1 // switch to LAND mode on EKF failsafe -#define FS_EKF_ACTION_ALTHOLD 2 // switch to ALTHOLD mode on EKF failsafe -#define FS_EKF_ACTION_LAND_EVEN_STABILIZE 3 // switch to Land mode on EKF failsafe even if in a manual flight mode like stabilize +#define FS_EKF_ACTION_LAND_EVEN_MANUAL 3 // switch to Land mode on EKF failsafe even if in Manual mode // for PILOT_THR_BHV parameter #define THR_BEHAVE_FEEDBACK_FROM_MID_STICK (1<<0) diff --git a/Blimp/ekf_check.cpp b/Blimp/ekf_check.cpp index 8bc678922f..fd524b99ab 100644 --- a/Blimp/ekf_check.cpp +++ b/Blimp/ekf_check.cpp @@ -148,16 +148,16 @@ void Blimp::failsafe_ekf_event() AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_OCCURRED); // does this mode require position? - if (!blimp.flightmode->requires_GPS() && (g.fs_ekf_action != FS_EKF_ACTION_LAND_EVEN_STABILIZE)) { + if (!blimp.flightmode->requires_GPS() && (g.fs_ekf_action != FS_EKF_ACTION_LAND_EVEN_MANUAL)) { return; } // take action based on fs_ekf_action parameter switch (g.fs_ekf_action) { case FS_EKF_ACTION_LAND: - case FS_EKF_ACTION_LAND_EVEN_STABILIZE: + case FS_EKF_ACTION_LAND_EVEN_MANUAL: default: - set_mode_land_with_pause(ModeReason::EKF_FAILSAFE); + set_mode_land_failsafe(ModeReason::EKF_FAILSAFE); break; } } diff --git a/Blimp/events.cpp b/Blimp/events.cpp index cbd778cd6c..452f624c5b 100644 --- a/Blimp/events.cpp +++ b/Blimp/events.cpp @@ -98,8 +98,8 @@ void Blimp::failsafe_gcs_check() const uint32_t gcs_last_seen_ms = gcs().sysid_myggcs_last_seen_time_ms(); if (gcs_last_seen_ms == 0) { - return; - } + return; + } // calc time since last gcs update // note: this only looks at the heartbeat from the device id set by g.sysid_my_gcs @@ -148,7 +148,7 @@ void Blimp::do_failsafe_action(Failsafe_Action action, ModeReason reason) case Failsafe_Action_None: return; case Failsafe_Action_Land: - set_mode_land_with_pause(reason); + set_mode_land_failsafe(reason); break; case Failsafe_Action_Terminate: { arming.disarm(AP_Arming::Method::FAILSAFE_ACTION_TERMINATE); diff --git a/Blimp/mode.h b/Blimp/mode.h index 107cc8c1a3..8a990fbb31 100644 --- a/Blimp/mode.h +++ b/Blimp/mode.h @@ -113,65 +113,6 @@ protected: float &G_Dt; public: - // Navigation Yaw control - class AutoYaw - { - - public: - // mode(): current method of determining desired yaw: - autopilot_yaw_mode mode() const - { - return (autopilot_yaw_mode)_mode; - } - void set_mode_to_default(bool rtl); - void set_mode(autopilot_yaw_mode new_mode); - autopilot_yaw_mode default_mode(bool rtl) const; - - void set_rate(float new_rate_cds); - - // set_roi(...): set a "look at" location: - void set_roi(const Location &roi_location); - - void set_fixed_yaw(float angle_deg, - float turn_rate_dps, - int8_t direction, - bool relative_angle); - - private: - - // yaw_cd(): main product of AutoYaw; the heading: - float yaw_cd(); - - // rate_cds(): desired yaw rate in centidegrees/second: - float rate_cds(); - - float look_ahead_yaw(); - float roi_yaw(); - - // auto flight mode's yaw mode - uint8_t _mode = AUTO_YAW_LOOK_AT_NEXT_WP; - - // Yaw will point at this location if mode is set to AUTO_YAW_ROI - Vector3f roi; - - // bearing from current location to the ROI - float _roi_yaw; - - // yaw used for YAW_FIXED yaw_mode - int32_t _fixed_yaw; - - // Deg/s we should turn - int16_t _fixed_yaw_slewrate; - - // heading when in yaw_look_ahead_yaw - float _look_ahead_yaw; - - // used to reduce update rate to 100hz: - uint8_t roi_yaw_counter; - - }; - static AutoYaw auto_yaw; - // pass-through functions to reduce code churn on conversion; // these are candidates for moving into the Mode base // class. diff --git a/Blimp/mode_land.cpp b/Blimp/mode_land.cpp index f6e334839e..640e69ef52 100644 --- a/Blimp/mode_land.cpp +++ b/Blimp/mode_land.cpp @@ -13,12 +13,11 @@ void ModeLand::run() motors->down_out = 0; } -// set_mode_land_with_pause - sets mode to LAND and triggers 4 second delay before descent starts -// this is always called from a failsafe so we trigger notification to pilot -void Blimp::set_mode_land_with_pause(ModeReason reason) +// set_mode_land_failsafe - sets mode to LAND +// this is always called from a failsafe so we trigger notification to pilot +void Blimp::set_mode_land_failsafe(ModeReason reason) { set_mode(Mode::Number::LAND, reason); - //TODO: Add pause // alert pilot to mode change AP_Notify::events.failsafe_mode_change = 1; diff --git a/Blimp/wscript b/Blimp/wscript index a45b13a313..34cb8c1704 100644 --- a/Blimp/wscript +++ b/Blimp/wscript @@ -7,9 +7,7 @@ def build(bld): name=vehicle + '_libs', ap_vehicle=vehicle, ap_libraries=bld.ap_common_vehicle_libraries() + [ - 'AC_AttitudeControl', 'AC_InputManager', - 'AC_WPNav', 'AP_InertialNav', 'AP_RCMapper', 'AP_Avoidance', @@ -27,5 +25,4 @@ def build(bld): program_name='blimp', program_groups=['bin', 'blimp'], use=vehicle + '_libs', - defines=['FRAME_CONFIG=MULTICOPTER_FRAME'], )