From fcbd88558c307ad51dee86df9574edf96d3671a0 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 28 Feb 2024 23:18:11 +1100 Subject: [PATCH] Blimp: adjust for logging having moved to AC_PosControl --- Blimp/Loiter.cpp | 14 ++++++++------ Blimp/wscript | 1 + 2 files changed, 9 insertions(+), 6 deletions(-) diff --git a/Blimp/Loiter.cpp b/Blimp/Loiter.cpp index 52e85f1b53..2553e6d3e7 100644 --- a/Blimp/Loiter.cpp +++ b/Blimp/Loiter.cpp @@ -1,5 +1,7 @@ #include "Blimp.h" +#include + #define MA 0.99 #define MO (1-MA) @@ -123,9 +125,9 @@ void Loiter::run(Vector3f& target_pos, float& target_yaw, Vector4b axes_disabled } #if HAL_LOGGING_ENABLED - AP::logger().Write_PSCN(target_pos.x * 100.0, blimp.pos_ned.x * 100.0, 0.0, target_vel_ef_c.x * 100.0, blimp.vel_ned_filtd.x * 100.0, 0.0, 0.0, 0.0); - AP::logger().Write_PSCE(target_pos.y * 100.0, blimp.pos_ned.y * 100.0, 0.0, target_vel_ef_c.y * 100.0, blimp.vel_ned_filtd.y * 100.0, 0.0, 0.0, 0.0); - AP::logger().Write_PSCD(-target_pos.z * 100.0, -blimp.pos_ned.z * 100.0, 0.0, -target_vel_ef_c.z * 100.0, -blimp.vel_ned_filtd.z * 100.0, 0.0, 0.0, 0.0); + AC_PosControl::Write_PSCN(target_pos.x * 100.0, blimp.pos_ned.x * 100.0, 0.0, target_vel_ef_c.x * 100.0, blimp.vel_ned_filtd.x * 100.0, 0.0, 0.0, 0.0); + AC_PosControl::Write_PSCE(target_pos.y * 100.0, blimp.pos_ned.y * 100.0, 0.0, target_vel_ef_c.y * 100.0, blimp.vel_ned_filtd.y * 100.0, 0.0, 0.0, 0.0); + AC_PosControl::Write_PSCD(-target_pos.z * 100.0, -blimp.pos_ned.z * 100.0, 0.0, -target_vel_ef_c.z * 100.0, -blimp.vel_ned_filtd.z * 100.0, 0.0, 0.0, 0.0); #endif } @@ -201,8 +203,8 @@ void Loiter::run_vel(Vector3f& target_vel_ef, float& target_vel_yaw, Vector4b ax } #if HAL_LOGGING_ENABLED - AP::logger().Write_PSCN(0.0, blimp.pos_ned.x * 100.0, 0.0, target_vel_ef_c.x * 100.0, blimp.vel_ned_filtd.x * 100.0, 0.0, 0.0, 0.0); - AP::logger().Write_PSCE(0.0, blimp.pos_ned.y * 100.0, 0.0, target_vel_ef_c.y * 100.0, blimp.vel_ned_filtd.y * 100.0, 0.0, 0.0, 0.0); - AP::logger().Write_PSCD(0.0, -blimp.pos_ned.z * 100.0, 0.0, -target_vel_ef_c.z * 100.0, -blimp.vel_ned_filtd.z * 100.0, 0.0, 0.0, 0.0); + AC_PosControl::Write_PSCN(0.0, blimp.pos_ned.x * 100.0, 0.0, target_vel_ef_c.x * 100.0, blimp.vel_ned_filtd.x * 100.0, 0.0, 0.0, 0.0); + AC_PosControl::Write_PSCE(0.0, blimp.pos_ned.y * 100.0, 0.0, target_vel_ef_c.y * 100.0, blimp.vel_ned_filtd.y * 100.0, 0.0, 0.0, 0.0); + AC_PosControl::Write_PSCD(0.0, -blimp.pos_ned.z * 100.0, 0.0, -target_vel_ef_c.z * 100.0, -blimp.vel_ned_filtd.z * 100.0, 0.0, 0.0, 0.0); #endif } diff --git a/Blimp/wscript b/Blimp/wscript index 34cb8c1704..1427a85552 100644 --- a/Blimp/wscript +++ b/Blimp/wscript @@ -18,6 +18,7 @@ def build(bld): 'AP_KDECAN', 'AP_Beacon', 'AP_AdvancedFailsafe', # TODO for some reason compiling GCS_Common.cpp (in libraries) fails if this isn't included. + 'AC_AttitudeControl', # for PSCx logging ], )