zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Blimp: Add modes Velocity and Loiter

gps-1.3.1
Michelle Rossouw 4 years ago committed by Andrew Tridgell
parent
1a0f44a8ad
commit
5bd3e3e330
11 changed files with 672 additions and 8 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 26
      Blimp/Blimp.cpp
  2. 32
      Blimp/Blimp.h
  3. 6
      Blimp/Fins.cpp
  4. 383
      Blimp/Parameters.cpp
  5. 33
      Blimp/Parameters.h
  6. 4
      Blimp/RC_Channel.cpp
  7. 6
      Blimp/mode.cpp
  8. 89
      Blimp/mode.h
  9. 61
      Blimp/mode_loiter.cpp
  10. 33
      Blimp/mode_velocity.cpp
  11. 5
      Blimp/system.cpp

26
Blimp/Blimp.cpp
Unescape View File

@ -145,6 +145,9 @@ void Blimp::full_rate_logging() @@ -145,6 +145,9 @@ void Blimp::full_rate_logging()
if (should_log(MASK_LOG_ATTITUDE_FAST) && !blimp.flightmode->logs_attitude()) {
Log_Write_Attitude();
}
if (should_log(MASK_LOG_PID)) {
Log_Write_PIDs();
}
}
// ten_hz_logging_loop
@ -220,6 +223,14 @@ void Blimp::read_AHRS(void) @@ -220,6 +223,14 @@ void Blimp::read_AHRS(void)
{
// we tell AHRS to skip INS update as we have already done it in fast_loop()
ahrs.update(true);
IGNORE_RETURN(ahrs.get_velocity_NED(vel_ned));
IGNORE_RETURN(ahrs.get_relative_position_NED_home(pos_ned));
vel_yaw = ahrs.get_yaw_rate_earth();
Vector2f vel_xy_filtd = vel_xy_filter.apply({vel_ned.x, vel_ned.y});
vel_ned_filtd = {vel_xy_filtd.x, vel_xy_filtd.y, vel_z_filter.apply(vel_ned.z)};
vel_yaw_filtd = vel_yaw_filter.apply(vel_yaw);
}
// read baro and log control tuning
@ -237,6 +248,21 @@ void Blimp::update_altitude() @@ -237,6 +248,21 @@ void Blimp::update_altitude()
}
}
//Conversions are in 2D so that up remains up in world frame when the blimp is not exactly level.
void Blimp::rotate_BF_to_NE(float &x, float &y)
{
float ne_x = x*ahrs.cos_yaw() - y*ahrs.sin_yaw();
float ne_y = x*ahrs.sin_yaw() + y*ahrs.cos_yaw();
x = ne_x;
y = ne_y;
}
void Blimp::rotate_NE_to_BF(float &x, float &y)
{
float bf_x = x*ahrs.cos_yaw() + y*ahrs.sin_yaw();
float bf_y = -x*ahrs.sin_yaw() + y*ahrs.cos_yaw();
x = bf_x;
y = bf_y;
}

32
Blimp/Blimp.h
Unescape View File

@ -49,6 +49,10 @@ @@ -49,6 +49,10 @@
#include <AP_BattMonitor/AP_BattMonitor.h> // Battery monitor library
#include <AP_Arming/AP_Arming.h>
#include <AP_Scripting/AP_Scripting.h>
#include <AC_PID/AC_PID_2D.h>
#include <AC_PID/AC_PID_Basic.h>
#include <AC_PID/AC_PID.h>
#include <Filter/NotchFilter.h>
// Configuration
#include "defines.h"
@ -90,6 +94,8 @@ public: @@ -90,6 +94,8 @@ public:
friend class Mode;
friend class ModeManual;
friend class ModeLand;
friend class ModeVelocity;
friend class ModeLoiter;
friend class Fins;
@ -232,17 +238,27 @@ private: @@ -232,17 +238,27 @@ private:
// 3D Location vectors
// Current location of the vehicle (altitude is relative to home)
Location current_loc;
Vector3f vel_ned;
Vector3f vel_ned_filtd;
Vector3f pos_ned;
float vel_yaw;
float vel_yaw_filtd;
NotchFilterVector2f vel_xy_filter;
NotchFilterFloat vel_z_filter;
NotchFilterFloat vel_yaw_filter;
// Inertial Navigation
AP_InertialNav_NavEKF inertial_nav;
// Attitude, Position and Waypoint navigation objects
// To-Do: move inertial nav up or other navigation variables down here
// AC_AttitudeControl_t *attitude_control;
// AC_PosControl *pos_control;
// AC_WPNav *wp_nav;
// AC_Loiter *loiter_nav;
// Vel & pos PIDs
AC_PID_2D pid_vel_xy{3, 0.2, 0, 0, 0.2, 3, 3, 0.02}; //These are the defaults - P I D FF IMAX FiltHz FiltDHz DT
AC_PID_Basic pid_vel_z{7, 1.5, 0, 0, 1, 3, 3, 0.02};
AC_PID_Basic pid_vel_yaw{3, 0.4, 0, 0, 0.2, 3, 3, 0.02};
AC_PID_2D pid_pos_xy{1, 0.05, 0, 0, 0.1, 3, 3, 0.02};
AC_PID_Basic pid_pos_z{0.7, 0, 0, 0, 0, 3, 3, 0.02};
AC_PID pid_pos_yaw{1.2, 0.5, 0, 0, 2, 3, 3, 3, 0.02}; //p, i, d, ff, imax, filt_t, filt_e, filt_d, dt, opt srmax, opt srtau
// System Timers
// --------------
@ -314,6 +330,8 @@ private: @@ -314,6 +330,8 @@ private:
void one_hz_loop();
void read_AHRS(void);
void update_altitude();
void rotate_NE_to_BF(float &x, float &y);
void rotate_BF_to_NE(float &x, float &y);
// commands.cpp
void update_home_from_EKF();
@ -446,6 +464,8 @@ private: @@ -446,6 +464,8 @@ private:
Mode *flightmode;
ModeManual mode_manual;
ModeLand mode_land;
ModeVelocity mode_velocity;
ModeLoiter mode_loiter;
// mode.cpp
Mode *mode_from_mode_num(const Mode::Number mode);

6
Blimp/Fins.cpp
Unescape View File

@ -80,6 +80,12 @@ void Fins::output() @@ -80,6 +80,12 @@ void Fins::output()
blimp.Write_FINI(right_out, front_out, down_out, yaw_out);
//Constrain after logging so as to still show when sub-optimal tuning is causing massive overshoots.
right_out = constrain_float(right_out, -1, 1);
front_out = constrain_float(front_out, -1, 1);
down_out = constrain_float(down_out, -1, 1);
yaw_out = constrain_float(yaw_out, -1, 1);
_time = AP_HAL::micros() * 1.0e-6;
for (int8_t i=0; i<NUM_FINS; i++) {

383
Blimp/Parameters.cpp
Unescape View File

@ -249,6 +249,66 @@ const AP_Param::Info Blimp::var_info[] = { @@ -249,6 +249,66 @@ const AP_Param::Info Blimp::var_info[] = {
// @User: Advanced
GSCALAR(fs_crash_check, "FS_CRASH_CHECK", 1),
// @Param: MAX_VEL_XY
// @DisplayName: Max XY Velocity
// @Description: Sets the maximum XY velocity, in m/s
// @Values:
// @User: Standard
GSCALAR(max_vel_xy, "MAX_VEL_XY", 0.5),
// @Param: MAX_VEL_Z
// @DisplayName: Max Z Velocity
// @Description: Sets the maximum Z velocity, in m/s
// @Values:
// @User: Standard
GSCALAR(max_vel_z, "MAX_VEL_Z", 0.4),
// @Param: MAX_VEL_YAW
// @DisplayName: Max yaw Velocity
// @Description: Sets the maximum yaw velocity, in rad/s
// @Values:
// @User: Standard
GSCALAR(max_vel_yaw, "MAX_VEL_YAW", 0.5),
// @Param: MAX_POS_XY
// @DisplayName: Max XY Position change
// @Description: Sets the maximum XY position change, in m/s
// @Values:
// @User: Standard
GSCALAR(max_pos_xy, "MAX_POS_XY", 0.2),
// @Param: MAX_POS_Z
// @DisplayName: Max Z Position change
// @Description: Sets the maximum Z position change, in m/s
// @Values:
// @User: Standard
GSCALAR(max_pos_z, "MAX_POS_Z", 0.15),
// @Param: MAX_POS_YAW
// @DisplayName: Max Yaw Position change
// @Description: Sets the maximum Yaw position change, in rad/s
// @Values:
// @User: Standard
GSCALAR(max_pos_yaw, "MAX_POS_YAW", 0.3),
// @Param: SIMPLE_MODE
// @DisplayName: Simple mode
// @Description: Simple mode for Position control - "forward" moves blimp in +ve X direction world-frame
// @Values: 0:Disabled, 1:Enabled
// @User: Standard
GSCALAR(simple_mode, "SIMPLE_MODE", 0),
// @Param: DIS_MASK
// @DisplayName: Disable output mask
// @Description: Mask for disabling 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
GSCALAR(dis_mask, "DIS_MASK", 0),
GSCALAR(notch_bw, "NOTCH_BW", 2),
GSCALAR(notch_att, "NOTCH_ATT", 15),
// @Param: RC_SPEED
// @DisplayName: ESC Update Speed
// @Description: This is the speed in Hertz that your ESCs will receive updates
@ -379,6 +439,329 @@ const AP_Param::Info Blimp::var_info[] = { @@ -379,6 +439,329 @@ const AP_Param::Info Blimp::var_info[] = {
// @Path: Fins.cpp
GOBJECTPTR(motors, "FINS_", Fins),
// @Param: VELXY_P
// @DisplayName: Velocity (horizontal) P gain
// @Description: Velocity (horizontal) P gain. Converts the difference between desired and actual velocity to a target acceleration
// @Range: 0.1 6.0
// @Increment: 0.1
// @User: Advanced
// @Param: VELXY_I
// @DisplayName: Velocity (horizontal) I gain
// @Description: Velocity (horizontal) I gain. Corrects long-term difference between desired and actual velocity to a target acceleration
// @Range: 0.02 1.00
// @Increment: 0.01
// @User: Advanced
// @Param: VELXY_D
// @DisplayName: Velocity (horizontal) D gain
// @Description: Velocity (horizontal) D gain. Corrects short-term changes in velocity
// @Range: 0.00 1.00
// @Increment: 0.001
// @User: Advanced
// @Param: VELXY_IMAX
// @DisplayName: Velocity (horizontal) integrator maximum
// @Description: Velocity (horizontal) integrator maximum. Constrains the target acceleration that the I gain will output
// @Range: 0 4500
// @Increment: 10
// @Units: cm/s/s
// @User: Advanced
// @Param: VELXY_FILT
// @DisplayName: Velocity (horizontal) input filter
// @Description: Velocity (horizontal) input filter. This filter (in Hz) is applied to the input for P and I terms
// @Range: 0 100
// @Units: Hz
// @User: Advanced
// @Param: VELXY_D_FILT
// @DisplayName: Velocity (horizontal) input filter
// @Description: Velocity (horizontal) input filter. This filter (in Hz) is applied to the input for D term
// @Range: 0 100
// @Units: Hz
// @User: Advanced
// @Param: VELXY_FF
// @DisplayName: Velocity (horizontal) feed forward gain
// @Description: Velocity (horizontal) feed forward gain. Converts the difference between desired velocity to a target acceleration
// @Range: 0 6
// @Increment: 0.01
// @User: Advanced
GOBJECT(pid_vel_xy, "VELXY_", AC_PID_2D),
// @Param: VELZ_P
// @DisplayName: Velocity (vertical) P gain
// @Description: Velocity (vertical) P gain. Converts the difference between desired and actual velocity to a target acceleration
// @Range: 0.1 6.0
// @Increment: 0.1
// @User: Advanced
// @Param: VELZ_I
// @DisplayName: Velocity (vertical) I gain
// @Description: Velocity (vertical) I gain. Corrects long-term difference between desired and actual velocity to a target acceleration
// @Range: 0.02 1.00
// @Increment: 0.01
// @User: Advanced
// @Param: VELZ_D
// @DisplayName: Velocity (vertical) D gain
// @Description: Velocity (vertical) D gain. Corrects short-term changes in velocity
// @Range: 0.00 1.00
// @Increment: 0.001
// @User: Advanced
// @Param: VELZ_IMAX
// @DisplayName: Velocity (vertical) integrator maximum
// @Description: Velocity (vertical) integrator maximum. Constrains the target acceleration that the I gain will output
// @Range: 0 4500
// @Increment: 10
// @Units: cm/s/s
// @User: Advanced
// @Param: VELZ_FILT
// @DisplayName: Velocity (vertical) input filter
// @Description: Velocity (vertical) input filter. This filter (in Hz) is applied to the input for P and I terms
// @Range: 0 100
// @Units: Hz
// @User: Advanced
// @Param: VELZ_D_FILT
// @DisplayName: Velocity (vertical) input filter
// @Description: Velocity (vertical) input filter. This filter (in Hz) is applied to the input for D term
// @Range: 0 100
// @Units: Hz
// @User: Advanced
// @Param: VELZ_FF
// @DisplayName: Velocity (vertical) feed forward gain
// @Description: Velocity (vertical) feed forward gain. Converts the difference between desired velocity to a target acceleration
// @Range: 0 6
// @Increment: 0.01
// @User: Advanced
GOBJECT(pid_vel_z, "VELZ_", AC_PID_Basic),
// @Param: VELYAW_P
// @DisplayName: Velocity (yaw) P gain
// @Description: Velocity (yaw) P gain. Converts the difference between desired and actual velocity to a target acceleration
// @Range: 0.1 6.0
// @Increment: 0.1
// @User: Advanced
// @Param: VELYAW_I
// @DisplayName: Velocity (yaw) I gain
// @Description: Velocity (yaw) I gain. Corrects long-term difference between desired and actual velocity to a target acceleration
// @Range: 0.02 1.00
// @Increment: 0.01
// @User: Advanced
// @Param: VELYAW_D
// @DisplayName: Velocity (yaw) D gain
// @Description: Velocity (yaw) D gain. Corrects short-term changes in velocity
// @Range: 0.00 1.00
// @Increment: 0.001
// @User: Advanced
// @Param: VELYAW_IMAX
// @DisplayName: Velocity (yaw) integrator maximum
// @Description: Velocity (yaw) integrator maximum. Constrains the target acceleration that the I gain will output
// @Range: 0 4500
// @Increment: 10
// @Units: cm/s/s
// @User: Advanced
// @Param: VELYAW_FILT
// @DisplayName: Velocity (yaw) input filter
// @Description: Velocity (yaw) input filter. This filter (in Hz) is applied to the input for P and I terms
// @Range: 0 100
// @Units: Hz
// @User: Advanced
// @Param: VELYAW_D_FILT
// @DisplayName: Velocity (yaw) input filter
// @Description: Velocity (yaw) input filter. This filter (in Hz) is applied to the input for D term
// @Range: 0 100
// @Units: Hz
// @User: Advanced
// @Param: VELYAW_FF
// @DisplayName: Velocity (yaw) feed forward gain
// @Description: Velocity (yaw) feed forward gain. Converts the difference between desired velocity to a target acceleration
// @Range: 0 6
// @Increment: 0.01
// @User: Advanced
GOBJECT(pid_vel_yaw, "VELYAW_", AC_PID_Basic),
// @Param: POSXY_P
// @DisplayName: Position (horizontal) P gain
// @Description: Position (horizontal) P gain. Converts the difference between desired and actual position to a target velocity
// @Range: 0.1 6.0
// @Increment: 0.1
// @User: Advanced
// @Param: POSXY_I
// @DisplayName: Position (horizontal) I gain
// @Description: Position (horizontal) I gain. Corrects long-term difference between desired and actual position to a target velocity
// @Range: 0.02 1.00
// @Increment: 0.01
// @User: Advanced
// @Param: POSXY_D
// @DisplayName: Position (horizontal) D gain
// @Description: Position (horizontal) D gain. Corrects short-term changes in position
// @Range: 0.00 1.00
// @Increment: 0.001
// @User: Advanced
// @Param: POSXY_IMAX
// @DisplayName: Position (horizontal) integrator maximum
// @Description: Position (horizontal) integrator maximum. Constrains the target acceleration that the I gain will output
// @Range: 0 4500
// @Increment: 10
// @Units: cm/s/s
// @User: Advanced
// @Param: POSXY_FILT
// @DisplayName: Position (horizontal) input filter
// @Description: Position (horizontal) input filter. This filter (in Hz) is applied to the input for P and I terms
// @Range: 0 100
// @Units: Hz
// @User: Advanced
// @Param: POSXY_D_FILT
// @DisplayName: Position (horizontal) input filter
// @Description: Position (horizontal) input filter. This filter (in Hz) is applied to the input for D term
// @Range: 0 100
// @Units: Hz
// @User: Advanced
// @Param: POSXY_FF
// @DisplayName: Position (horizontal) feed forward gain
// @Description: Position (horizontal) feed forward gain. Converts the difference between desired position to a target velocity
// @Range: 0 6
// @Increment: 0.01
// @User: Advanced
GOBJECT(pid_pos_xy, "POSXY_", AC_PID_2D),
// @Param: POSZ_P
// @DisplayName: Position (vertical) P gain
// @Description: Position (vertical) P gain. Converts the difference between desired and actual position to a target velocity
// @Range: 0.1 6.0
// @Increment: 0.1
// @User: Advanced
// @Param: POSZ_I
// @DisplayName: Position (vertical) I gain
// @Description: Position (vertical) I gain. Corrects long-term difference between desired and actual position to a target velocity
// @Range: 0.02 1.00
// @Increment: 0.01
// @User: Advanced
// @Param: POSZ_D
// @DisplayName: Position (vertical) D gain
// @Description: Position (vertical) D gain. Corrects short-term changes in position
// @Range: 0.00 1.00
// @Increment: 0.001
// @User: Advanced
// @Param: POSZ_IMAX
// @DisplayName: Position (vertical) integrator maximum
// @Description: Position (vertical) integrator maximum. Constrains the target acceleration that the I gain will output
// @Range: 0 4500
// @Increment: 10
// @Units: cm/s/s
// @User: Advanced
// @Param: POSZ_FILT
// @DisplayName: Position (vertical) input filter
// @Description: Position (vertical) input filter. This filter (in Hz) is applied to the input for P and I terms
// @Range: 0 100
// @Units: Hz
// @User: Advanced
// @Param: POSZ_D_FILT
// @DisplayName: Position (vertical) input filter
// @Description: Position (vertical) input filter. This filter (in Hz) is applied to the input for D term
// @Range: 0 100
// @Units: Hz
// @User: Advanced
// @Param: POSZ_FF
// @DisplayName: Position (vertical) feed forward gain
// @Description: Position (vertical) feed forward gain. Converts the difference between desired position to a target velocity
// @Range: 0 6
// @Increment: 0.01
// @User: Advanced
GOBJECT(pid_pos_z, "POSZ_", AC_PID_Basic),
// @Param: POSYAW_P
// @DisplayName: Position (yaw) axis controller P gain
// @Description: Position (yaw) axis controller P gain.
// @Range: 0.0 3.0
// @Increment: 0.01
// @User: Standard
// @Param: POSYAW_I
// @DisplayName: Position (yaw) axis controller I gain
// @Description: Position (yaw) axis controller I gain.
// @Range: 0.0 3.0
// @Increment: 0.01
// @User: Standard
// @Param: POSYAW_IMAX
// @DisplayName: Position (yaw) axis controller I gain maximum
// @Description: Position (yaw) axis controller I gain maximum.
// @Range: 0 4000
// @Increment: 10
// @Units: d%
// @User: Standard
// @Param: POSYAW_D
// @DisplayName: Position (yaw) axis controller D gain
// @Description: Position (yaw) axis controller D gain.
// @Range: 0.001 0.1
// @Increment: 0.001
// @User: Standard
// @Param: POSYAW_FF
// @DisplayName: Position (yaw) axis controller feed forward
// @Description: Position (yaw) axis controller feed forward
// @Range: 0 0.5
// @Increment: 0.001
// @User: Standard
// @Param: POSYAW_FLTT
// @DisplayName: Position (yaw) target frequency filter in Hz
// @Description: Position (yaw) target frequency filter in Hz
// @Range: 1 50
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: POSYAW_FLTE
// @DisplayName: Position (yaw) error frequency filter in Hz
// @Description: Position (yaw) error frequency filter in Hz
// @Range: 1 100
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: POSYAW_FLTD
// @DisplayName: Position (yaw) derivative input filter in Hz
// @Description: Position (yaw) derivative input filter in Hz
// @Range: 1 100
// @Increment: 1
// @Units: Hz
// @User: Standard
// @Param: POSYAW_SMAX
// @DisplayName: Yaw slew rate limit
// @Description: Sets an upper limit on the slew rate produced by the combined P and D gains.
// @Range: 0 200
// @Increment: 0.5
// @User: Advanced
GOBJECT(pid_pos_yaw, "POSYAW_", AC_PID),
// @Group:
// @Path: ../libraries/AP_Vehicle/AP_Vehicle.cpp
{ AP_PARAM_GROUP, "", Parameters::k_param_vehicle, (const void *)&blimp, {group_info : AP_Vehicle::var_info} },

33
Blimp/Parameters.h
Unescape View File

@ -90,7 +90,13 @@ public: @@ -90,7 +90,13 @@ public:
k_param_adsb, // 72
k_param_notify, // 73
//PID Controllers
k_param_pid_vel_xy,
k_param_pid_pos_xy,
k_param_pid_vel_z,
k_param_pid_vel_yaw,
k_param_pid_pos_z,
k_param_pid_pos_yaw,
//
// 90: misc2
//
@ -173,6 +179,19 @@ public: @@ -173,6 +179,19 @@ public:
//
// 220: Misc
//
k_param_fs_ekf_action = 220,
k_param_max_vel_xy,
k_param_arming,
k_param_max_vel_z,
k_param_simple_mode,
k_param_dis_mask, //225
k_param_notch_bw,
k_param_notch_att,
k_param_max_vel_yaw,
k_param_max_pos_xy,
k_param_max_pos_z,
k_param_max_pos_yaw,
k_param_logger = 253, // 253 - Logging Group
k_param_vehicle = 257, // vehicle common block of parameters
@ -224,6 +243,18 @@ public: @@ -224,6 +243,18 @@ public:
AP_Float fs_ekf_thresh;
AP_Int16 gcs_pid_mask;
AP_Float max_vel_xy;
AP_Float max_vel_z;
AP_Float max_vel_yaw;
AP_Float max_pos_xy;
AP_Float max_pos_z;
AP_Float max_pos_yaw;
AP_Int8 simple_mode;
AP_Int16 dis_mask;
AP_Float notch_bw;
AP_Float notch_att;
AP_Int8 rtl_alt_type;
AP_Int16 rc_speed; // speed of fast RC Channels in Hz

4
Blimp/RC_Channel.cpp
Unescape View File

@ -106,6 +106,10 @@ bool RC_Channel_Blimp::do_aux_function(const aux_func_t ch_option, const AuxSwit @@ -106,6 +106,10 @@ bool RC_Channel_Blimp::do_aux_function(const aux_func_t ch_option, const AuxSwit
}
break;
case AUX_FUNC::LOITER:
do_aux_function_change_mode(Mode::Number::LOITER, ch_flag);
break;
case AUX_FUNC::MANUAL:
do_aux_function_change_mode(Mode::Number::MANUAL, ch_flag);
break;

6
Blimp/mode.cpp
Unescape View File

@ -33,6 +33,12 @@ Mode *Blimp::mode_from_mode_num(const Mode::Number mode) @@ -33,6 +33,12 @@ Mode *Blimp::mode_from_mode_num(const Mode::Number mode)
case Mode::Number::LAND:
ret = &mode_land;
break;
case Mode::Number::VELOCITY:
ret = &mode_velocity;
break;
case Mode::Number::LOITER:
ret = &mode_loiter;
break;
default:
break;
}

89
Blimp/mode.h
Unescape View File

@ -15,6 +15,8 @@ public: @@ -15,6 +15,8 @@ public:
enum class Number : uint8_t {
MANUAL = 0, // manual control
LAND = 1, // currently just stops moving
VELOCITY = 2, // velocity mode
LOITER = 3, // loiter mode (position hold)
};
// constructor
@ -228,6 +230,93 @@ private: @@ -228,6 +230,93 @@ private:
};
class ModeVelocity : public Mode
{
public:
// inherit constructor
using Mode::Mode;
virtual void run() override;
bool requires_GPS() const override
{
return true;
}
bool has_manual_throttle() const override
{
return false;
}
bool allows_arming(bool from_gcs) const override
{
return true;
};
bool is_autopilot() const override
{
return false;
//TODO
}
protected:
const char *name() const override
{
return "VELOCITY";
}
const char *name4() const override
{
return "VELY";
}
private:
};
class ModeLoiter : public Mode
{
public:
// inherit constructor
using Mode::Mode;
virtual bool init(bool ignore_checks) override;
virtual void run() override;
bool requires_GPS() const override
{
return true;
}
bool has_manual_throttle() const override
{
return false;
}
bool allows_arming(bool from_gcs) const override
{
return true;
};
bool is_autopilot() const override
{
return false;
//TODO
}
protected:
const char *name() const override
{
return "LOITER";
}
const char *name4() const override
{
return "LOIT";
}
private:
Vector3f target_pos;
float target_yaw;
float loop_period;
};
class ModeLand : public Mode
{

61
Blimp/mode_loiter.cpp
Unescape View File

@ -0,0 +1,61 @@ @@ -0,0 +1,61 @@
#include "Blimp.h"
/*
* Init and run calls for loiter flight mode
*/
bool ModeLoiter::init(bool ignore_checks){
target_pos = blimp.pos_ned;
target_yaw = blimp.ahrs.get_yaw();
return true;
}
//Runs the main loiter controller
void ModeLoiter::run()
{
float pilot_fwd = channel_front->get_control_in() / float(RC_SCALE) * g.max_pos_xy * blimp.scheduler.get_loop_period_s();
float pilot_rgt = channel_right->get_control_in() / float(RC_SCALE) * g.max_pos_xy * blimp.scheduler.get_loop_period_s();
float pilot_dwn = channel_down->get_control_in() / float(RC_SCALE) * g.max_pos_z * blimp.scheduler.get_loop_period_s();
float pilot_yaw = channel_yaw->get_control_in() / float(RC_SCALE) * g.max_pos_yaw * blimp.scheduler.get_loop_period_s();
if (g.simple_mode == 0){
//If simple mode is disabled, input is in body-frame, thus needs to be rotated.
blimp.rotate_BF_to_NE(pilot_fwd, pilot_rgt);
}
target_pos.x += pilot_fwd;
target_pos.y += pilot_rgt;
target_pos.z += pilot_dwn;
target_yaw = wrap_PI(target_yaw + pilot_yaw);
//Pos controller's output becomes target for velocity controller
Vector3f target_vel_ef{blimp.pid_pos_xy.update_all(target_pos, blimp.pos_ned, {0,0,0}), 0};
target_vel_ef.z = blimp.pid_pos_z.update_all(target_pos.z, blimp.pos_ned.z);
float yaw_ef = blimp.ahrs.get_yaw();
float target_vel_yaw = blimp.pid_pos_yaw.update_error(wrap_PI(target_yaw - yaw_ef));
blimp.pid_pos_yaw.set_target_rate(target_yaw);
blimp.pid_pos_yaw.set_actual_rate(yaw_ef);
Vector3f target_vel_ef_c{constrain_float(target_vel_ef.x, -g.max_vel_xy, g.max_vel_xy),
constrain_float(target_vel_ef.y, -g.max_vel_xy, g.max_vel_xy),
constrain_float(target_vel_ef.z, -g.max_vel_xy, g.max_vel_xy)};
float target_vel_yaw_c = constrain_float(target_vel_yaw, -g.max_vel_yaw, g.max_vel_yaw);
Vector2f actuator = blimp.pid_vel_xy.update_all(target_vel_ef_c, blimp.vel_ned_filtd, {0,0,0});
float act_down = blimp.pid_vel_z.update_all(target_vel_ef_c.z, blimp.vel_ned_filtd.z);
blimp.rotate_NE_to_BF(actuator.x, actuator.y);
float act_yaw = blimp.pid_vel_yaw.update_all(target_vel_yaw_c, blimp.vel_yaw_filtd);
if(!(blimp.g.dis_mask & (1<<(2-1)))){
motors->front_out = actuator.x;
} if(!(blimp.g.dis_mask & (1<<(1-1)))){
motors->right_out = actuator.y;
} if(!(blimp.g.dis_mask & (1<<(3-1)))){
motors->down_out = act_down;
} if(!(blimp.g.dis_mask & (1<<(4-1)))){
motors->yaw_out = act_yaw;
}
AP::logger().Write_PSC(target_pos*100.0f, blimp.pos_ned*100.0f, target_vel_ef_c*100.0f, blimp.vel_ned_filtd*100.0f, blimp.vel_ned*100.0f, target_yaw*100.0f, yaw_ef*100.0f); //last entries here are just for debugging
AP::logger().Write_PSCZ(target_pos.z*100.0f, blimp.pos_ned.z*100.0f, blimp.scheduler.get_loop_period_s()*100.0f, target_vel_ef_c.z*100.0f, blimp.vel_ned_filtd.z*100.0f, 0.0f, blimp.vel_ned.z*100.0f, blimp.vel_yaw*100.0f, blimp.vel_yaw_filtd*100.0f);
}

33
Blimp/mode_velocity.cpp
Unescape View File

@ -0,0 +1,33 @@ @@ -0,0 +1,33 @@
#include "Blimp.h"
/*
* Init and run calls for velocity flight mode
*/
// Runs the main velocity controller
void ModeVelocity::run()
{
Vector3f target_vel;
target_vel.x = channel_front->get_control_in() / float(RC_SCALE) * g.max_vel_xy;
target_vel.y = channel_right->get_control_in() / float(RC_SCALE) * g.max_vel_xy;
blimp.rotate_BF_to_NE(target_vel.x, target_vel.y);
target_vel.z = channel_down->get_control_in() / float(RC_SCALE) * g.max_vel_z;
float target_vel_yaw = channel_yaw->get_control_in() / float(RC_SCALE) * g.max_vel_yaw;
Vector2f actuator = blimp.pid_vel_xy.update_all(target_vel, blimp.vel_ned_filtd, {0,0,0});
blimp.rotate_NE_to_BF(actuator.x, actuator.y);
float act_down = blimp.pid_vel_z.update_all(target_vel.z, blimp.vel_ned_filtd.z);
float act_yaw = blimp.pid_vel_yaw.update_all(target_vel_yaw, blimp.vel_yaw_filtd);
if(!(blimp.g.dis_mask & (1<<(2-1)))){
motors->front_out = actuator.x;
} if(!(blimp.g.dis_mask & (1<<(1-1)))){
motors->right_out = actuator.y;
} if(!(blimp.g.dis_mask & (1<<(3-1)))){
motors->down_out = act_down;
} if(!(blimp.g.dis_mask & (1<<(4-1)))){
motors->yaw_out = act_yaw;
}
AP::logger().Write_PSC({0,0,0}, blimp.pos_ned, target_vel*100.0f, blimp.vel_ned_filtd*100.0f, blimp.vel_ned*100.0f, 0, 0);
AP::logger().Write_PSCZ(0.0f, 0.0f, 0.0f, target_vel.z*100.0f, blimp.vel_ned_filtd.z*100.0f, 0.0f, blimp.vel_ned.z*100.0f, blimp.vel_yaw*100.0f, blimp.vel_yaw_filtd*100.0f);
}

5
Blimp/system.cpp
Unescape View File

@ -101,6 +101,11 @@ void Blimp::init_ardupilot() @@ -101,6 +101,11 @@ void Blimp::init_ardupilot()
enable_motor_output();
}
//Initialise fin filters
vel_xy_filter.init(scheduler.get_loop_rate_hz(), motors->freq_hz, 0.5f, 15.0f);
vel_z_filter.init(scheduler.get_loop_rate_hz(), motors->freq_hz, 1.0f, 15.0f);
vel_yaw_filter.init(scheduler.get_loop_rate_hz(),motors->freq_hz, 5.0f, 15.0f);
// attempt to switch to MANUAL, if this fails then switch to Land
if (!set_mode((enum Mode::Number)g.initial_mode.get(), ModeReason::INITIALISED)) {
// set mode to MANUAL will trigger mode change notification to pilot

Write Preview
Loading…
Cancel
Save