zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Blimp: Code cleanups

gps-1.3.1
Michelle Rossouw 4 years ago committed by Andrew Tridgell
parent
5bd3e3e330
commit
0e2cb4a00c
11 changed files with 114 additions and 121 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 19
      Blimp/AP_Arming.cpp
  2. 24
      Blimp/Blimp.cpp
  3. 45
      Blimp/Blimp.h
  4. 2
      Blimp/Fins.h
  5. 15
      Blimp/Parameters.cpp
  6. 37
      Blimp/Parameters.h
  7. 4
      Blimp/mode.h
  8. 36
      Blimp/mode_loiter.cpp
  9. 21
      Blimp/mode_velocity.cpp

19
Blimp/AP_Arming.cpp
Unescape View File

@ -40,14 +40,6 @@ bool AP_Arming_Blimp::run_pre_arm_checks(bool display_failure)
return false; 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 pre arm checks are disabled run only the mandatory checks
if (checks_to_perform == 0) { if (checks_to_perform == 0) {
return mandatory_checks(display_failure); return mandatory_checks(display_failure);
@ -273,6 +265,15 @@ bool AP_Arming_Blimp::mandatory_gps_checks(bool display_failure)
return true; 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 // if we got here all must be ok
return true; 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; 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); ahrs.set_correct_centrifugal(true);
hal.util->set_soft_armed(true); hal.util->set_soft_armed(true);

24
Blimp/Blimp.cpp
Unescape View File

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

45
Blimp/Blimp.h
Unescape View File

@ -150,35 +150,26 @@ private:
// Documentation of Globals: // Documentation of Globals:
typedef union { typedef union {
struct { struct {
uint8_t unused1 : 1; // 0 uint8_t pre_arm_rc_check : 1; // 1 // true if rc input pre-arm checks have been completed successfully
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; // 2 // true if all pre-arm checks (rc, accel calibration, gps lock) have been performed
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; // 3 // stops auto missions from beginning until throttle is raised
uint8_t auto_armed : 1; // 5 // stops auto missions from beginning until throttle is raised uint8_t logging_started : 1; // 4 // true if logging has started
uint8_t logging_started : 1; // 6 // true if logging has started uint8_t land_complete : 1; // 5 // true if we have detected a landing
uint8_t land_complete : 1; // 7 // 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 new_radio_frame : 1; // 8 // 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 usb_connected_unused : 1; // 9 // UNUSED uint8_t compass_mot : 1; // 8 // true if we are currently performing compassmot calibration
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 motor_test : 1; // 9 // true if we are currently performing the motors test
uint8_t compass_mot : 1; // 11 // true if we are currently performing compassmot calibration 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 motor_test : 1; // 12 // true if we are currently performing the motors test uint8_t land_complete_maybe : 1; // 11 // true if we may have landed (less strict version of land_complete)
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 throttle_zero : 1; // 12 // true if the throttle stick is at zero, debounced, determines if pilot intends shut-down
uint8_t land_complete_maybe : 1; // 14 // true if we may have landed (less strict version of land_complete) uint8_t gps_glitching : 1; // 13 // true if GPS glitching is affecting navigation accuracy
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 in_arming_delay : 1; // 14 // true while we are armed but waiting to spin motors
uint8_t system_time_set_unused : 1; // 16 // true if the system time has been set from the GPS 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
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"
}; };
uint32_t value; uint32_t value;
} ap_t; } 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"); static_assert(sizeof(uint32_t) == sizeof(ap), "ap_t must be uint32_t");
@ -330,8 +321,8 @@ private:
void one_hz_loop(); void one_hz_loop();
void read_AHRS(void); void read_AHRS(void);
void update_altitude(); void update_altitude();
void rotate_NE_to_BF(float &x, float &y); void rotate_NE_to_BF(Vector2f &vec);
void rotate_BF_to_NE(float &x, float &y); void rotate_BF_to_NE(Vector2f &vec);
// commands.cpp // commands.cpp
void update_home_from_EKF(); void update_home_from_EKF();

2
Blimp/Fins.h
Unescape View File

@ -96,6 +96,8 @@ public:
float get_throttle() float get_throttle()
{ {
//Only for Mavlink - essentially just an indicator of how hard the fins are working. //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))); return fmaxf(fmaxf(fabsf(down_out),fabsf(front_out)), fmaxf(fabsf(right_out),fabsf(yaw_out)));
} }

15
Blimp/Parameters.cpp
Unescape View File

@ -252,42 +252,42 @@ const AP_Param::Info Blimp::var_info[] = {
// @Param: MAX_VEL_XY // @Param: MAX_VEL_XY
// @DisplayName: Max XY Velocity // @DisplayName: Max XY Velocity
// @Description: Sets the maximum XY velocity, in m/s // @Description: Sets the maximum XY velocity, in m/s
// @Values: // @Range: 0.2 5
// @User: Standard // @User: Standard
GSCALAR(max_vel_xy, "MAX_VEL_XY", 0.5), GSCALAR(max_vel_xy, "MAX_VEL_XY", 0.5),
// @Param: MAX_VEL_Z // @Param: MAX_VEL_Z
// @DisplayName: Max Z Velocity // @DisplayName: Max Z Velocity
// @Description: Sets the maximum Z velocity, in m/s // @Description: Sets the maximum Z velocity, in m/s
// @Values: // @Range: 0.2 5
// @User: Standard // @User: Standard
GSCALAR(max_vel_z, "MAX_VEL_Z", 0.4), GSCALAR(max_vel_z, "MAX_VEL_Z", 0.4),
// @Param: MAX_VEL_YAW // @Param: MAX_VEL_YAW
// @DisplayName: Max yaw Velocity // @DisplayName: Max yaw Velocity
// @Description: Sets the maximum yaw velocity, in rad/s // @Description: Sets the maximum yaw velocity, in rad/s
// @Values: // @Range: 0.2 5
// @User: Standard // @User: Standard
GSCALAR(max_vel_yaw, "MAX_VEL_YAW", 0.5), GSCALAR(max_vel_yaw, "MAX_VEL_YAW", 0.5),
// @Param: MAX_POS_XY // @Param: MAX_POS_XY
// @DisplayName: Max XY Position change // @DisplayName: Max XY Position change
// @Description: Sets the maximum XY position change, in m/s // @Description: Sets the maximum XY position change, in m/s
// @Values: // @Range: 0.1 5
// @User: Standard // @User: Standard
GSCALAR(max_pos_xy, "MAX_POS_XY", 0.2), GSCALAR(max_pos_xy, "MAX_POS_XY", 0.2),
// @Param: MAX_POS_Z // @Param: MAX_POS_Z
// @DisplayName: Max Z Position change // @DisplayName: Max Z Position change
// @Description: Sets the maximum Z position change, in m/s // @Description: Sets the maximum Z position change, in m/s
// @Values: // @Range: 0.1 5
// @User: Standard // @User: Standard
GSCALAR(max_pos_z, "MAX_POS_Z", 0.15), GSCALAR(max_pos_z, "MAX_POS_Z", 0.15),
// @Param: MAX_POS_YAW // @Param: MAX_POS_YAW
// @DisplayName: Max Yaw Position change // @DisplayName: Max Yaw Position change
// @Description: Sets the maximum Yaw position change, in rad/s // @Description: Sets the maximum Yaw position change, in rad/s
// @Values: // @Range: 0.1 5
// @User: Standard // @User: Standard
GSCALAR(max_pos_yaw, "MAX_POS_YAW", 0.3), GSCALAR(max_pos_yaw, "MAX_POS_YAW", 0.3),
@ -306,9 +306,6 @@ const AP_Param::Info Blimp::var_info[] = {
// @User: Standard // @User: Standard
GSCALAR(dis_mask, "DIS_MASK", 0), GSCALAR(dis_mask, "DIS_MASK", 0),
GSCALAR(notch_bw, "NOTCH_BW", 2),
GSCALAR(notch_att, "NOTCH_ATT", 15),
// @Param: RC_SPEED // @Param: RC_SPEED
// @DisplayName: ESC Update Speed // @DisplayName: ESC Update Speed
// @Description: This is the speed in Hertz that your ESCs will receive updates // @Description: This is the speed in Hertz that your ESCs will receive updates

37
Blimp/Parameters.h
Unescape View File

@ -67,10 +67,10 @@ public:
k_param_parachute, k_param_parachute,
// Landing gear object // Landing gear object
k_param_landinggear, // 18 k_param_landinggear,
// Input Management object // Input Management object
k_param_input_manager, // 19 k_param_input_manager,
// Misc // Misc
k_param_gps_hdop_good, k_param_gps_hdop_good,
@ -84,19 +84,30 @@ public:
k_param_log_bitmask, k_param_log_bitmask,
k_param_throttle_filt, k_param_throttle_filt,
k_param_throttle_behavior, k_param_throttle_behavior,
k_param_pilot_takeoff_alt, // 64 k_param_pilot_takeoff_alt,
// AP_ADSB Library // AP_ADSB Library
k_param_adsb, // 72 k_param_adsb,
k_param_notify, // 73 k_param_notify,
//PID Controllers //PID Controllers
k_param_pid_vel_xy, k_param_pid_vel_xy = 32,
k_param_pid_pos_xy,
k_param_pid_vel_z, k_param_pid_vel_z,
k_param_pid_vel_yaw, k_param_pid_vel_yaw,
k_param_pid_pos_xy,
k_param_pid_pos_z, k_param_pid_pos_z,
k_param_pid_pos_yaw, 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 // 90: misc2
// //
@ -180,17 +191,7 @@ public:
// 220: Misc // 220: Misc
// //
k_param_fs_ekf_action = 220, k_param_fs_ekf_action = 220,
k_param_max_vel_xy,
k_param_arming, 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_logger = 253, // 253 - Logging Group
@ -252,8 +253,6 @@ public:
AP_Int8 simple_mode; AP_Int8 simple_mode;
AP_Int16 dis_mask; AP_Int16 dis_mask;
AP_Float notch_bw;
AP_Float notch_att;
AP_Int8 rtl_alt_type; AP_Int8 rtl_alt_type;

4
Blimp/mode.h
Unescape View File

@ -47,10 +47,6 @@ public:
{ {
return false; return false;
} }
virtual bool logs_attitude() const
{
return false;
}
// return a string for this flightmode // return a string for this flightmode
virtual const char *name() const = 0; virtual const char *name() const = 0;

36
Blimp/mode_loiter.cpp
Unescape View File

@ -14,18 +14,19 @@
//Runs the main loiter controller //Runs the main loiter controller
void ModeLoiter::run() void ModeLoiter::run()
{ {
float pilot_fwd = channel_front->get_control_in() / float(RC_SCALE) * g.max_pos_xy * blimp.scheduler.get_loop_period_s(); Vector3f pilot;
float pilot_rgt = channel_right->get_control_in() / float(RC_SCALE) * g.max_pos_xy * blimp.scheduler.get_loop_period_s(); pilot.x = channel_front->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(); 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(); 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 (g.simple_mode == 0){
//If simple mode is disabled, input is in body-frame, thus needs to be rotated. //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.x += pilot.x;
target_pos.y += pilot_rgt; target_pos.y += pilot.y;
target_pos.z += pilot_dwn; target_pos.z += pilot.z;
target_yaw = wrap_PI(target_yaw + pilot_yaw); target_yaw = wrap_PI(target_yaw + pilot_yaw);
//Pos controller's output becomes target for velocity controller //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), 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.y, -g.max_vel_xy, g.max_vel_xy),
constrain_float(target_vel_ef.z, -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); 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}); 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); 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); 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)))){ if(!(blimp.g.dis_mask & (1<<(2-1)))){
motors->front_out = actuator.x; motors->front_out = actuator.x;
} if(!(blimp.g.dis_mask & (1<<(1-1)))){ }
motors->right_out = actuator.y; if(!(blimp.g.dis_mask & (1<<(1-1)))){
} if(!(blimp.g.dis_mask & (1<<(3-1)))){ motors->right_out = actuator.y;
motors->down_out = act_down; }
} if(!(blimp.g.dis_mask & (1<<(4-1)))){ if(!(blimp.g.dis_mask & (1<<(3-1)))){
motors->yaw_out = act_yaw; 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 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
Unescape View File

@ -9,23 +9,26 @@ void ModeVelocity::run()
Vector3f target_vel; Vector3f target_vel;
target_vel.x = channel_front->get_control_in() / float(RC_SCALE) * g.max_vel_xy; 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; 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; 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 = 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}); 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_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); float act_yaw = blimp.pid_vel_yaw.update_all(target_vel_yaw, blimp.vel_yaw_filtd);
if(!(blimp.g.dis_mask & (1<<(2-1)))){ if(!(blimp.g.dis_mask & (1<<(2-1)))){
motors->front_out = actuator.x; motors->front_out = actuator.x;
} if(!(blimp.g.dis_mask & (1<<(1-1)))){ }
motors->right_out = actuator.y; if(!(blimp.g.dis_mask & (1<<(1-1)))){
} if(!(blimp.g.dis_mask & (1<<(3-1)))){ motors->right_out = actuator.y;
motors->down_out = act_down; }
} if(!(blimp.g.dis_mask & (1<<(4-1)))){ if(!(blimp.g.dis_mask & (1<<(3-1)))){
motors->yaw_out = act_yaw; 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); 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);

Write Preview
Loading…
Cancel
Save