aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Blimp: Fix formatting to follow codestyle using script

Browse Source
This commit is contained in:
Michelle Rossouw 2023-08-21 21:19:19 +10:00 committed by Andrew Tridgell
parent 84b3c9e267
commit 362d4bf73b
7 changed files with 97 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Blimp/Blimp.cpp
View File

@ -53,7 +53,7 @@ const AP_Scheduler::Task Blimp::scheduler_tasks[] = {
FAST_TASK_CLASS(AP_InertialSensor, &blimp.ins, update),
// send outputs to the motors library immediately
FAST_TASK(motors_output),
// run EKF state estimator (expensive)
// run EKF state estimator (expensive)
FAST_TASK(read_AHRS),
// Inertial Nav
FAST_TASK(read_inertia),

2
Blimp/Fins.h
View File

@ -71,7 +71,7 @@ protected:
int8_t _num_added;
//MIR This should probably become private in future.
//MIR This should probably become private in future.
public:
float right_out; //input right movement, negative for left, +1 to -1
float front_out; //input front/forwards movement, negative for backwards, +1 to -1

4
Blimp/GCS_Mavlink.cpp
View File

@ -52,8 +52,8 @@ void GCS_MAVLINK_Blimp::send_position_target_global_int()
}
static constexpr uint16_t POSITION_TARGET_TYPEMASK_LAST_BYTE = 0xF000;
static constexpr uint16_t TYPE_MASK = POSITION_TARGET_TYPEMASK_VX_IGNORE | POSITION_TARGET_TYPEMASK_VY_IGNORE | POSITION_TARGET_TYPEMASK_VZ_IGNORE |
POSITION_TARGET_TYPEMASK_AX_IGNORE | POSITION_TARGET_TYPEMASK_AY_IGNORE | POSITION_TARGET_TYPEMASK_AZ_IGNORE |
POSITION_TARGET_TYPEMASK_YAW_IGNORE | POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE | POSITION_TARGET_TYPEMASK_LAST_BYTE;
POSITION_TARGET_TYPEMASK_AX_IGNORE | POSITION_TARGET_TYPEMASK_AY_IGNORE | POSITION_TARGET_TYPEMASK_AZ_IGNORE |
POSITION_TARGET_TYPEMASK_YAW_IGNORE | POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE | POSITION_TARGET_TYPEMASK_LAST_BYTE;
mavlink_msg_position_target_global_int_send(
chan,

46
Blimp/Log.cpp
View File

@ -30,7 +30,7 @@ struct PACKED log_FINO {
//Write a fin input packet
void Blimp::Write_FINI(float right, float front, float down, float yaw)
{
const struct log_FINI pkt{
const struct log_FINI pkt {
LOG_PACKET_HEADER_INIT(LOG_FINI_MSG),
time_us : AP_HAL::micros64(),
Right : right,
@ -44,7 +44,7 @@ void Blimp::Write_FINI(float right, float front, float down, float yaw)
//Write a fin output packet
void Blimp::Write_FINO(float *amp, float *off)
{
const struct log_FINO pkt{
const struct log_FINO pkt {
LOG_PACKET_HEADER_INIT(LOG_FINO_MSG),
time_us : AP_HAL::micros64(),
Fin1_Amp : amp[0],
@ -248,8 +248,10 @@ const struct LogStructure Blimp::log_structure[] = {
// @Field: D: Down
// @Field: Y: Yaw
{ LOG_FINI_MSG, sizeof(log_FINI),
"FINI", "Qffff", "TimeUS,R,F,D,Y", "s----", "F----" },
{
LOG_FINI_MSG, sizeof(log_FINI),
"FINI", "Qffff", "TimeUS,R,F,D,Y", "s----", "F----"
},
// @LoggerMessage: FINO
// @Description: Fin output
@ -263,8 +265,10 @@ const struct LogStructure Blimp::log_structure[] = {
// @Field: F4A: Fin 4 Amplitude
// @Field: F4O: Fin 4 Offset
{ LOG_FINO_MSG, sizeof(log_FINO),
"FINO", "Qffffffff", "TimeUS,F1A,F1O,F2A,F2O,F3A,F3O,F4A,F4O", "s--------", "F--------" },
{
LOG_FINO_MSG, sizeof(log_FINO),
"FINO", "Qffffffff", "TimeUS,F1A,F1O,F2A,F2O,F3A,F3O,F4A,F4O", "s--------", "F--------"
},
// @LoggerMessage: PIDD,PIVN,PIVE,PIVD,PIVY
// @Description: Proportional/Integral/Derivative gain values
@ -279,16 +283,26 @@ const struct LogStructure Blimp::log_structure[] = {
// @Field: Dmod: scaler applied to D gain to reduce limit cycling
// @Field: SRate: slew rate
// @Field: Limit: 1 if I term is limited due to output saturation
{ LOG_PIDD_MSG, sizeof(log_PID),
"PIDD", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS },
{ LOG_PIVN_MSG, sizeof(log_PID),
"PIVN", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS },
{ LOG_PIVE_MSG, sizeof(log_PID),
"PIVE", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS },
{ LOG_PIVD_MSG, sizeof(log_PID),
"PIVD", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS },
{ LOG_PIVY_MSG, sizeof(log_PID),
"PIVY", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS },
{
LOG_PIDD_MSG, sizeof(log_PID),
"PIDD", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS
},
{
LOG_PIVN_MSG, sizeof(log_PID),
"PIVN", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS
},
{
LOG_PIVE_MSG, sizeof(log_PID),
"PIVE", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS
},
{
LOG_PIVD_MSG, sizeof(log_PID),
"PIVD", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS
},
{
LOG_PIVY_MSG, sizeof(log_PID),
"PIVY", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS
},
// @LoggerMessage: PTUN
// @Description: Parameter Tuning information

68
Blimp/Loiter.cpp
View File

@ -9,14 +9,20 @@ void Loiter::run(Vector3f& target_pos, float& target_yaw, Vector4b axes_disabled
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;
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;
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",
@ -29,17 +35,27 @@ void Loiter::run(Vector3f& target_pos, float& target_yaw, Vector4b axes_disabled
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;
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);
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) {
@ -59,10 +75,14 @@ void Loiter::run(Vector3f& target_pos, float& target_yaw, Vector4b axes_disabled
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);
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 (!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));
@ -110,10 +130,18 @@ void Loiter::run_vel(Vector3f& target_vel_ef, float& target_vel_yaw, Vector4b ax
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;
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;
@ -128,10 +156,14 @@ void Loiter::run_vel(Vector3f& target_vel_ef, float& target_vel_yaw, Vector4b ax
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);
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 (!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));

2
Blimp/mode.cpp
View File

@ -140,7 +140,7 @@ void Blimp::update_flight_mode()
// exit_mode - high level call to organise cleanup as a flight mode is exited
void Blimp::exit_mode(Mode *&old_flightmode,
Mode *&new_flightmode){}
Mode *&new_flightmode) {}
// notify_flight_mode - sets notify object based on current flight mode. Only used for OreoLED notify device
void Blimp::notify_flight_mode()

16
Blimp/mode_loiter.cpp
View File

@ -32,10 +32,18 @@ void ModeLoiter::run()
blimp.rotate_BF_to_NE(pilot.xy());
}
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);
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);
}
blimp.loiter->run(target_pos, target_yaw, Vector4b{false,false,false,false});
}