From 76e20479ea831e788abd0df8cd7c58512a0c54f2 Mon Sep 17 00:00:00 2001
From: Davide Mor <davide.mor@skywarder.eu>
Date: Fri, 1 Mar 2024 17:25:14 +0100
Subject: [PATCH] [RIGv2] Updated actuators and RadioConfig

---
 src/boards/RIGv2/Actuators/Actuators.cpp | 34 +++++++++++-------------
 src/boards/RIGv2/Actuators/Actuators.h   |  4 +--
 src/boards/RIGv2/Configs/RadioConfig.h   |  2 +-
 src/entrypoints/RIGv2/rig-v2-entry.cpp   |  3 ++-
 4 files changed, 21 insertions(+), 22 deletions(-)

diff --git a/src/boards/RIGv2/Actuators/Actuators.cpp b/src/boards/RIGv2/Actuators/Actuators.cpp
index 626c60ca4..b5d1d5a83 100644
--- a/src/boards/RIGv2/Actuators/Actuators.cpp
+++ b/src/boards/RIGv2/Actuators/Actuators.cpp
@@ -38,8 +38,8 @@ void Actuators::ServoInfo::openServo(uint64_t time)
 {
     long long currentTime = getTime();
 
-    openedTs = currentTime;
-    closeTs  = currentTime + (time * Constants::NS_IN_MS);
+    closeTs      = currentTime + (time * Constants::NS_IN_MS);
+    lastActionTs = currentTime;
 
     if (openingEvent != 0)
     {
@@ -49,7 +49,8 @@ void Actuators::ServoInfo::openServo(uint64_t time)
 
 void Actuators::ServoInfo::closeServo()
 {
-    closeTs = 0;
+    closeTs      = 0;
+    lastActionTs = getTime();
 
     if (closingEvent != 0)
     {
@@ -204,7 +205,7 @@ bool Actuators::start()
 bool Actuators::wiggleServo(ServosList servo)
 {
     // Wiggle means open the servo for 1s
-    return openServoWithTime(servo, 1000);
+    return openServoWithTime(servo, 10000);
 }
 
 bool Actuators::toggleServo(ServosList servo)
@@ -386,9 +387,9 @@ void Actuators::updatePositionsTask()
         if (currentTime < infos[idx].closeTs)
         {
             // The valve should be open
-            if (currentTime <
-                infos[idx].openedTs + (Config::Servos::SERVO_CONFIDENCE_TIME *
-                                       Constants::NS_IN_MS))
+            if (currentTime < infos[idx].lastActionTs +
+                                  (Config::Servos::SERVO_CONFIDENCE_TIME *
+                                   Constants::NS_IN_MS))
             {
                 // We should open the valve all the way
                 unsafeSetServoPosition(idx, infos[idx].maxAperture);
@@ -406,18 +407,13 @@ void Actuators::updatePositionsTask()
             // Ok the valve should be closed
             if (infos[idx].closeTs != 0)
             {
-                // The valve JUST closed, notify everybody
-                infos[idx].closeTs = 0;
-                if (infos[idx].closingEvent)
-                {
-                    EventBroker::getInstance().post(infos[idx].closingEvent,
-                                                    TOPIC_MOTOR);
-                }
+                // Perform the servo closing
+                infos[idx].closeServo();
             }
 
-            if (currentTime <
-                infos[idx].closeTs + (Config::Servos::SERVO_CONFIDENCE_TIME *
-                                      Constants::NS_IN_MS))
+            if (currentTime < infos[idx].lastActionTs +
+                                  (Config::Servos::SERVO_CONFIDENCE_TIME *
+                                   Constants::NS_IN_MS))
             {
                 // We should close the valve all the way
                 unsafeSetServoPosition(idx, 0.0);
@@ -425,7 +421,9 @@ void Actuators::updatePositionsTask()
             else
             {
                 // Time to wiggle the valve a little
-                unsafeSetServoPosition(idx, Config::Servos::SERVO_CONFIDENCE);
+                unsafeSetServoPosition(
+                    idx,
+                    infos[idx].maxAperture * Config::Servos::SERVO_CONFIDENCE);
             }
         }
     }
diff --git a/src/boards/RIGv2/Actuators/Actuators.h b/src/boards/RIGv2/Actuators/Actuators.h
index 438f505d3..7bc883ab1 100644
--- a/src/boards/RIGv2/Actuators/Actuators.h
+++ b/src/boards/RIGv2/Actuators/Actuators.h
@@ -55,8 +55,8 @@ private:
 
         // Timestamp of when the servo should close, 0 if closed
         long long closeTs = 0;
-        // Timestamp of when the servo was opened
-        long long openedTs = 0;
+        // Timestamp of last servo action (open/close)
+        long long lastActionTs = 0;
 
         void openServo(uint64_t time);
         void closeServo();
diff --git a/src/boards/RIGv2/Configs/RadioConfig.h b/src/boards/RIGv2/Configs/RadioConfig.h
index d3f761eb2..a70b425a4 100644
--- a/src/boards/RIGv2/Configs/RadioConfig.h
+++ b/src/boards/RIGv2/Configs/RadioConfig.h
@@ -44,7 +44,7 @@ static constexpr unsigned int CIRCULAR_BUFFER_SIZE = 8;
 static constexpr uint8_t MAV_SYSTEM_ID    = 171;
 static constexpr uint8_t MAV_COMPONENT_ID = 96;
 
-static constexpr long long LAST_COMMAND_THRESHOLD = 1000;
+static constexpr long long LAST_COMMAND_THRESHOLD = 300;
 
 }  // namespace Radio
 
diff --git a/src/entrypoints/RIGv2/rig-v2-entry.cpp b/src/entrypoints/RIGv2/rig-v2-entry.cpp
index b93bbab23..76fba1d13 100644
--- a/src/entrypoints/RIGv2/rig-v2-entry.cpp
+++ b/src/entrypoints/RIGv2/rig-v2-entry.cpp
@@ -168,10 +168,11 @@ int main()
     }
 
     // Periodic statistics
+    int i = 0;
     while (true)
     {
         Thread::sleep(1000);
-        sdLogger.log(modules.get<Radio>()->getMavStatus());
+        // sdLogger.log(modules.get<Radio>()->getMavStatus());
         sdLogger.log(CpuMeter::getCpuStats());
         CpuMeter::resetCpuStats();
         // TODO: What the fuck is this?
-- 
GitLab