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