Skip to content

Commit

Permalink
Blimp: Add prop blimp
Browse files Browse the repository at this point in the history
  • Loading branch information
@MichelleRos
MichelleRos committed May 24, 2024
1 parent b0a68d6 commit c8abfca
Show file tree
Hide file tree
Showing 16 changed files with 967 additions and 390 deletions.
21 changes: 16 additions & 5 deletions Blimp/Blimp.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -212,7 +212,14 @@ void Blimp::one_hz_loop()

AP_Notify::flags.flying = !ap.land_complete;

blimp.pid_pos_yaw.set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz());
pid_vel_x.set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz());
pid_vel_y.set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz());
pid_vel_z.set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz());
pid_vel_yaw.set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz());
pid_pos_x.set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz());
pid_pos_y.set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz());
pid_pos_z.set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz());
pid_pos_yaw.set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz());
}

void Blimp::read_AHRS(void)
Expand All @@ -222,11 +229,15 @@ void Blimp::read_AHRS(void)

IGNORE_RETURN(ahrs.get_velocity_NED(vel_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);

if (g2.frame_class.get() == Fins::MOTOR_FRAME_FISHBLIMP) {
vel_ned_filtd = {vel_x_filter.apply(vel_ned.x), vel_y_filter.apply(vel_ned.y), vel_z_filter.apply(vel_ned.z)};
vel_yaw_filtd = vel_yaw_filter.apply(vel_yaw);
} else {
vel_ned_filtd = vel_ned;
vel_yaw_filtd = vel_yaw;
}

#if HAL_LOGGING_ENABLED
AP::logger().WriteStreaming("VNF", "TimeUS,X,XF,Y,YF,Z,ZF,Yaw,YawF,PX,PY,PZ,PYaw", "Qffffffffffff",
Expand Down
22 changes: 12 additions & 10 deletions Blimp/Blimp.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,8 +46,6 @@
#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 <AP_Vehicle/AP_MultiCopter.h>

Expand Down Expand Up @@ -219,21 +217,25 @@ class Blimp : public AP_Vehicle
Vector3f pos_ned;
float vel_yaw;
float vel_yaw_filtd;
NotchFilterVector2f vel_xy_filter;
NotchFilterFloat vel_x_filter;
NotchFilterFloat vel_y_filter;
NotchFilterFloat vel_z_filter;
NotchFilterFloat vel_yaw_filter;

// Inertial Navigation
AP_InertialNav inertial_nav;

// Vel & pos PIDs
AC_PID_2D pid_vel_xy{3, 0.2, 0, 0, 0.2, 3, 3}; //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};
AC_PID_Basic pid_vel_yaw{3, 0.4, 0, 0, 0.2, 3, 3};

AC_PID_2D pid_pos_xy{1, 0.05, 0, 0, 0.1, 3, 3};
AC_PID_Basic pid_pos_z{0.7, 0, 0, 0, 0, 3, 3};
AC_PID pid_pos_yaw{1.2, 0.5, 0, 0, 2, 3, 3, 3}; //p, i, d, ff, imax, filt_t, filt_e, filt_d, dt, opt srmax, opt srtau
// p i d ff imax ft fe fd
AC_PID pid_vel_x{3, 0.2, 0, 0, 0.2, 0, 0, 0};
AC_PID pid_vel_y{3, 0.2, 0, 0, 0.2, 0, 0, 0};
AC_PID pid_vel_z{7, 1.5, 0, 0, 1, 0, 0, 0};
AC_PID pid_vel_yaw{3, 0.4, 0, 0, 0.2, 0, 0, 0};

AC_PID pid_pos_x{1, 0.05, 0, 0, 0.1, 0, 0, 0};
AC_PID pid_pos_y{1, 0.05, 0, 0, 0.1, 0, 0, 0};
AC_PID pid_pos_z{0.7, 0, 0, 0, 0, 0, 0, 0};
AC_PID pid_pos_yaw{1.2, 0.5, 0, 0, 2, 0, 0, 0};

// System Timers
// --------------
Expand Down
134 changes: 115 additions & 19 deletions Blimp/Fins.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,12 +2,6 @@

#include <SRV_Channel/SRV_Channel.h>

// This is the scale used for RC inputs so that they can be scaled to the float point values used in the sine wave code.
#define FIN_SCALE_MAX 1000

/*
2nd group of parameters
*/
const AP_Param::GroupInfo Fins::var_info[] = {

// @Param: FREQ_HZ
Expand All @@ -19,24 +13,50 @@ const AP_Param::GroupInfo Fins::var_info[] = {

// @Param: TURBO_MODE
// @DisplayName: Enable turbo mode
// @Description: Enables double speed on high offset.
// @Description: Enables double speed on high offset (finned blimp only).
// @Range: 0 1
// @User: Standard
AP_GROUPINFO("TURBO_MODE", 2, Fins, turbo_mode, 0),

// @Param: THR_MAX
// @DisplayName: Maximum throttle
// @Description: Maximum throttle allowed. Constrains any throttle input to this value (negative and positive) Set it to 1 to disable (i.e. allow max throttle).
// @Range: 0 1
// @User: Standard
AP_GROUPINFO("THR_MAX", 3, Fins, thr_max, 1),

AP_GROUPEND
};

//constructor
Fins::Fins(uint16_t loop_rate) :
_loop_rate(loop_rate)
Fins::Fins(uint16_t loop_rate, motor_frame_class frame) :
_loop_rate(loop_rate),
_frame(frame)
{
AP_Param::setup_object_defaults(this, var_info);
}

void Fins::setup_finsmotors()
{
switch ((Fins::motor_frame_class)_frame) {
case Fins::MOTOR_FRAME_FISHBLIMP:
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Setting up FishBlimp.");
setup_fins();
break;
case Fins::MOTOR_FRAME_FOUR_MOTOR:
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Setting up FourMotor.");
setup_motors();
break;
default:
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "ERROR: Wrong frame class.");
break;
}
}

void Fins::setup_fins()
{
//fin # r f d y, r f d y right, front, down, yaw for amplitude then for offset
//right, front, down, yaw for amplitude then for offset
//fin # r f d y, r f d y
add_fin(0, 0, 1, 0.5, 0, 0, 0, 0.5, 0); //Back
add_fin(1, 0, -1, 0.5, 0, 0, 0, 0.5, 0); //Front
add_fin(2, -1, 0, 0, 0.5, 0, 0, 0, 0.5); //Right
Expand All @@ -48,10 +68,22 @@ void Fins::setup_fins()
SRV_Channels::set_angle(SRV_Channel::k_motor4, FIN_SCALE_MAX);
}

void Fins::setup_motors()
{ // motor# r f d y
add_motor(0, 0, 1, 0, 1); //FrontLeft
add_motor(1, 0, 1, 0, -1); //FrontRight
add_motor(2, 0, 0, -1, 0); //Up
add_motor(3, 1, 0, 0, 0); //Right

SRV_Channels::set_angle(SRV_Channel::k_motor1, FIN_SCALE_MAX);
SRV_Channels::set_angle(SRV_Channel::k_motor2, FIN_SCALE_MAX);
SRV_Channels::set_angle(SRV_Channel::k_motor3, FIN_SCALE_MAX);
SRV_Channels::set_angle(SRV_Channel::k_motor4, FIN_SCALE_MAX);
}

void Fins::add_fin(int8_t fin_num, float right_amp_fac, float front_amp_fac, float down_amp_fac, float yaw_amp_fac,
float right_off_fac, float front_off_fac, float down_off_fac, float yaw_off_fac)
{

// ensure valid fin number is provided
if (fin_num >= 0 && fin_num < NUM_FINS) {

Expand All @@ -69,6 +101,17 @@ void Fins::add_fin(int8_t fin_num, float right_amp_fac, float front_amp_fac, flo
}
}

void Fins::add_motor(int8_t fin_num, float right_amp_fac, float front_amp_fac, float down_amp_fac, float yaw_amp_fac)
{
// ensure valid fin number is provided
if (fin_num >= 0 && fin_num < NUM_FINS) {
_right_amp_factor[fin_num] = right_amp_fac;
_front_amp_factor[fin_num] = front_amp_fac;
_down_amp_factor[fin_num] = down_amp_fac;
_yaw_amp_factor[fin_num] = yaw_amp_fac;
}
}

//B,F,R,L = 0,1,2,3
void Fins::output()
{
Expand All @@ -84,12 +127,35 @@ void Fins::output()
blimp.Write_FINI(right_out, front_out, down_out, yaw_out);
#endif

if (AP_HAL::millis() % blimp.g.stream_rate < 30){
gcs().send_named_float("FINIR", right_out);
gcs().send_named_float("FINIF", front_out);
gcs().send_named_float("FINID", down_out);
gcs().send_named_float("FINIY", 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);

right_out = constrain_float(right_out, -thr_max, thr_max);
front_out = constrain_float(front_out, -thr_max, thr_max);
down_out = constrain_float(down_out, -thr_max, thr_max);
yaw_out = constrain_float(yaw_out, -thr_max, thr_max);

switch ((Fins::motor_frame_class)_frame) {
case Fins::MOTOR_FRAME_FISHBLIMP:
output_fins();
break;
case Fins::MOTOR_FRAME_FOUR_MOTOR:
output_motors();
break;
default:
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "ERROR: Wrong frame class.");
break;
}
}

void Fins::output_fins()
{
_time = AP_HAL::micros() * 1.0e-6;

for (int8_t i=0; i<NUM_FINS; i++) {
Expand Down Expand Up @@ -117,8 +183,8 @@ void Fins::output()
_off[i] = _off[i]/_num_added; //average the offsets
}

if ((_amp[i]+fabsf(_off[i])) > 1) {
_amp[i] = 1 - fabsf(_off[i]);
if ((_amp[i]+fabsf(_off[i])) > thr_max) {
_amp[i] = thr_max - fabsf(_off[i]);
}

if (turbo_mode) {
Expand All @@ -128,15 +194,27 @@ void Fins::output()
}
}
// finding and outputting current position for each servo from sine wave
_pos[i]= _amp[i]*cosf(freq_hz * _freq[i] * _time * 2 * M_PI) + _off[i];
SRV_Channels::set_output_scaled(SRV_Channels::get_motor_function(i), _pos[i] * FIN_SCALE_MAX);
_thrpos[i]= _amp[i]*cosf(freq_hz * _freq[i] * _time * 2 * M_PI) + _off[i];
SRV_Channels::set_output_scaled(SRV_Channels::get_motor_function(i), _thrpos[i] * FIN_SCALE_MAX);
}

#if HAL_LOGGING_ENABLED
blimp.Write_FINO(_amp, _off);
#endif
}

void Fins::output_motors()
{
for (int8_t i=0; i<NUM_FINS; i++) {
//Calculate throttle for each motor
_thrpos[i] = constrain_float(_right_amp_factor[i]*right_out + _front_amp_factor[i]*front_out + _down_amp_factor[i]*down_out + _yaw_amp_factor[i]*yaw_out, -thr_max, thr_max);

//Set output
SRV_Channels::set_output_scaled(SRV_Channels::get_motor_function(i), _thrpos[i] * FIN_SCALE_MAX);
}

}

void Fins::output_min()
{
right_out = 0;
Expand All @@ -145,3 +223,21 @@ void Fins::output_min()
yaw_out = 0;
Fins::output();
}

const char* Fins::get_frame_string()
{
switch ((Fins::motor_frame_class)_frame) {
case Fins::MOTOR_FRAME_FISHBLIMP:
return "FISHBLIMP";
break;
case Fins::MOTOR_FRAME_FOUR_MOTOR:
return "FOURMOTOR";
break;
default:
return "NOFRAME";
break;
}



}
31 changes: 20 additions & 11 deletions Blimp/Fins.h
View file
Original file line number Diff line number Diff line change
@@ -1,11 +1,14 @@
//This class converts horizontal acceleration commands to fin flapping commands.
#pragma once
#include <AP_Notify/AP_Notify.h>
#include <AP_AHRS/AP_AHRS.h>

extern const AP_HAL::HAL& hal;

#define NUM_FINS 4 //Current maximum number of fins that can be added.
#define RC_SCALE 1000
#define NUM_FINS 4 //Current maximum number of fins or motors that can be added.
#define RC_SCALE 1000 //Input scaling
#define FIN_SCALE_MAX 1000 //Output scaling

class Fins
{
public:
Expand All @@ -14,14 +17,14 @@ class Fins

enum motor_frame_class {
MOTOR_FRAME_UNDEFINED = 0,
MOTOR_FRAME_AIRFISH = 1,
};
enum motor_frame_type {
MOTOR_FRAME_TYPE_AIRFISH = 1,
MOTOR_FRAME_FISHBLIMP = 1,
MOTOR_FRAME_FOUR_MOTOR = 2,
};

motor_frame_class _frame;

//constructor
Fins(uint16_t loop_rate);
Fins(uint16_t loop_rate, motor_frame_class frame);

// var_info for holding Parameter information
static const struct AP_Param::GroupInfo var_info[];
Expand Down Expand Up @@ -57,7 +60,7 @@ class Fins
float _amp[NUM_FINS]; //amplitudes
float _off[NUM_FINS]; //offsets
float _freq[NUM_FINS]; //frequency multiplier
float _pos[NUM_FINS]; //servo positions
float _thrpos[NUM_FINS]; //servo positions or motor throttles

float _right_amp_factor[NUM_FINS];
float _front_amp_factor[NUM_FINS];
Expand All @@ -80,18 +83,24 @@ class Fins

AP_Float freq_hz;
AP_Int8 turbo_mode;

bool _interlock; // 1 if the motor interlock is enabled (i.e. motors run), 0 if disabled (motors don't run)
bool _initialised_ok; // 1 if initialisation was successful
AP_Float thr_max;

void output_min();

void add_fin(int8_t fin_num, float right_amp_fac, float front_amp_fac, float yaw_amp_fac, float down_amp_fac,
float right_off_fac, float front_off_fac, float yaw_off_fac, float down_off_fac);

void add_motor(int8_t fin_num, float right_amp_fac, float front_amp_fac, float yaw_amp_fac, float down_amp_fac);

void setup_finsmotors();
void setup_fins();
void setup_motors();

void output();
void output_fins();
void output_motors();

const char* get_frame_string();

float get_throttle()
{
Expand Down
8 changes: 4 additions & 4 deletions Blimp/GCS_Mavlink.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -128,10 +128,10 @@ void GCS_MAVLINK_Blimp::send_pid_tuning()
const AP_PIDInfo *pid_info = nullptr;
switch (axes[i]) {
case PID_SEND::VELX:
pid_info = &blimp.pid_vel_xy.get_pid_info_x();
pid_info = &blimp.pid_vel_x.get_pid_info();
break;
case PID_SEND::VELY:
pid_info = &blimp.pid_vel_xy.get_pid_info_y();
pid_info = &blimp.pid_vel_y.get_pid_info();
break;
case PID_SEND::VELZ:
pid_info = &blimp.pid_vel_z.get_pid_info();
Expand All @@ -140,10 +140,10 @@ void GCS_MAVLINK_Blimp::send_pid_tuning()
pid_info = &blimp.pid_vel_yaw.get_pid_info();
break;
case PID_SEND::POSX:
pid_info = &blimp.pid_pos_xy.get_pid_info_x();
pid_info = &blimp.pid_pos_x.get_pid_info();
break;
case PID_SEND::POSY:
pid_info = &blimp.pid_pos_xy.get_pid_info_y();
pid_info = &blimp.pid_pos_y.get_pid_info();
break;
case PID_SEND::POSZ:
pid_info = &blimp.pid_pos_z.get_pid_info();
Expand Down
Loading

0 comments on commit c8abfca

Please sign in to comment.