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

Blimp: Add Loiter class and RTL mode

This commit is contained in:
Michelle Rossouw 2023-07-27 15:03:08 +10:00 committed by Andrew Tridgell
parent aa09b0b409
commit 81650d34a3
14 changed files with 379 additions and 89 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
Blimp/Blimp.cpp
View File

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

5
Blimp/Blimp.h
View File

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

3
Blimp/Fins.h
View File

@ -10,6 +10,7 @@ class Fins
{
public:
friend class Blimp;
friend class Loiter;
enum motor_frame_class {
MOTOR_FRAME_UNDEFINED = 0,
@ -95,7 +96,7 @@ public:
float get_throttle()
{
//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.
return fmaxf(fmaxf(fabsf(down_out),fabsf(front_out)), fmaxf(fabsf(right_out),fabsf(yaw_out)));
}

170
Blimp/Loiter.cpp Normal file
View File

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

59
Blimp/Loiter.h Normal file
View File

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

8
Blimp/Parameters.cpp
View File

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

2
Blimp/Parameters.h
View File

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

23
Blimp/mode.cpp
View File

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

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

65
Blimp/mode_loiter.cpp
View File

@ -3,66 +3,39 @@
* 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()
{
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.
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});
}

11
Blimp/mode_manual.cpp
View File

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

20
Blimp/mode_rtl.cpp Normal file
View File

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

39
Blimp/mode_velocity.cpp
View File

@ -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;
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;
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 *= 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});
}

1
Blimp/system.cpp
View File

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