Skip to content

Commit

Permalink
Fix ZigZag
Browse files Browse the repository at this point in the history
  • Loading branch information
@lthall
lthall committed Sep 28, 2024
1 parent dfec4e4 commit 533a25b
Show file tree
Hide file tree
Showing 7 changed files with 47 additions and 21 deletions.
3 changes: 3 additions & 0 deletions ArduCopter/Copter.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -266,6 +266,9 @@ class Copter : public AP_Vehicle {
// measured ground or ceiling level measured using the range finder.
void update_surface_offset();

// initalised already by terrain following so don't initalise again if tracking the ground
void external_init();

// get target and actual distances (in m) for logging purposes
bool get_target_dist_for_logging(float &target_dist) const;
float get_dist_for_logging() const;
Expand Down
9 changes: 9 additions & 0 deletions ArduCopter/mode_zigzag.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -244,6 +244,11 @@ void ModeZigZag::return_to_manual_control(bool maintain_target)
if (maintain_target) {
const Vector3f& wp_dest = wp_nav->get_wp_destination();
loiter_nav->init_target(wp_dest.xy());
#if AP_RANGEFINDER_ENABLED
if (copter.rangefinder_alt_ok() && wp_nav->rangefinder_used_and_healthy()) {
copter.surface_tracking.external_init();
}
#endif
} else {
loiter_nav->init_target();
}
Expand Down Expand Up @@ -450,8 +455,12 @@ bool ModeZigZag::calculate_next_dest(Destination ab_dest, bool use_wpnav_alt, Ve
terrain_alt = wp_nav->origin_and_destination_are_terrain_alt();
next_dest.z = wp_nav->get_wp_destination().z;
} else {
// terrain_alt = copter.rangefinder_alt_ok() && wp_nav->rangefinder_used_and_healthy();
terrain_alt = copter.rangefinder_alt_ok() && wp_nav->rangefinder_used_and_healthy();
next_dest.z = pos_control->get_pos_desired_z_cm();
if (!terrain_alt) {
next_dest.z += pos_control->get_pos_terrain_cm();
}
}

return true;
Expand Down
11 changes: 10 additions & 1 deletion ArduCopter/surface_tracking.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,13 +36,22 @@ void Copter::SurfaceTracking::update_surface_offset()
} else {
// reset position controller offsets if surface tracking is inactive
// flag target should be reset when/if it next becomes active
if (timeout) {
if (timeout && !reset_target) {
copter.pos_control->init_pos_terrain_cm(0);
valid_for_logging = false;
reset_target = true;
}
}
}

void Copter::SurfaceTracking::external_init()
{
if ((surface == Surface::GROUND) && copter.rangefinder_alt_ok() && (copter.rangefinder_state.glitch_count == 0)) {
last_update_ms = millis();
reset_target = false;
}
}

bool Copter::SurfaceTracking::get_target_dist_for_logging(float &target_dist) const
{
if (!valid_for_logging || (surface == Surface::NONE)) {
Expand Down
6 changes: 3 additions & 3 deletions libraries/AC_AttitudeControl/AC_PosControl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1338,10 +1338,10 @@ void AC_PosControl::write_log()
if (is_active_xy()) {
float accel_x, accel_y;
lean_angles_to_accel_xy(accel_x, accel_y);
Write_PSCN(_pos_target.x, _inav.get_position_neu_cm().x,
Write_PSCN(_pos_desired.x, _pos_target.x, _inav.get_position_neu_cm().x,
_vel_desired.x, _vel_target.x, _inav.get_velocity_neu_cms().x,
_accel_desired.x, _accel_target.x, accel_x);
Write_PSCE(_pos_target.y, _inav.get_position_neu_cm().y,
Write_PSCE(_pos_desired.y, _pos_target.y, _inav.get_position_neu_cm().y,
_vel_desired.y, _vel_target.y, _inav.get_velocity_neu_cms().y,
_accel_desired.y, _accel_target.y, accel_y);

Expand All @@ -1352,7 +1352,7 @@ void AC_PosControl::write_log()
}

if (is_active_z()) {
Write_PSCD(-_pos_target.z, -_inav.get_position_z_up_cm(),
Write_PSCD(-_pos_desired.z, -_pos_target.z, -_inav.get_position_z_up_cm(),
-_vel_desired.z, -_vel_target.z, -_inav.get_velocity_z_up_cms(),
-_accel_desired.z, -_accel_target.z, -get_z_accel_cmss());

Expand Down
8 changes: 4 additions & 4 deletions libraries/AC_AttitudeControl/AC_PosControl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -444,9 +444,9 @@ class AC_PosControl

static const struct AP_Param::GroupInfo var_info[];

static void Write_PSCN(float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel);
static void Write_PSCE(float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel);
static void Write_PSCD(float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel);
static void Write_PSCN(float pos_desired, float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel);
static void Write_PSCE(float pos_desired, float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel);
static void Write_PSCD(float pos_desired, float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel);
static void Write_PSCO(const Vector2p& pos_target_offset_ne, const Vector2p& pos_offset_ne,
const Vector2f& vel_target_offset_ne, const Vector2f& vel_offset_ne,
const Vector2f& accel_target_offset_ne, const Vector2f& accel_offset_ne);
Expand Down Expand Up @@ -583,7 +583,7 @@ class AC_PosControl
private:
// convenience method for writing out the identical PSCE, PSCN, PSCD - and
// to save bytes
static void Write_PSCx(LogMessages ID, float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel);
static void Write_PSCx(LogMessages ID, float pos_desired, float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel);

// singleton
static AC_PosControl *_singleton;
Expand Down
15 changes: 8 additions & 7 deletions libraries/AC_AttitudeControl/AC_PosControl_Logging.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,11 +8,12 @@
#include "LogStructure.h"

// a convenience function for writing out the position controller PIDs
void AC_PosControl::Write_PSCx(LogMessages id, float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel)
void AC_PosControl::Write_PSCx(LogMessages id, float pos_desired, float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel)
{
const struct log_PSCx pkt{
LOG_PACKET_HEADER_INIT(id),
time_us : AP_HAL::micros64(),
pos_desired : pos_desired * 0.01f,
pos_target : pos_target * 0.01f,
pos : pos * 0.01f,
vel_desired : vel_desired * 0.01f,
Expand All @@ -25,19 +26,19 @@ void AC_PosControl::Write_PSCx(LogMessages id, float pos_target, float pos, floa
AP::logger().WriteBlock(&pkt, sizeof(pkt));
}

void AC_PosControl::Write_PSCN(float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel)
void AC_PosControl::Write_PSCN(float pos_desired, float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel)
{
Write_PSCx(LOG_PSCN_MSG, pos_target, pos, vel_desired, vel_target, vel, accel_desired, accel_target, accel);
Write_PSCx(LOG_PSCN_MSG, pos_desired, pos_target, pos, vel_desired, vel_target, vel, accel_desired, accel_target, accel);
}

void AC_PosControl::Write_PSCE(float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel)
void AC_PosControl::Write_PSCE(float pos_desired, float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel)
{
Write_PSCx(LOG_PSCE_MSG, pos_target, pos, vel_desired, vel_target, vel, accel_desired, accel_target, accel);
Write_PSCx(LOG_PSCE_MSG, pos_desired, pos_target, pos, vel_desired, vel_target, vel, accel_desired, accel_target, accel);
}

void AC_PosControl::Write_PSCD(float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel)
void AC_PosControl::Write_PSCD(float pos_desired, float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel)
{
Write_PSCx(LOG_PSCD_MSG, pos_target, pos, vel_desired, vel_target, vel, accel_desired, accel_target, accel);
Write_PSCx(LOG_PSCD_MSG, pos_desired, pos_target, pos, vel_desired, vel_target, vel, accel_desired, accel_target, accel);
}

// a convenience function for writing out the position controller PIDs
Expand Down
16 changes: 10 additions & 6 deletions libraries/AC_AttitudeControl/LogStructure.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@
// @LoggerMessage: PSCN
// @Description: Position Control North
// @Field: TimeUS: Time since system startup
// @Field: DPN: Desired position relative to EKF origin
// @Field: TPN: Target position relative to EKF origin
// @Field: PN: Position relative to EKF origin
// @Field: DVN: Desired velocity North
Expand All @@ -24,6 +25,7 @@
// @LoggerMessage: PSCE
// @Description: Position Control East
// @Field: TimeUS: Time since system startup
// @Field: DPE: Desired position relative to EKF origin + Offsets
// @Field: TPE: Target position relative to EKF origin
// @Field: PE: Position relative to EKF origin
// @Field: DVE: Desired velocity East
Expand All @@ -36,6 +38,7 @@
// @LoggerMessage: PSCD
// @Description: Position Control Down
// @Field: TimeUS: Time since system startup
// @Field: DPD: Desired position relative to EKF origin + Offsets
// @Field: TPD: Target position relative to EKF origin
// @Field: PD: Position relative to EKF origin
// @Field: DVD: Desired velocity Down
Expand All @@ -50,6 +53,7 @@
struct PACKED log_PSCx {
LOG_PACKET_HEADER;
uint64_t time_us;
float pos_desired;
float pos_target;
float pos;
float vel_desired;
Expand Down Expand Up @@ -148,17 +152,17 @@ struct PACKED log_ANG {
float sensor_dt;
};

#define PSCx_FMT "Qffffffff"
#define PSCx_UNITS "smmnnnooo"
#define PSCx_MULTS "F00000000"
#define PSCx_FMT "Qfffffffff"
#define PSCx_UNITS "smmmnnnooo"
#define PSCx_MULTS "F000000000"

#define LOG_STRUCTURE_FROM_AC_ATTITUDECONTROL \
{ LOG_PSCN_MSG, sizeof(log_PSCx), \
"PSCN", PSCx_FMT, "TimeUS,TPN,PN,DVN,TVN,VN,DAN,TAN,AN", PSCx_UNITS, PSCx_MULTS }, \
"PSCN", PSCx_FMT, "TimeUS,DPN,TPN,PN,DVN,TVN,VN,DAN,TAN,AN", PSCx_UNITS, PSCx_MULTS }, \
{ LOG_PSCE_MSG, sizeof(log_PSCx), \
"PSCE", PSCx_FMT, "TimeUS,TPE,PE,DVE,TVE,VE,DAE,TAE,AE", PSCx_UNITS, PSCx_MULTS }, \
"PSCE", PSCx_FMT, "TimeUS,DPN,TPE,PE,DVE,TVE,VE,DAE,TAE,AE", PSCx_UNITS, PSCx_MULTS }, \
{ LOG_PSCD_MSG, sizeof(log_PSCx), \
"PSCD", PSCx_FMT, "TimeUS,TPD,PD,DVD,TVD,VD,DAD,TAD,AD", PSCx_UNITS, PSCx_MULTS }, \
"PSCD", PSCx_FMT, "TimeUS,DPN,TPD,PD,DVD,TVD,VD,DAD,TAD,AD", PSCx_UNITS, PSCx_MULTS }, \
{ LOG_PSCO_MSG, sizeof(log_PSCO), \
"PSCO", "Qffffffffffff", "TimeUS,TPON,PON,TVON,VON,TAON,AON,TPOE,POE,TVOE,VOE,TAOE,AOE", "smmnnoommnnoo", "F000000000000" }, \
{ LOG_RATE_MSG, sizeof(log_Rate), \
Expand Down

0 comments on commit 533a25b

Please sign in to comment.