From 560d6a9d4b46998a769bbfdfa6532d2b4f37923b Mon Sep 17 00:00:00 2001
From: PerFrivik <per.a.frivik@gmail.com>
Date: Tue, 30 Jan 2024 14:48:53 +0100
Subject: [PATCH] cleanup + updated acro

---
 src/modules/differential_drive/DifferentialDrive.cpp     | 9 ++++++++-
 src/modules/differential_drive/DifferentialDrive.hpp     | 2 ++
 .../DifferentialDriveGuidance.hpp                        | 2 --
 3 files changed, 10 insertions(+), 3 deletions(-)

diff --git a/src/modules/differential_drive/DifferentialDrive.cpp b/src/modules/differential_drive/DifferentialDrive.cpp
index f020fd1827..c37b3ad0c1 100644
--- a/src/modules/differential_drive/DifferentialDrive.cpp
+++ b/src/modules/differential_drive/DifferentialDrive.cpp
@@ -84,11 +84,18 @@ void DifferentialDrive::Run()
 		if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) {
 			_differential_drive_kinematics.setArmed(vehicle_control_mode.flag_armed);
 			_manual_driving = vehicle_control_mode.flag_control_manual_enabled;
-			_acro_driving = vehicle_control_mode.flag_control_rates_enabled;
 			_mission_driving = vehicle_control_mode.flag_control_auto_enabled;
 		}
 	}
 
+	if (_vehicle_status_sub.updated()) {
+		vehicle_status_s vehicle_status{};
+
+		if (_vehicle_status_sub.copy(&vehicle_status)) {
+			_acro_driving = (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ACRO);
+		}
+	}
+
 	if (_manual_driving) {
 		// Manual mode
 		// directly produce setpoints from the manual control setpoint (joystick)
diff --git a/src/modules/differential_drive/DifferentialDrive.hpp b/src/modules/differential_drive/DifferentialDrive.hpp
index 8e1d0936cd..98fd0b596a 100644
--- a/src/modules/differential_drive/DifferentialDrive.hpp
+++ b/src/modules/differential_drive/DifferentialDrive.hpp
@@ -44,6 +44,7 @@
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/vehicle_status.h>
 
 #include "DifferentialDriveControl/DifferentialDriveControl.hpp"
 #include "DifferentialDriveGuidance/DifferentialDriveGuidance.hpp"
@@ -78,6 +79,7 @@ private:
 	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
 	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
 
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
 	uORB::Publication<differential_drive_setpoint_s> _differential_drive_setpoint_pub{ORB_ID(differential_drive_setpoint)};
 
 	bool _manual_driving = false;
diff --git a/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.hpp b/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.hpp
index 75ac5cf038..b336c74c14 100644
--- a/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.hpp
+++ b/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.hpp
@@ -40,8 +40,6 @@
 #include <mathlib/mathlib.h>
 #include <lib/geo/geo.h>
 #include <math.h>
-#include <chrono>
-// #include <matrix/matrix/Matrix.hpp>
 
 #include <lib/motion_planning/PositionSmoothing.hpp>
 #include <lib/motion_planning/VelocitySmoothing.hpp>