diff --git a/src/boards/RIGv2/Actuators/Actuators.cpp b/src/boards/RIGv2/Actuators/Actuators.cpp index 626c60ca49368d17487082a2fd58f0f10f991a79..b5d1d5a83e534e6f24d0579d6f6507b0c2280539 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 438f505d305cfaceadb8cd8751411f9786ae098d..7bc883ab1d555d3ef32472b4ad2f17967fe216fd 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 d3f761eb24295c31d24328adf985da15351e9c35..a70b425a42024c9b4bf481a99798d261c9904a8e 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 b93bbab2329a5c323510b8e5ad53289ad1a04843..76fba1d13f7616177451e58e9999942fdb6b5352 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?