mirror of https://github.com/ArduPilot/ardupilot
Blimp: Add Loiter class and RTL mode
This commit is contained in:
parent
aa09b0b409
commit
81650d34a3
|
@ -217,12 +217,27 @@ void Blimp::read_AHRS(void)
|
||||||
ahrs.update(true);
|
ahrs.update(true);
|
||||||
|
|
||||||
IGNORE_RETURN(ahrs.get_velocity_NED(vel_ned));
|
IGNORE_RETURN(ahrs.get_velocity_NED(vel_ned));
|
||||||
IGNORE_RETURN(ahrs.get_relative_position_NED_home(pos_ned));
|
IGNORE_RETURN(ahrs.get_relative_position_NED_origin(pos_ned));
|
||||||
|
|
||||||
vel_yaw = ahrs.get_yaw_rate_earth();
|
vel_yaw = ahrs.get_yaw_rate_earth();
|
||||||
Vector2f vel_xy_filtd = vel_xy_filter.apply({vel_ned.x, vel_ned.y});
|
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_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);
|
vel_yaw_filtd = vel_yaw_filter.apply(vel_yaw);
|
||||||
|
|
||||||
|
AP::logger().WriteStreaming("VNF", "TimeUS,X,XF,Y,YF,Z,ZF,Yaw,YawF,PX,PY,PZ,PYaw", "Qffffffffffff",
|
||||||
|
AP_HAL::micros64(),
|
||||||
|
vel_ned.x,
|
||||||
|
vel_ned_filtd.x,
|
||||||
|
vel_ned.y,
|
||||||
|
vel_ned_filtd.y,
|
||||||
|
vel_ned.z,
|
||||||
|
vel_ned_filtd.z,
|
||||||
|
vel_yaw,
|
||||||
|
vel_yaw_filtd,
|
||||||
|
pos_ned.x,
|
||||||
|
pos_ned.y,
|
||||||
|
pos_ned.z,
|
||||||
|
blimp.ahrs.get_yaw());
|
||||||
}
|
}
|
||||||
|
|
||||||
// read baro and log control tuning
|
// read baro and log control tuning
|
||||||
|
|
|
@ -59,6 +59,7 @@
|
||||||
#include "config.h"
|
#include "config.h"
|
||||||
|
|
||||||
#include "Fins.h"
|
#include "Fins.h"
|
||||||
|
#include "Loiter.h"
|
||||||
|
|
||||||
#include "RC_Channel.h" // RC Channel Library
|
#include "RC_Channel.h" // RC Channel Library
|
||||||
|
|
||||||
|
@ -91,8 +92,10 @@ public:
|
||||||
friend class ModeLand;
|
friend class ModeLand;
|
||||||
friend class ModeVelocity;
|
friend class ModeVelocity;
|
||||||
friend class ModeLoiter;
|
friend class ModeLoiter;
|
||||||
|
friend class ModeRTL;
|
||||||
|
|
||||||
friend class Fins;
|
friend class Fins;
|
||||||
|
friend class Loiter;
|
||||||
|
|
||||||
Blimp(void);
|
Blimp(void);
|
||||||
|
|
||||||
|
@ -191,6 +194,7 @@ private:
|
||||||
|
|
||||||
// Motor Output
|
// Motor Output
|
||||||
Fins *motors;
|
Fins *motors;
|
||||||
|
Loiter *loiter;
|
||||||
|
|
||||||
int32_t _home_bearing;
|
int32_t _home_bearing;
|
||||||
uint32_t _home_distance;
|
uint32_t _home_distance;
|
||||||
|
@ -430,6 +434,7 @@ private:
|
||||||
ModeLand mode_land;
|
ModeLand mode_land;
|
||||||
ModeVelocity mode_velocity;
|
ModeVelocity mode_velocity;
|
||||||
ModeLoiter mode_loiter;
|
ModeLoiter mode_loiter;
|
||||||
|
ModeRTL mode_rtl;
|
||||||
|
|
||||||
// mode.cpp
|
// mode.cpp
|
||||||
Mode *mode_from_mode_num(const Mode::Number mode);
|
Mode *mode_from_mode_num(const Mode::Number mode);
|
||||||
|
|
|
@ -10,6 +10,7 @@ class Fins
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
friend class Blimp;
|
friend class Blimp;
|
||||||
|
friend class Loiter;
|
||||||
|
|
||||||
enum motor_frame_class {
|
enum motor_frame_class {
|
||||||
MOTOR_FRAME_UNDEFINED = 0,
|
MOTOR_FRAME_UNDEFINED = 0,
|
||||||
|
@ -95,7 +96,7 @@ public:
|
||||||
float get_throttle()
|
float get_throttle()
|
||||||
{
|
{
|
||||||
//Only for Mavlink - essentially just an indicator of how hard the fins are working.
|
//Only for Mavlink - essentially just an indicator of how hard the fins are working.
|
||||||
//Note that this is the unconstrained version, so if the higher level control gives too high input,
|
//Note that this is the unconstrained version, so if the higher level control gives too high input,
|
||||||
//throttle will be displayed as more than 100.
|
//throttle will be displayed as more than 100.
|
||||||
return fmaxf(fmaxf(fabsf(down_out),fabsf(front_out)), fmaxf(fabsf(right_out),fabsf(yaw_out)));
|
return fmaxf(fmaxf(fabsf(down_out),fabsf(front_out)), fmaxf(fabsf(right_out),fabsf(yaw_out)));
|
||||||
}
|
}
|
||||||
|
|
|
@ -0,0 +1,170 @@
|
||||||
|
#include "Blimp.h"
|
||||||
|
|
||||||
|
#define MA 0.99
|
||||||
|
#define MO (1-MA)
|
||||||
|
|
||||||
|
void Loiter::run(Vector3f& target_pos, float& target_yaw, Vector4b axes_disabled)
|
||||||
|
{
|
||||||
|
const float dt = blimp.scheduler.get_last_loop_time_s();
|
||||||
|
|
||||||
|
float scaler_xz_n;
|
||||||
|
float xz_out = fabsf(blimp.motors->front_out) + fabsf(blimp.motors->down_out);
|
||||||
|
if (xz_out > 1) scaler_xz_n = 1 / xz_out;
|
||||||
|
else scaler_xz_n = 1;
|
||||||
|
scaler_xz = scaler_xz*MA + scaler_xz_n*MO;
|
||||||
|
|
||||||
|
float scaler_yyaw_n;
|
||||||
|
float yyaw_out = fabsf(blimp.motors->right_out) + fabsf(blimp.motors->yaw_out);
|
||||||
|
if (yyaw_out > 1) scaler_yyaw_n = 1 / yyaw_out;
|
||||||
|
else scaler_yyaw_n = 1;
|
||||||
|
scaler_yyaw = scaler_yyaw*MA + scaler_yyaw_n*MO;
|
||||||
|
|
||||||
|
AP::logger().WriteStreaming("BSC", "TimeUS,xz,yyaw,xzn,yyawn",
|
||||||
|
"Qffff",
|
||||||
|
AP_HAL::micros64(),
|
||||||
|
scaler_xz, scaler_yyaw, scaler_xz_n, scaler_yyaw_n);
|
||||||
|
|
||||||
|
float yaw_ef = blimp.ahrs.get_yaw();
|
||||||
|
Vector3f err_xyz = target_pos - blimp.pos_ned;
|
||||||
|
float err_yaw = wrap_PI(target_yaw - yaw_ef);
|
||||||
|
|
||||||
|
Vector4b zero;
|
||||||
|
if((fabsf(err_xyz.x) < blimp.g.pid_dz) || !blimp.motors->_armed || (blimp.g.dis_mask & (1<<(2-1)))) zero.x = true;
|
||||||
|
if((fabsf(err_xyz.y) < blimp.g.pid_dz) || !blimp.motors->_armed || (blimp.g.dis_mask & (1<<(1-1)))) zero.y = true;
|
||||||
|
if((fabsf(err_xyz.z) < blimp.g.pid_dz) || !blimp.motors->_armed || (blimp.g.dis_mask & (1<<(3-1)))) zero.z = true;
|
||||||
|
if((fabsf(err_yaw) < blimp.g.pid_dz) || !blimp.motors->_armed || (blimp.g.dis_mask & (1<<(4-1)))) zero.yaw = true;
|
||||||
|
|
||||||
|
//Disabled means "don't update PIDs or output anything at all". Zero means actually output zero thrust. I term is limited in either case."
|
||||||
|
Vector4b limit = zero || axes_disabled;
|
||||||
|
|
||||||
|
Vector3f target_vel_ef;
|
||||||
|
if(!axes_disabled.x && !axes_disabled.y) target_vel_ef = {blimp.pid_pos_xy.update_all(target_pos, blimp.pos_ned, dt, {(float)limit.x, (float)limit.y, (float)limit.z}), 0};
|
||||||
|
if(!axes_disabled.z) target_vel_ef.z = blimp.pid_pos_z.update_all(target_pos.z, blimp.pos_ned.z, dt, limit.z);
|
||||||
|
|
||||||
|
float target_vel_yaw = 0;
|
||||||
|
if (!axes_disabled.yaw) {
|
||||||
|
target_vel_yaw = blimp.pid_pos_yaw.update_error(wrap_PI(target_yaw - yaw_ef), dt, limit.yaw);
|
||||||
|
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, -blimp.g.max_vel_xy, blimp.g.max_vel_xy),
|
||||||
|
constrain_float(target_vel_ef.y, -blimp.g.max_vel_xy, blimp.g.max_vel_xy),
|
||||||
|
constrain_float(target_vel_ef.z, -blimp.g.max_vel_z, blimp.g.max_vel_z)};
|
||||||
|
float target_vel_yaw_c = constrain_float(target_vel_yaw, -blimp.g.max_vel_yaw, blimp.g.max_vel_yaw);
|
||||||
|
|
||||||
|
Vector2f target_vel_ef_c_scaled_xy = {target_vel_ef_c.x * scaler_xz, target_vel_ef_c.y * scaler_yyaw};
|
||||||
|
Vector2f vel_ned_filtd_scaled_xy = {blimp.vel_ned_filtd.x * scaler_xz, blimp.vel_ned_filtd.y * scaler_yyaw};
|
||||||
|
|
||||||
|
Vector2f actuator;
|
||||||
|
if (!axes_disabled.x && !axes_disabled.y) actuator = blimp.pid_vel_xy.update_all(target_vel_ef_c_scaled_xy, vel_ned_filtd_scaled_xy, dt, {(float)limit.x, (float)limit.y});
|
||||||
|
float act_down = 0;
|
||||||
|
if(!axes_disabled.z) act_down = blimp.pid_vel_z.update_all(target_vel_ef_c.z * scaler_xz, blimp.vel_ned_filtd.z * scaler_xz, dt, limit.z);
|
||||||
|
blimp.rotate_NE_to_BF(actuator);
|
||||||
|
float act_yaw = 0;
|
||||||
|
if(!axes_disabled.yaw) act_yaw = blimp.pid_vel_yaw.update_all(target_vel_yaw_c * scaler_yyaw, blimp.vel_yaw_filtd * scaler_yyaw, dt, limit.yaw);
|
||||||
|
|
||||||
|
if (!blimp.motors->armed()) {
|
||||||
|
blimp.pid_pos_xy.set_integrator(Vector2f(0,0));
|
||||||
|
blimp.pid_pos_z.set_integrator(0);
|
||||||
|
blimp.pid_pos_yaw.set_integrator(0);
|
||||||
|
blimp.pid_vel_xy.set_integrator(Vector2f(0,0));
|
||||||
|
blimp.pid_vel_z.set_integrator(0);
|
||||||
|
blimp.pid_vel_yaw.set_integrator(0);
|
||||||
|
target_pos = blimp.pos_ned;
|
||||||
|
target_yaw = blimp.ahrs.get_yaw();
|
||||||
|
}
|
||||||
|
|
||||||
|
if (zero.x) {
|
||||||
|
blimp.motors->front_out = 0;
|
||||||
|
} else if (axes_disabled.x);
|
||||||
|
else {
|
||||||
|
blimp.motors->front_out = actuator.x;
|
||||||
|
}
|
||||||
|
if (zero.y) {
|
||||||
|
blimp.motors->right_out = 0;
|
||||||
|
} else if (axes_disabled.y);
|
||||||
|
else {
|
||||||
|
blimp.motors->right_out = actuator.y;
|
||||||
|
}
|
||||||
|
if (zero.z) {
|
||||||
|
blimp.motors->down_out = 0;
|
||||||
|
} else if (axes_disabled.z);
|
||||||
|
else {
|
||||||
|
blimp.motors->down_out = act_down;
|
||||||
|
}
|
||||||
|
if (zero.yaw) {
|
||||||
|
blimp.motors->yaw_out = 0;
|
||||||
|
} else if (axes_disabled.yaw);
|
||||||
|
else {
|
||||||
|
blimp.motors->yaw_out = act_yaw;
|
||||||
|
}
|
||||||
|
|
||||||
|
AP::logger().Write_PSCN(target_pos.x * 100.0, blimp.pos_ned.x * 100.0, 0.0, target_vel_ef_c.x * 100.0, blimp.vel_ned_filtd.x * 100.0, 0.0, 0.0, 0.0);
|
||||||
|
AP::logger().Write_PSCE(target_pos.y * 100.0, blimp.pos_ned.y * 100.0, 0.0, target_vel_ef_c.y * 100.0, blimp.vel_ned_filtd.y * 100.0, 0.0, 0.0, 0.0);
|
||||||
|
AP::logger().Write_PSCD(-target_pos.z * 100.0, -blimp.pos_ned.z * 100.0, 0.0, -target_vel_ef_c.z * 100.0, -blimp.vel_ned_filtd.z * 100.0, 0.0, 0.0, 0.0);
|
||||||
|
}
|
||||||
|
|
||||||
|
void Loiter::run_vel(Vector3f& target_vel_ef, float& target_vel_yaw, Vector4b axes_disabled)
|
||||||
|
{
|
||||||
|
const float dt = blimp.scheduler.get_last_loop_time_s();
|
||||||
|
|
||||||
|
Vector4b zero;
|
||||||
|
if(!blimp.motors->_armed || (blimp.g.dis_mask & (1<<(2-1)))) zero.x = true;
|
||||||
|
if(!blimp.motors->_armed || (blimp.g.dis_mask & (1<<(1-1)))) zero.y = true;
|
||||||
|
if(!blimp.motors->_armed || (blimp.g.dis_mask & (1<<(3-1)))) zero.z = true;
|
||||||
|
if(!blimp.motors->_armed || (blimp.g.dis_mask & (1<<(4-1)))) zero.yaw = true;
|
||||||
|
//Disabled means "don't update PIDs or output anything at all". Zero means actually output zero thrust. I term is limited in either case."
|
||||||
|
Vector4b limit = zero || axes_disabled;
|
||||||
|
|
||||||
|
Vector3f target_vel_ef_c{constrain_float(target_vel_ef.x, -blimp.g.max_vel_xy, blimp.g.max_vel_xy),
|
||||||
|
constrain_float(target_vel_ef.y, -blimp.g.max_vel_xy, blimp.g.max_vel_xy),
|
||||||
|
constrain_float(target_vel_ef.z, -blimp.g.max_vel_z, blimp.g.max_vel_z)};
|
||||||
|
float target_vel_yaw_c = constrain_float(target_vel_yaw, -blimp.g.max_vel_yaw, blimp.g.max_vel_yaw);
|
||||||
|
|
||||||
|
Vector2f target_vel_ef_c_scaled_xy = {target_vel_ef_c.x * scaler_xz, target_vel_ef_c.y * scaler_yyaw};
|
||||||
|
Vector2f vel_ned_filtd_scaled_xy = {blimp.vel_ned_filtd.x * scaler_xz, blimp.vel_ned_filtd.y * scaler_yyaw};
|
||||||
|
|
||||||
|
Vector2f actuator;
|
||||||
|
if (!axes_disabled.x && !axes_disabled.y) actuator = blimp.pid_vel_xy.update_all(target_vel_ef_c_scaled_xy, vel_ned_filtd_scaled_xy, dt, {(float)limit.x, (float)limit.y});
|
||||||
|
float act_down = 0;
|
||||||
|
if(!axes_disabled.z) act_down = blimp.pid_vel_z.update_all(target_vel_ef_c.z * scaler_xz, blimp.vel_ned_filtd.z * scaler_xz, dt, limit.z);
|
||||||
|
blimp.rotate_NE_to_BF(actuator);
|
||||||
|
float act_yaw = 0;
|
||||||
|
if(!axes_disabled.yaw) act_yaw = blimp.pid_vel_yaw.update_all(target_vel_yaw_c * scaler_yyaw, blimp.vel_yaw_filtd * scaler_yyaw, dt, limit.yaw);
|
||||||
|
|
||||||
|
if (!blimp.motors->armed()) {
|
||||||
|
blimp.pid_vel_xy.set_integrator(Vector2f(0,0));
|
||||||
|
blimp.pid_vel_z.set_integrator(0);
|
||||||
|
blimp.pid_vel_yaw.set_integrator(0);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (zero.x) {
|
||||||
|
blimp.motors->front_out = 0;
|
||||||
|
} else if (axes_disabled.x);
|
||||||
|
else {
|
||||||
|
blimp.motors->front_out = actuator.x;
|
||||||
|
}
|
||||||
|
if (zero.y) {
|
||||||
|
blimp.motors->right_out = 0;
|
||||||
|
} else if (axes_disabled.y);
|
||||||
|
else {
|
||||||
|
blimp.motors->right_out = actuator.y;
|
||||||
|
}
|
||||||
|
if (zero.z) {
|
||||||
|
blimp.motors->down_out = 0;
|
||||||
|
} else if (axes_disabled.z);
|
||||||
|
else {
|
||||||
|
blimp.motors->down_out = act_down;
|
||||||
|
}
|
||||||
|
if (zero.yaw) {
|
||||||
|
blimp.motors->yaw_out = 0;
|
||||||
|
} else if (axes_disabled.yaw);
|
||||||
|
else {
|
||||||
|
blimp.motors->yaw_out = act_yaw;
|
||||||
|
}
|
||||||
|
|
||||||
|
AP::logger().Write_PSCN(0.0, blimp.pos_ned.x * 100.0, 0.0, target_vel_ef_c.x * 100.0, blimp.vel_ned_filtd.x * 100.0, 0.0, 0.0, 0.0);
|
||||||
|
AP::logger().Write_PSCE(0.0, blimp.pos_ned.y * 100.0, 0.0, target_vel_ef_c.y * 100.0, blimp.vel_ned_filtd.y * 100.0, 0.0, 0.0, 0.0);
|
||||||
|
AP::logger().Write_PSCD(0.0, -blimp.pos_ned.z * 100.0, 0.0, -target_vel_ef_c.z * 100.0, -blimp.vel_ned_filtd.z * 100.0, 0.0, 0.0, 0.0);
|
||||||
|
}
|
|
@ -0,0 +1,59 @@
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
class Vector4b
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
bool x;
|
||||||
|
bool y;
|
||||||
|
bool z;
|
||||||
|
bool yaw;
|
||||||
|
|
||||||
|
constexpr Vector4b()
|
||||||
|
: x(0)
|
||||||
|
, y(0)
|
||||||
|
, z(0)
|
||||||
|
, yaw(0) {}
|
||||||
|
|
||||||
|
constexpr Vector4b(const bool x0, const bool y0, const bool z0, const bool yaw0)
|
||||||
|
: x(x0)
|
||||||
|
, y(y0)
|
||||||
|
, z(z0)
|
||||||
|
, yaw(yaw0) {}
|
||||||
|
|
||||||
|
Vector4b operator &&(const Vector4b &v)
|
||||||
|
{
|
||||||
|
Vector4b temp{x && v.x, y && v.y, z && v.z, yaw && v.yaw};
|
||||||
|
return temp;
|
||||||
|
}
|
||||||
|
|
||||||
|
Vector4b operator ||(const Vector4b &v)
|
||||||
|
{
|
||||||
|
Vector4b temp{x || v.x, y || v.y, z || v.z, yaw || v.yaw};
|
||||||
|
return temp;
|
||||||
|
}
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
class Loiter
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
friend class Blimp;
|
||||||
|
friend class Fins;
|
||||||
|
|
||||||
|
float scaler_xz;
|
||||||
|
float scaler_yyaw;
|
||||||
|
|
||||||
|
//constructor
|
||||||
|
Loiter(uint16_t loop_rate)
|
||||||
|
{
|
||||||
|
scaler_xz = 1;
|
||||||
|
scaler_yyaw = 1;
|
||||||
|
};
|
||||||
|
|
||||||
|
//Run Loiter controller with target position and yaw in global frame. Expects to be called at loop rate.
|
||||||
|
void run(Vector3f& target_pos, float& target_yaw, Vector4b axes_disabled);
|
||||||
|
//Run Loiter controller with target velocity and velocity in global frame. Expects to be called at loop rate.
|
||||||
|
void run_vel(Vector3f& target_vel, float& target_vel_yaw, Vector4b axes_disabled);
|
||||||
|
};
|
|
@ -278,6 +278,14 @@ const AP_Param::Info Blimp::var_info[] = {
|
||||||
// @User: Standard
|
// @User: Standard
|
||||||
GSCALAR(dis_mask, "DIS_MASK", 0),
|
GSCALAR(dis_mask, "DIS_MASK", 0),
|
||||||
|
|
||||||
|
// @Param: PID_DZ
|
||||||
|
// @DisplayName: Deadzone for the position PIDs
|
||||||
|
// @Description: Output 0 thrust signal when blimp is within this distance (in meters) of the target position. Warning: If this param is greater than MAX_POS_XY param then the blimp won't move at all in the XY plane in Loiter mode as it does not allow more than a second's lag. Same for the other axes.
|
||||||
|
// @Units: m
|
||||||
|
// @Range: 0.1 1
|
||||||
|
// @User: Standard
|
||||||
|
GSCALAR(pid_dz, "PID_DZ", 0),
|
||||||
|
|
||||||
// @Param: RC_SPEED
|
// @Param: RC_SPEED
|
||||||
// @DisplayName: ESC Update Speed
|
// @DisplayName: ESC Update Speed
|
||||||
// @Description: This is the speed in Hertz that your ESCs will receive updates
|
// @Description: This is the speed in Hertz that your ESCs will receive updates
|
||||||
|
|
|
@ -110,6 +110,7 @@ public:
|
||||||
k_param_max_pos_yaw,
|
k_param_max_pos_yaw,
|
||||||
k_param_simple_mode,
|
k_param_simple_mode,
|
||||||
k_param_dis_mask,
|
k_param_dis_mask,
|
||||||
|
k_param_pid_dz,
|
||||||
|
|
||||||
//
|
//
|
||||||
// 90: misc2
|
// 90: misc2
|
||||||
|
@ -254,6 +255,7 @@ public:
|
||||||
|
|
||||||
AP_Int8 simple_mode;
|
AP_Int8 simple_mode;
|
||||||
AP_Int16 dis_mask;
|
AP_Int16 dis_mask;
|
||||||
|
AP_Float pid_dz;
|
||||||
|
|
||||||
AP_Int8 rtl_alt_type;
|
AP_Int8 rtl_alt_type;
|
||||||
|
|
||||||
|
|
|
@ -16,6 +16,7 @@ Mode::Mode(void) :
|
||||||
inertial_nav(blimp.inertial_nav),
|
inertial_nav(blimp.inertial_nav),
|
||||||
ahrs(blimp.ahrs),
|
ahrs(blimp.ahrs),
|
||||||
motors(blimp.motors),
|
motors(blimp.motors),
|
||||||
|
loiter(blimp.loiter),
|
||||||
channel_right(blimp.channel_right),
|
channel_right(blimp.channel_right),
|
||||||
channel_front(blimp.channel_front),
|
channel_front(blimp.channel_front),
|
||||||
channel_down(blimp.channel_down),
|
channel_down(blimp.channel_down),
|
||||||
|
@ -41,6 +42,9 @@ Mode *Blimp::mode_from_mode_num(const Mode::Number mode)
|
||||||
case Mode::Number::LOITER:
|
case Mode::Number::LOITER:
|
||||||
ret = &mode_loiter;
|
ret = &mode_loiter;
|
||||||
break;
|
break;
|
||||||
|
case Mode::Number::RTL:
|
||||||
|
ret = &mode_rtl;
|
||||||
|
break;
|
||||||
default:
|
default:
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
@ -152,18 +156,23 @@ void Mode::update_navigation()
|
||||||
run_autopilot();
|
run_autopilot();
|
||||||
}
|
}
|
||||||
|
|
||||||
// returns desired angle in centi-degrees
|
// returns desired thrust/acceleration
|
||||||
void Mode::get_pilot_desired_accelerations(float &right_out, float &front_out) const
|
void Mode::get_pilot_input(Vector3f &pilot, float &yaw)
|
||||||
{
|
{
|
||||||
// throttle failsafe check
|
// throttle failsafe check
|
||||||
if (blimp.failsafe.radio || !blimp.ap.rc_receiver_present) {
|
if (blimp.failsafe.radio || !blimp.ap.rc_receiver_present) {
|
||||||
right_out = 0;
|
pilot.y = 0;
|
||||||
front_out = 0;
|
pilot.x = 0;
|
||||||
|
pilot.z = 0;
|
||||||
|
yaw = 0;
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
// fetch roll and pitch inputs
|
// fetch pilot inputs
|
||||||
right_out = channel_right->get_control_in();
|
pilot.y = channel_right->get_control_in() / float(RC_SCALE);
|
||||||
front_out = channel_front->get_control_in();
|
pilot.x = channel_front->get_control_in() / float(RC_SCALE);
|
||||||
|
//TODO: need to make this channel_up instead, and then have it .negative. before being sent to pilot.z -> this is "throttle" channel, so higher = up.
|
||||||
|
pilot.z = -channel_up->get_control_in() / float(RC_SCALE);
|
||||||
|
yaw = channel_yaw->get_control_in() / float(RC_SCALE);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool Mode::is_disarmed_or_landed() const
|
bool Mode::is_disarmed_or_landed() const
|
||||||
|
|
45
Blimp/mode.h
45
Blimp/mode.h
|
@ -17,6 +17,7 @@ public:
|
||||||
MANUAL = 1, // manual control
|
MANUAL = 1, // manual control
|
||||||
VELOCITY = 2, // velocity mode
|
VELOCITY = 2, // velocity mode
|
||||||
LOITER = 3, // loiter mode (position hold)
|
LOITER = 3, // loiter mode (position hold)
|
||||||
|
RTL = 4, // rtl
|
||||||
};
|
};
|
||||||
|
|
||||||
// constructor
|
// constructor
|
||||||
|
@ -83,7 +84,7 @@ public:
|
||||||
void update_navigation();
|
void update_navigation();
|
||||||
|
|
||||||
// pilot input processing
|
// pilot input processing
|
||||||
void get_pilot_desired_accelerations(float &right_out, float &front_out) const;
|
void get_pilot_input(Vector3f &pilot, float &yaw);
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
|
|
||||||
|
@ -104,6 +105,7 @@ protected:
|
||||||
AP_InertialNav &inertial_nav;
|
AP_InertialNav &inertial_nav;
|
||||||
AP_AHRS &ahrs;
|
AP_AHRS &ahrs;
|
||||||
Fins *&motors;
|
Fins *&motors;
|
||||||
|
Loiter *&loiter;
|
||||||
RC_Channel *&channel_right;
|
RC_Channel *&channel_right;
|
||||||
RC_Channel *&channel_front;
|
RC_Channel *&channel_front;
|
||||||
RC_Channel *&channel_down;
|
RC_Channel *&channel_down;
|
||||||
|
@ -304,7 +306,6 @@ protected:
|
||||||
private:
|
private:
|
||||||
Vector3f target_pos;
|
Vector3f target_pos;
|
||||||
float target_yaw;
|
float target_yaw;
|
||||||
float loop_period;
|
|
||||||
};
|
};
|
||||||
|
|
||||||
class ModeLand : public Mode
|
class ModeLand : public Mode
|
||||||
|
@ -347,3 +348,43 @@ protected:
|
||||||
private:
|
private:
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
|
class ModeRTL : 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 "RTL";
|
||||||
|
}
|
||||||
|
const char *name4() const override
|
||||||
|
{
|
||||||
|
return "RTL";
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
|
@ -3,66 +3,39 @@
|
||||||
* Init and run calls for loiter flight mode
|
* Init and run calls for loiter flight mode
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
//Number of seconds of movement that the target position can be ahead of actual position.
|
||||||
|
#define POS_LAG 1
|
||||||
|
|
||||||
bool ModeLoiter::init(bool ignore_checks){
|
bool ModeLoiter::init(bool ignore_checks)
|
||||||
|
{
|
||||||
target_pos = blimp.pos_ned;
|
target_pos = blimp.pos_ned;
|
||||||
target_yaw = blimp.ahrs.get_yaw();
|
target_yaw = blimp.ahrs.get_yaw();
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
//Runs the main loiter controller
|
//Runs the main loiter controller
|
||||||
void ModeLoiter::run()
|
void ModeLoiter::run()
|
||||||
{
|
{
|
||||||
const float dt = blimp.scheduler.get_last_loop_time_s();
|
const float dt = blimp.scheduler.get_last_loop_time_s();
|
||||||
|
|
||||||
Vector3f pilot;
|
|
||||||
pilot.x = channel_front->get_control_in() / float(RC_SCALE) * g.max_pos_xy * dt;
|
|
||||||
pilot.y = channel_right->get_control_in() / float(RC_SCALE) * g.max_pos_xy * dt;
|
|
||||||
pilot.z = channel_down->get_control_in() / float(RC_SCALE) * g.max_pos_z * dt;
|
|
||||||
float pilot_yaw = channel_yaw->get_control_in() / float(RC_SCALE) * g.max_pos_yaw * dt;
|
|
||||||
|
|
||||||
if (g.simple_mode == 0){
|
Vector3f pilot;
|
||||||
|
float pilot_yaw;
|
||||||
|
get_pilot_input(pilot, pilot_yaw);
|
||||||
|
pilot.x *= g.max_pos_xy * dt;
|
||||||
|
pilot.y *= g.max_pos_xy * dt;
|
||||||
|
pilot.z *= g.max_pos_z * dt;
|
||||||
|
pilot_yaw *= g.max_pos_yaw * dt;
|
||||||
|
|
||||||
|
if (g.simple_mode == 0) {
|
||||||
//If simple mode is disabled, input is in body-frame, thus needs to be rotated.
|
//If simple mode is disabled, input is in body-frame, thus needs to be rotated.
|
||||||
blimp.rotate_BF_to_NE(pilot.xy());
|
blimp.rotate_BF_to_NE(pilot.xy());
|
||||||
}
|
}
|
||||||
target_pos.x += pilot.x;
|
|
||||||
target_pos.y += pilot.y;
|
|
||||||
target_pos.z += pilot.z;
|
|
||||||
target_yaw = wrap_PI(target_yaw + pilot_yaw);
|
|
||||||
|
|
||||||
//Pos controller's output becomes target for velocity controller
|
if(fabsf(target_pos.x-blimp.pos_ned.x) < (g.max_pos_xy*POS_LAG)) target_pos.x += pilot.x;
|
||||||
Vector3f target_vel_ef{blimp.pid_pos_xy.update_all(target_pos, blimp.pos_ned, dt, {0,0,0}), 0};
|
if(fabsf(target_pos.y-blimp.pos_ned.y) < (g.max_pos_xy*POS_LAG)) target_pos.y += pilot.y;
|
||||||
target_vel_ef.z = blimp.pid_pos_z.update_all(target_pos.z, blimp.pos_ned.z, dt);
|
if(fabsf(target_pos.z-blimp.pos_ned.z) < (g.max_pos_z*POS_LAG)) target_pos.z += pilot.z;
|
||||||
float yaw_ef = blimp.ahrs.get_yaw();
|
if(fabsf(wrap_PI(target_yaw-ahrs.get_yaw())) < (g.max_pos_yaw*POS_LAG)) target_yaw = wrap_PI(target_yaw + pilot_yaw);
|
||||||
float target_vel_yaw = blimp.pid_pos_yaw.update_error(wrap_PI(target_yaw - yaw_ef), dt);
|
|
||||||
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),
|
blimp.loiter->run(target_pos, target_yaw, Vector4b{false,false,false,false});
|
||||||
constrain_float(target_vel_ef.y, -g.max_vel_xy, g.max_vel_xy),
|
|
||||||
constrain_float(target_vel_ef.z, -g.max_vel_z, g.max_vel_z)};
|
|
||||||
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, dt, {0,0,0});
|
|
||||||
float act_down = blimp.pid_vel_z.update_all(target_vel_ef_c.z, blimp.vel_ned_filtd.z, dt);
|
|
||||||
blimp.rotate_NE_to_BF(actuator);
|
|
||||||
float act_yaw = blimp.pid_vel_yaw.update_all(target_vel_yaw_c, blimp.vel_yaw_filtd, dt);
|
|
||||||
|
|
||||||
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_PSCN(target_pos.x * 100.0, blimp.pos_ned.x * 100.0, 0.0, target_vel_ef_c.x * 100.0, blimp.vel_ned_filtd.x * 100.0, 0.0, 0.0, 0.0);
|
|
||||||
AP::logger().Write_PSCE(target_pos.y * 100.0, blimp.pos_ned.y * 100.0, 0.0, target_vel_ef_c.y * 100.0, blimp.vel_ned_filtd.y * 100.0, 0.0, 0.0, 0.0);
|
|
||||||
AP::logger().Write_PSCD(-target_pos.z * 100.0, -blimp.pos_ned.z * 100.0, 0.0, -target_vel_ef_c.z * 100.0, -blimp.vel_ned_filtd.z * 100.0, 0.0, 0.0, 0.0);
|
|
||||||
}
|
}
|
||||||
|
|
|
@ -6,8 +6,11 @@
|
||||||
// Runs the main manual controller
|
// Runs the main manual controller
|
||||||
void ModeManual::run()
|
void ModeManual::run()
|
||||||
{
|
{
|
||||||
motors->right_out = channel_right->get_control_in() / float(RC_SCALE);
|
Vector3f pilot;
|
||||||
motors->front_out = channel_front->get_control_in() / float(RC_SCALE);
|
float pilot_yaw;
|
||||||
motors->yaw_out = channel_yaw->get_control_in() / float(RC_SCALE);
|
get_pilot_input(pilot, pilot_yaw);
|
||||||
motors->down_out = channel_down->get_control_in() / float(RC_SCALE);
|
motors->right_out = pilot.y;
|
||||||
|
motors->front_out = pilot.x;
|
||||||
|
motors->yaw_out = pilot_yaw;
|
||||||
|
motors->down_out = pilot.z;
|
||||||
}
|
}
|
||||||
|
|
|
@ -0,0 +1,20 @@
|
||||||
|
#include "Blimp.h"
|
||||||
|
/*
|
||||||
|
* Init and run calls for rtl flight mode
|
||||||
|
*/
|
||||||
|
|
||||||
|
//Number of seconds of movement that the target position can be ahead of actual position.
|
||||||
|
#define POS_LAG 1
|
||||||
|
|
||||||
|
bool ModeRTL::init(bool ignore_checks)
|
||||||
|
{
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
//Runs the main rtl controller
|
||||||
|
void ModeRTL::run()
|
||||||
|
{
|
||||||
|
Vector3f target_pos = {0,0,0};
|
||||||
|
float target_yaw = 0;
|
||||||
|
blimp.loiter->run(target_pos, target_yaw, Vector4b{false,false,false,false});
|
||||||
|
}
|
|
@ -8,34 +8,17 @@
|
||||||
// Runs the main velocity controller
|
// Runs the main velocity controller
|
||||||
void ModeVelocity::run()
|
void ModeVelocity::run()
|
||||||
{
|
{
|
||||||
const float dt = blimp.scheduler.get_last_loop_time_s();
|
|
||||||
|
|
||||||
Vector3f target_vel;
|
Vector3f target_vel;
|
||||||
target_vel.x = channel_front->get_control_in() / float(RC_SCALE) * g.max_vel_xy;
|
float target_vel_yaw;
|
||||||
target_vel.y = channel_right->get_control_in() / float(RC_SCALE) * g.max_vel_xy;
|
get_pilot_input(target_vel, target_vel_yaw);
|
||||||
blimp.rotate_BF_to_NE(target_vel.xy());
|
target_vel.x *= g.max_vel_xy;
|
||||||
target_vel.z = channel_down->get_control_in() / float(RC_SCALE) * g.max_vel_z;
|
target_vel.y *= g.max_vel_xy;
|
||||||
float target_vel_yaw = channel_yaw->get_control_in() / float(RC_SCALE) * g.max_vel_yaw;
|
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(target_vel.xy());
|
||||||
|
}
|
||||||
|
target_vel.z *= g.max_vel_z;
|
||||||
|
target_vel_yaw *= g.max_vel_yaw;
|
||||||
|
|
||||||
Vector2f actuator = blimp.pid_vel_xy.update_all(target_vel, blimp.vel_ned_filtd, dt, {0,0,0});
|
blimp.loiter->run_vel(target_vel, target_vel_yaw, Vector4b{false,false,false,false});
|
||||||
blimp.rotate_NE_to_BF(actuator);
|
|
||||||
float act_down = blimp.pid_vel_z.update_all(target_vel.z, blimp.vel_ned_filtd.z, dt);
|
|
||||||
float act_yaw = blimp.pid_vel_yaw.update_all(target_vel_yaw, blimp.vel_yaw_filtd, dt);
|
|
||||||
|
|
||||||
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_PSCN(0.0, blimp.pos_ned.x * 100.0, 0.0, 0.0, blimp.vel_ned_filtd.x * 100.0, 0.0, 0.0, 0.0);
|
|
||||||
AP::logger().Write_PSCE(0.0, blimp.pos_ned.y * 100.0, 0.0, 0.0, blimp.vel_ned_filtd.y * 100.0, 0.0, 0.0, 0.0);
|
|
||||||
AP::logger().Write_PSCD(0.0, -blimp.pos_ned.z * 100.0, 0.0, 0.0, -blimp.vel_ned_filtd.z * 100.0, 0.0, 0.0, 0.0);
|
|
||||||
}
|
}
|
||||||
|
|
|
@ -46,6 +46,7 @@ void Blimp::init_ardupilot()
|
||||||
|
|
||||||
// allocate the motors class
|
// allocate the motors class
|
||||||
allocate_motors();
|
allocate_motors();
|
||||||
|
loiter = new Loiter(blimp.scheduler.get_loop_rate_hz());
|
||||||
|
|
||||||
// initialise rc channels including setting mode
|
// initialise rc channels including setting mode
|
||||||
rc().convert_options(RC_Channel::AUX_FUNC::ARMDISARM_UNUSED, RC_Channel::AUX_FUNC::ARMDISARM);
|
rc().convert_options(RC_Channel::AUX_FUNC::ARMDISARM_UNUSED, RC_Channel::AUX_FUNC::ARMDISARM);
|
||||||
|
|
Loading…
Reference in New Issue