mirror of https://github.com/ArduPilot/ardupilot
Blimp: Add Loiter class and RTL mode
This commit is contained in:
Michelle Rossouw
Andrew Tridgell
parent
aa09b0b409
commit
81650d34a3
|
|
@ -217,12 +217,27 @@ void Blimp::read_AHRS(void)
|
|||
ahrs.update(true);
|
||||
|
||||
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();
|
||||
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);
|
||||
|
||||
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
|
||||
|
|
|
|||
|
|
@ -59,6 +59,7 @@
|
|||
#include "config.h"
|
||||
|
||||
#include "Fins.h"
|
||||
#include "Loiter.h"
|
||||
|
||||
#include "RC_Channel.h" // RC Channel Library
|
||||
|
||||
|
|
@ -91,8 +92,10 @@ public:
|
|||
friend class ModeLand;
|
||||
friend class ModeVelocity;
|
||||
friend class ModeLoiter;
|
||||
friend class ModeRTL;
|
||||
|
||||
friend class Fins;
|
||||
friend class Loiter;
|
||||
|
||||
Blimp(void);
|
||||
|
||||
|
|
@ -191,6 +194,7 @@ private:
|
|||
|
||||
// Motor Output
|
||||
Fins *motors;
|
||||
Loiter *loiter;
|
||||
|
||||
int32_t _home_bearing;
|
||||
uint32_t _home_distance;
|
||||
|
|
@ -430,6 +434,7 @@ private:
|
|||
ModeLand mode_land;
|
||||
ModeVelocity mode_velocity;
|
||||
ModeLoiter mode_loiter;
|
||||
ModeRTL mode_rtl;
|
||||
|
||||
// mode.cpp
|
||||
Mode *mode_from_mode_num(const Mode::Number mode);
|
||||
|
|
|
|||
|
|
@ -10,6 +10,7 @@ class Fins
|
|||
{
|
||||
public:
|
||||
friend class Blimp;
|
||||
friend class Loiter;
|
||||
|
||||
enum motor_frame_class {
|
||||
MOTOR_FRAME_UNDEFINED = 0,
|
||||
|
|
|
|||
|
|
@ -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
|
||||
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
|
||||
// @DisplayName: ESC Update Speed
|
||||
// @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_simple_mode,
|
||||
k_param_dis_mask,
|
||||
k_param_pid_dz,
|
||||
|
||||
//
|
||||
// 90: misc2
|
||||
|
|
@ -254,6 +255,7 @@ public:
|
|||
|
||||
AP_Int8 simple_mode;
|
||||
AP_Int16 dis_mask;
|
||||
AP_Float pid_dz;
|
||||
|
||||
AP_Int8 rtl_alt_type;
|
||||
|
||||
|
|
|
|||
|
|
@ -16,6 +16,7 @@ Mode::Mode(void) :
|
|||
inertial_nav(blimp.inertial_nav),
|
||||
ahrs(blimp.ahrs),
|
||||
motors(blimp.motors),
|
||||
loiter(blimp.loiter),
|
||||
channel_right(blimp.channel_right),
|
||||
channel_front(blimp.channel_front),
|
||||
channel_down(blimp.channel_down),
|
||||
|
|
@ -41,6 +42,9 @@ Mode *Blimp::mode_from_mode_num(const Mode::Number mode)
|
|||
case Mode::Number::LOITER:
|
||||
ret = &mode_loiter;
|
||||
break;
|
||||
case Mode::Number::RTL:
|
||||
ret = &mode_rtl;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
|
@ -152,18 +156,23 @@ void Mode::update_navigation()
|
|||
run_autopilot();
|
||||
}
|
||||
|
||||
// returns desired angle in centi-degrees
|
||||
void Mode::get_pilot_desired_accelerations(float &right_out, float &front_out) const
|
||||
// returns desired thrust/acceleration
|
||||
void Mode::get_pilot_input(Vector3f &pilot, float &yaw)
|
||||
{
|
||||
// throttle failsafe check
|
||||
if (blimp.failsafe.radio || !blimp.ap.rc_receiver_present) {
|
||||
right_out = 0;
|
||||
front_out = 0;
|
||||
pilot.y = 0;
|
||||
pilot.x = 0;
|
||||
pilot.z = 0;
|
||||
yaw = 0;
|
||||
return;
|
||||
}
|
||||
// fetch roll and pitch inputs
|
||||
right_out = channel_right->get_control_in();
|
||||
front_out = channel_front->get_control_in();
|
||||
// fetch pilot inputs
|
||||
pilot.y = channel_right->get_control_in() / float(RC_SCALE);
|
||||
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
|
||||
|
|
|
|||
45
Blimp/mode.h
45
Blimp/mode.h
|
|
@ -17,6 +17,7 @@ public:
|
|||
MANUAL = 1, // manual control
|
||||
VELOCITY = 2, // velocity mode
|
||||
LOITER = 3, // loiter mode (position hold)
|
||||
RTL = 4, // rtl
|
||||
};
|
||||
|
||||
// constructor
|
||||
|
|
@ -83,7 +84,7 @@ public:
|
|||
void update_navigation();
|
||||
|
||||
// pilot input processing
|
||||
void get_pilot_desired_accelerations(float &right_out, float &front_out) const;
|
||||
void get_pilot_input(Vector3f &pilot, float &yaw);
|
||||
|
||||
protected:
|
||||
|
||||
|
|
@ -104,6 +105,7 @@ protected:
|
|||
AP_InertialNav &inertial_nav;
|
||||
AP_AHRS &ahrs;
|
||||
Fins *&motors;
|
||||
Loiter *&loiter;
|
||||
RC_Channel *&channel_right;
|
||||
RC_Channel *&channel_front;
|
||||
RC_Channel *&channel_down;
|
||||
|
|
@ -304,7 +306,6 @@ protected:
|
|||
private:
|
||||
Vector3f target_pos;
|
||||
float target_yaw;
|
||||
float loop_period;
|
||||
};
|
||||
|
||||
class ModeLand : public Mode
|
||||
|
|
@ -347,3 +348,43 @@ protected:
|
|||
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,13 +3,16 @@
|
|||
* 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_yaw = blimp.ahrs.get_yaw();
|
||||
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
//Runs the main loiter controller
|
||||
void ModeLoiter::run()
|
||||
|
|
@ -17,52 +20,22 @@ void ModeLoiter::run()
|
|||
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;
|
||||
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 (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.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
|
||||
Vector3f target_vel_ef{blimp.pid_pos_xy.update_all(target_pos, blimp.pos_ned, dt, {0,0,0}), 0};
|
||||
target_vel_ef.z = blimp.pid_pos_z.update_all(target_pos.z, blimp.pos_ned.z, dt);
|
||||
float yaw_ef = blimp.ahrs.get_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);
|
||||
if(fabsf(target_pos.x-blimp.pos_ned.x) < (g.max_pos_xy*POS_LAG)) target_pos.x += pilot.x;
|
||||
if(fabsf(target_pos.y-blimp.pos_ned.y) < (g.max_pos_xy*POS_LAG)) target_pos.y += pilot.y;
|
||||
if(fabsf(target_pos.z-blimp.pos_ned.z) < (g.max_pos_z*POS_LAG)) target_pos.z += pilot.z;
|
||||
if(fabsf(wrap_PI(target_yaw-ahrs.get_yaw())) < (g.max_pos_yaw*POS_LAG)) target_yaw = wrap_PI(target_yaw + pilot_yaw);
|
||||
|
||||
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_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);
|
||||
blimp.loiter->run(target_pos, target_yaw, Vector4b{false,false,false,false});
|
||||
}
|
||||
|
|
|
|||
|
|
@ -6,8 +6,11 @@
|
|||
// Runs the main manual controller
|
||||
void ModeManual::run()
|
||||
{
|
||||
motors->right_out = channel_right->get_control_in() / float(RC_SCALE);
|
||||
motors->front_out = channel_front->get_control_in() / float(RC_SCALE);
|
||||
motors->yaw_out = channel_yaw->get_control_in() / float(RC_SCALE);
|
||||
motors->down_out = channel_down->get_control_in() / float(RC_SCALE);
|
||||
Vector3f pilot;
|
||||
float pilot_yaw;
|
||||
get_pilot_input(pilot, pilot_yaw);
|
||||
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
|
||||
void ModeVelocity::run()
|
||||
{
|
||||
const float dt = blimp.scheduler.get_last_loop_time_s();
|
||||
|
||||
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;
|
||||
float target_vel_yaw;
|
||||
get_pilot_input(target_vel, target_vel_yaw);
|
||||
target_vel.x *= g.max_vel_xy;
|
||||
target_vel.y *= g.max_vel_xy;
|
||||
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 = 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;
|
||||
}
|
||||
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.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);
|
||||
blimp.loiter->run_vel(target_vel, target_vel_yaw, Vector4b{false,false,false,false});
|
||||
}
|
||||
|
|
|
|||
|
|
@ -46,6 +46,7 @@ void Blimp::init_ardupilot()
|
|||
|
||||
// allocate the motors class
|
||||
allocate_motors();
|
||||
loiter = new Loiter(blimp.scheduler.get_loop_rate_hz());
|
||||
|
||||
// initialise rc channels including setting mode
|
||||
rc().convert_options(RC_Channel::AUX_FUNC::ARMDISARM_UNUSED, RC_Channel::AUX_FUNC::ARMDISARM);
|
||||
|
|
|
|||
Loading…
Reference in New Issue