mirror of https://github.com/ArduPilot/ardupilot
Blimp: Add modes Velocity and Loiter
This commit is contained in:
Michelle Rossouw
Andrew Tridgell
parent
1a0f44a8ad
commit
5bd3e3e330
|
|
@ -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)
|
|||
{
|
||||
// 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()
|
|||
}
|
||||
}
|
||||
|
||||
//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;
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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:
|
|||
friend class Mode;
|
||||
friend class ModeManual;
|
||||
friend class ModeLand;
|
||||
friend class ModeVelocity;
|
||||
friend class ModeLoiter;
|
||||
|
||||
friend class Fins;
|
||||
|
||||
|
|
@ -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;
|
||||
|
||||
// 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};
|
||||
|
||||
// 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;
|
||||
|
||||
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:
|
|||
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:
|
|||
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);
|
||||
|
|
|
|||
|
|
@ -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++) {
|
||||
|
|
|
|||
|
|
@ -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[] = {
|
|||
// @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} },
|
||||
|
|
|
|||
|
|
@ -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:
|
|||
//
|
||||
// 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:
|
|||
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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
89
Blimp/mode.h
|
|
@ -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:
|
|||
|
||||
};
|
||||
|
||||
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
|
||||
{
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
|
@ -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);
|
||||
}
|
||||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Reference in New Issue