Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Blimp: Remove extra defines and other code, fixup some formatting and naming

This commit is contained in:
Michelle Rossouw 2023-07-27 15:06:32 +10:00 committed by Andrew Tridgell
parent de0dcdd9e4
commit b5d22ce47c
13 changed files with 23 additions and 520 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
Blimp/Blimp.h
View File

@ -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

2
Blimp/GCS_Mavlink.cpp
View File

@ -102,7 +102,7 @@ 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;
}

7
Blimp/Log.cpp
View File

@ -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) {}

13
Blimp/Parameters.cpp
View File

@ -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

5
Blimp/Parameters.h
View File

@ -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

317
Blimp/config.h
View File

@ -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
//

108
Blimp/defines.h
View File

@ -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)

6
Blimp/ekf_check.cpp
View File

@ -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;
}
}

6
Blimp/events.cpp
View File

@ -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);

59
Blimp/mode.h
View File

@ -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.

7
Blimp/mode_land.cpp
View File

@ -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;

3
Blimp/wscript
View File

@ -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'],
)