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

Blimp: Code cleanups

This commit is contained in:
Michelle Rossouw 2021-06-22 16:36:11 +10:00 committed by Andrew Tridgell
parent 5bd3e3e330
commit 0e2cb4a00c
11 changed files with 114 additions and 121 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
Blimp/AP_Arming.cpp
View File

@ -40,14 +40,6 @@ bool AP_Arming_Blimp::run_pre_arm_checks(bool display_failure)
return false;
}
// check if motor interlock aux switch is in use
// if it is, switch needs to be in disabled position to arm
// otherwise exit immediately. This check to be repeated,
// as state can change at any time.
if (blimp.ap.using_interlock && blimp.ap.motor_interlock_switch) {
check_failed(display_failure, "Motor Interlock Enabled");
}
// if pre arm checks are disabled run only the mandatory checks
if (checks_to_perform == 0) {
return mandatory_checks(display_failure);
@ -155,7 +147,7 @@ bool AP_Arming_Blimp::parameter_checks(bool display_failure)
}
}
}
return true;
}
@ -273,6 +265,15 @@ bool AP_Arming_Blimp::mandatory_gps_checks(bool display_failure)
return true;
}
// check for GPS glitch (as reported by EKF)
nav_filter_status filt_status;
if (ahrs.get_filter_status(filt_status)) {
if (filt_status.flags.gps_glitching) {
check_failed(display_failure, "GPS glitching");
return false;
}
}
// if we got here all must be ok
return true;
}
@ -383,7 +384,7 @@ bool AP_Arming_Blimp::arm(const AP_Arming::Method method, const bool do_arming_c
blimp.arming_altitude_m = blimp.inertial_nav.get_altitude() * 0.01;
}
// enable gps velocity based centrefugal force compensation
// enable gps velocity based centrifugal force compensation
ahrs.set_correct_centrifugal(true);
hal.util->set_soft_armed(true);

26
Blimp/Blimp.cpp
View File

@ -142,7 +142,7 @@ void Blimp::update_batt_compass(void)
// Full rate logging of attitude, rate and pid loops
void Blimp::full_rate_logging()
{
if (should_log(MASK_LOG_ATTITUDE_FAST) && !blimp.flightmode->logs_attitude()) {
if (should_log(MASK_LOG_ATTITUDE_FAST)) {
Log_Write_Attitude();
}
if (should_log(MASK_LOG_PID)) {
@ -155,7 +155,7 @@ void Blimp::full_rate_logging()
void Blimp::ten_hz_logging_loop()
{
// log attitude data if we're not already logging at the higher rate
if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST) && !blimp.flightmode->logs_attitude()) {
if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST)) {
Log_Write_Attitude();
}
// log EKF attitude data
@ -223,7 +223,7 @@ 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));
@ -249,20 +249,20 @@ 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)
void Blimp::rotate_BF_to_NE(Vector2f &vec)
{
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;
float ne_x = vec.x*ahrs.cos_yaw() - vec.y*ahrs.sin_yaw();
float ne_y = vec.x*ahrs.sin_yaw() + vec.y*ahrs.cos_yaw();
vec.x = ne_x;
vec.y = ne_y;
}
void Blimp::rotate_NE_to_BF(float &x, float &y)
void Blimp::rotate_NE_to_BF(Vector2f &vec)
{
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;
float bf_x = vec.x*ahrs.cos_yaw() + vec.y*ahrs.sin_yaw();
float bf_y = -vec.x*ahrs.sin_yaw() + vec.y*ahrs.cos_yaw();
vec.x = bf_x;
vec.y = bf_y;
}

51
Blimp/Blimp.h
View File

@ -150,35 +150,26 @@ private:
// Documentation of Globals:
typedef union {
struct {
uint8_t unused1 : 1; // 0
uint8_t pre_arm_rc_check : 1; // 3 // true if rc input pre-arm checks have been completed successfully
uint8_t pre_arm_check : 1; // 4 // true if all pre-arm checks (rc, accel calibration, gps lock) have been performed
uint8_t auto_armed : 1; // 5 // stops auto missions from beginning until throttle is raised
uint8_t logging_started : 1; // 6 // true if logging has started
uint8_t land_complete : 1; // 7 // true if we have detected a landing
uint8_t new_radio_frame : 1; // 8 // Set true if we have new PWM data to act on from the Radio
uint8_t usb_connected_unused : 1; // 9 // UNUSED
uint8_t rc_receiver_present : 1; // 10 // true if we have an rc receiver present (i.e. if we've ever received an update
uint8_t compass_mot : 1; // 11 // true if we are currently performing compassmot calibration
uint8_t motor_test : 1; // 12 // true if we are currently performing the motors test
uint8_t initialised : 1; // 13 // true once the init_ardupilot function has completed. Extended status to GCS is not sent until this completes
uint8_t land_complete_maybe : 1; // 14 // true if we may have landed (less strict version of land_complete)
uint8_t throttle_zero : 1; // 15 // true if the throttle stick is at zero, debounced, determines if pilot intends shut-down when not using motor interlock
uint8_t system_time_set_unused : 1; // 16 // true if the system time has been set from the GPS
uint8_t gps_glitching : 1; // 17 // true if GPS glitching is affecting navigation accuracy
uint8_t using_interlock : 1; // 20 // aux switch motor interlock function is in use
uint8_t land_repo_active : 1; // 21 // true if the pilot is overriding the landing position
uint8_t motor_interlock_switch : 1; // 22 // true if pilot is requesting motor interlock enable
uint8_t in_arming_delay : 1; // 23 // true while we are armed but waiting to spin motors
uint8_t initialised_params : 1; // 24 // true when the all parameters have been initialised. we cannot send parameters to the GCS until this is done
uint8_t unused3 : 1; // 25 // was compass_init_location; true when the compass's initial location has been set
uint8_t unused2 : 1; // 26 // aux switch rc_override is allowed
uint8_t unused4 : 1; // 27 // was "we armed using a arming switch"
uint8_t pre_arm_rc_check : 1; // 1 // true if rc input pre-arm checks have been completed successfully
uint8_t pre_arm_check : 1; // 2 // true if all pre-arm checks (rc, accel calibration, gps lock) have been performed
uint8_t auto_armed : 1; // 3 // stops auto missions from beginning until throttle is raised
uint8_t logging_started : 1; // 4 // true if logging has started
uint8_t land_complete : 1; // 5 // true if we have detected a landing
uint8_t new_radio_frame : 1; // 6 // Set true if we have new PWM data to act on from the Radio
uint8_t rc_receiver_present : 1; // 7 // true if we have an rc receiver present (i.e. if we've ever received an update
uint8_t compass_mot : 1; // 8 // true if we are currently performing compassmot calibration
uint8_t motor_test : 1; // 9 // true if we are currently performing the motors test
uint8_t initialised : 1; // 10 // true once the init_ardupilot function has completed. Extended status to GCS is not sent until this completes
uint8_t land_complete_maybe : 1; // 11 // true if we may have landed (less strict version of land_complete)
uint8_t throttle_zero : 1; // 12 // true if the throttle stick is at zero, debounced, determines if pilot intends shut-down
uint8_t gps_glitching : 1; // 13 // true if GPS glitching is affecting navigation accuracy
uint8_t in_arming_delay : 1; // 14 // true while we are armed but waiting to spin motors
uint8_t initialised_params : 1; // 15 // true when the all parameters have been initialised. we cannot send parameters to the GCS until this is done
};
uint32_t value;
} ap_t;
ap_t ap; //MIR Set of general variables
ap_t ap;
static_assert(sizeof(uint32_t) == sizeof(ap), "ap_t must be uint32_t");
@ -240,7 +231,7 @@ private:
Location current_loc;
Vector3f vel_ned;
Vector3f vel_ned_filtd;
Vector3f pos_ned;
float vel_yaw;
float vel_yaw_filtd;
@ -250,14 +241,14 @@ private:
// 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};
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_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
@ -330,8 +321,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);
void rotate_NE_to_BF(Vector2f &vec);
void rotate_BF_to_NE(Vector2f &vec);
// commands.cpp
void update_home_from_EKF();

2
Blimp/Fins.h
View File

@ -96,6 +96,8 @@ 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,
//throttle will be displayed as more than 100.
return fmaxf(fmaxf(fabsf(down_out),fabsf(front_out)), fmaxf(fabsf(right_out),fabsf(yaw_out)));
}

6
Blimp/GCS_Mavlink.h
View File

@ -67,10 +67,10 @@ private:
//This is 1-indexed, unlike most enums for consistency with the mavlink PID_TUNING enums.
enum PID_SEND : uint8_t {
VELX = 1,
VELY = 2,
VELX = 1,
VELY = 2,
VELZ = 3,
VELYAW = 4,
VELYAW = 4,
POSX = 5,
POSY = 6,
POSZ = 7,

4
Blimp/Log.cpp
View File

@ -355,10 +355,10 @@ const struct LogStructure Blimp::log_structure[] = {
// @Field: R: Right
// @Field: F: Front
// @Field: D: Down
// @Field: Y: Yaw
// @Field: Y: Yaw
{ LOG_FINI_MSG, sizeof(log_FINI),
"FINI", "Qffff", "TimeUS,R,F,D,Y", "s----", "F----" },
"FINI", "Qffff", "TimeUS,R,F,D,Y", "s----", "F----" },
// @LoggerMessage: FINO
// @Description: Fin output

19
Blimp/Parameters.cpp
View File

@ -252,42 +252,42 @@ const AP_Param::Info Blimp::var_info[] = {
// @Param: MAX_VEL_XY
// @DisplayName: Max XY Velocity
// @Description: Sets the maximum XY velocity, in m/s
// @Values:
// @Range: 0.2 5
// @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:
// @Range: 0.2 5
// @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:
// @Range: 0.2 5
// @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:
// @Range: 0.1 5
// @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:
// @Range: 0.1 5
// @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:
// @Range: 0.1 5
// @User: Standard
GSCALAR(max_pos_yaw, "MAX_POS_YAW", 0.3),
@ -306,9 +306,6 @@ const AP_Param::Info Blimp::var_info[] = {
// @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
@ -756,7 +753,7 @@ const AP_Param::Info Blimp::var_info[] = {
// @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.
// @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

39
Blimp/Parameters.h
View File

@ -67,10 +67,10 @@ public:
k_param_parachute,
// Landing gear object
k_param_landinggear, // 18
k_param_landinggear,
// Input Management object
k_param_input_manager, // 19
k_param_input_manager,
// Misc
k_param_gps_hdop_good,
@ -84,19 +84,30 @@ public:
k_param_log_bitmask,
k_param_throttle_filt,
k_param_throttle_behavior,
k_param_pilot_takeoff_alt, // 64
k_param_pilot_takeoff_alt,
// AP_ADSB Library
k_param_adsb, // 72
k_param_notify, // 73
k_param_adsb,
k_param_notify,
//PID Controllers
k_param_pid_vel_xy,
k_param_pid_pos_xy,
k_param_pid_vel_xy = 32,
k_param_pid_vel_z,
k_param_pid_vel_yaw,
k_param_pid_pos_xy,
k_param_pid_pos_z,
k_param_pid_pos_yaw,
//Position & Velocity controller params
k_param_max_vel_xy = 50,
k_param_max_vel_z,
k_param_max_vel_yaw,
k_param_max_pos_xy,
k_param_max_pos_z,
k_param_max_pos_yaw,
k_param_simple_mode,
k_param_dis_mask,
//
// 90: misc2
//
@ -180,18 +191,8 @@ 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
@ -252,8 +253,6 @@ public:
AP_Int8 simple_mode;
AP_Int16 dis_mask;
AP_Float notch_bw;
AP_Float notch_att;
AP_Int8 rtl_alt_type;

6
Blimp/mode.h
View File

@ -16,7 +16,7 @@ public:
MANUAL = 0, // manual control
LAND = 1, // currently just stops moving
VELOCITY = 2, // velocity mode
LOITER = 3, // loiter mode (position hold)
LOITER = 3, // loiter mode (position hold)
};
// constructor
@ -47,10 +47,6 @@ public:
{
return false;
}
virtual bool logs_attitude() const
{
return false;
}
// return a string for this flightmode
virtual const char *name() const = 0;

40
Blimp/mode_loiter.cpp
View File

@ -14,18 +14,19 @@
//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();
Vector3f pilot;
pilot.x = channel_front->get_control_in() / float(RC_SCALE) * g.max_pos_xy * blimp.scheduler.get_loop_period_s();
pilot.y = channel_right->get_control_in() / float(RC_SCALE) * g.max_pos_xy * blimp.scheduler.get_loop_period_s();
pilot.z = 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);
blimp.rotate_BF_to_NE(pilot.xy());
}
target_pos.x += pilot_fwd;
target_pos.y += pilot_rgt;
target_pos.z += pilot_dwn;
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
@ -38,22 +39,25 @@ void ModeLoiter::run()
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);
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, {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);
blimp.rotate_NE_to_BF(actuator);
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;
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

21
Blimp/mode_velocity.cpp
View File

@ -9,23 +9,26 @@ 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);
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;
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);
blimp.rotate_NE_to_BF(actuator);
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;
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);