From a77a964775ada411d999f132c5e2d17d7e3951fb Mon Sep 17 00:00:00 2001
From: Davide Mor <davide.mor@skywarder.eu>
Date: Tue, 5 Mar 2024 11:41:05 +0100
Subject: [PATCH] [RIGv2] Fixed servo timings
---
src/boards/RIGv2/Actuators/Actuators.cpp | 32 ++++++++++++++--------
src/boards/RIGv2/Configs/ActuatorsConfig.h | 5 ++--
src/boards/RIGv2/Configs/SensorsConfig.h | 2 +-
src/boards/RIGv2/Radio/Radio.cpp | 6 +++-
src/entrypoints/RIGv2/rig-v2-entry.cpp | 1 -
5 files changed, 29 insertions(+), 17 deletions(-)
diff --git a/src/boards/RIGv2/Actuators/Actuators.cpp b/src/boards/RIGv2/Actuators/Actuators.cpp
index 8908ac082..cacfb260f 100644
--- a/src/boards/RIGv2/Actuators/Actuators.cpp
+++ b/src/boards/RIGv2/Actuators/Actuators.cpp
@@ -98,36 +98,46 @@ Actuators::Actuators(TaskScheduler &scheduler) : scheduler{scheduler}
// Initialize servos
infos[0].servo = std::make_unique<Servo>(
MIOSIX_SERVOS_1_TIM, TimerUtils::Channel::MIOSIX_SERVOS_1_CHANNEL,
- Config::Servos::MIN_PULSE, Config::Servos::MAX_PULSE);
+ Config::Servos::MIN_PULSE, Config::Servos::MAX_PULSE,
+ Config::Servos::FREQUENCY);
infos[1].servo = std::make_unique<Servo>(
MIOSIX_SERVOS_2_TIM, TimerUtils::Channel::MIOSIX_SERVOS_2_CHANNEL,
- Config::Servos::MIN_PULSE, Config::Servos::MAX_PULSE);
+ Config::Servos::MIN_PULSE, Config::Servos::MAX_PULSE,
+ Config::Servos::FREQUENCY);
infos[2].servo = std::make_unique<Servo>(
MIOSIX_SERVOS_3_TIM, TimerUtils::Channel::MIOSIX_SERVOS_3_CHANNEL,
- Config::Servos::MIN_PULSE, Config::Servos::MAX_PULSE);
+ Config::Servos::MIN_PULSE, Config::Servos::MAX_PULSE,
+ Config::Servos::FREQUENCY);
infos[3].servo = std::make_unique<Servo>(
MIOSIX_SERVOS_4_TIM, TimerUtils::Channel::MIOSIX_SERVOS_4_CHANNEL,
- Config::Servos::MIN_PULSE, Config::Servos::MAX_PULSE);
+ Config::Servos::MIN_PULSE, Config::Servos::MAX_PULSE,
+ Config::Servos::FREQUENCY);
// This servo is currently unusable, due to it sharing the same timer as
// miosix, TIM5 infos[4].servo = std::make_unique<Servo>(
// MIOSIX_SERVOS_5_TIM, TimerUtils::Channel::MIOSIX_SERVOS_5_CHANNEL,
- // Config::Servos::MIN_PULSE, Config::Servos::MAX_PULSE);
+ // Config::Servos::MIN_PULSE, Config::Servos::MAX_PULSE,
+ // Config::Servos::FREQUENCY);
infos[5].servo = std::make_unique<Servo>(
MIOSIX_SERVOS_6_TIM, TimerUtils::Channel::MIOSIX_SERVOS_6_CHANNEL,
- Config::Servos::MIN_PULSE, Config::Servos::MAX_PULSE);
+ Config::Servos::MIN_PULSE, Config::Servos::MAX_PULSE,
+ Config::Servos::FREQUENCY);
infos[6].servo = std::make_unique<Servo>(
MIOSIX_SERVOS_7_TIM, TimerUtils::Channel::MIOSIX_SERVOS_7_CHANNEL,
- Config::Servos::MIN_PULSE, Config::Servos::MAX_PULSE);
+ Config::Servos::MIN_PULSE, Config::Servos::MAX_PULSE,
+ Config::Servos::FREQUENCY);
// This servo is currently unusable, due to it sharing the same timer as
// servo 1 infos[7].servo = std::make_unique<Servo>(
// MIOSIX_SERVOS_8_TIM, TimerUtils::Channel::MIOSIX_SERVOS_8_CHANNEL,
- // Config::Servos::MIN_PULSE, Config::Servos::MAX_PULSE);
+ // Config::Servos::MIN_PULSE, Config::Servos::MAX_PULSE,
+ // Config::Servos::FREQUENCY);
infos[8].servo = std::make_unique<Servo>(
MIOSIX_SERVOS_9_TIM, TimerUtils::Channel::MIOSIX_SERVOS_9_CHANNEL,
- Config::Servos::MIN_PULSE, Config::Servos::MAX_PULSE);
+ Config::Servos::MIN_PULSE, Config::Servos::MAX_PULSE,
+ Config::Servos::FREQUENCY);
infos[9].servo = std::make_unique<Servo>(
MIOSIX_SERVOS_10_TIM, TimerUtils::Channel::MIOSIX_SERVOS_10_CHANNEL,
- Config::Servos::MIN_PULSE, Config::Servos::MAX_PULSE);
+ Config::Servos::MIN_PULSE, Config::Servos::MAX_PULSE,
+ Config::Servos::FREQUENCY);
ServoInfo *info;
info = getServo(ServosList::FILLING_VALVE);
@@ -314,7 +324,7 @@ bool Actuators::isServoOpen(ServosList servo)
return false;
}
- return info->getServoPosition() > Config::Servos::SERVO_OPEN_THRESHOLD;
+ return info->closeTs == 0;
}
uint64_t Actuators::getServoOpeningTime(ServosList servo)
diff --git a/src/boards/RIGv2/Configs/ActuatorsConfig.h b/src/boards/RIGv2/Configs/ActuatorsConfig.h
index afe032ba2..9fb5cdf62 100644
--- a/src/boards/RIGv2/Configs/ActuatorsConfig.h
+++ b/src/boards/RIGv2/Configs/ActuatorsConfig.h
@@ -33,13 +33,12 @@ namespace Servos
{
static constexpr unsigned int MIN_PULSE = 500;
-static constexpr unsigned int MAX_PULSE = 2500;
+static constexpr unsigned int MAX_PULSE = 2460;
+static constexpr unsigned int FREQUENCY = 333;
static constexpr uint32_t SERVO_TIMINGS_CHECK_PERIOD = 100;
static constexpr long long SERVO_CONFIDENCE_TIME = 500; // 0.5s
static constexpr float SERVO_CONFIDENCE = 0.02; // 2%
-// More than 10% is considered open
-static constexpr float SERVO_OPEN_THRESHOLD = 0.10; // 10%
static constexpr uint32_t DEFAULT_FILLING_OPENING_TIME = 15000; // 15s
static constexpr uint32_t DEFAULT_VENTING_OPENING_TIME = 15000; // 15s
diff --git a/src/boards/RIGv2/Configs/SensorsConfig.h b/src/boards/RIGv2/Configs/SensorsConfig.h
index 530fefe36..eb588426c 100644
--- a/src/boards/RIGv2/Configs/SensorsConfig.h
+++ b/src/boards/RIGv2/Configs/SensorsConfig.h
@@ -36,7 +36,7 @@ namespace Sensors
static constexpr uint32_t ADC_SAMPLE_PERIOD = 10;
static constexpr uint32_t TC_SAMPLE_PERIOD = 100;
-static constexpr float ADC1_CH1_SHUNT_RESISTANCE = 30.3;
+static constexpr float ADC1_CH1_SHUNT_RESISTANCE = 29.4048;
static constexpr float ADC1_CH2_SHUNT_RESISTANCE = 30.4;
static constexpr float ADC1_CH3_SHUNT_RESISTANCE = 30.5;
static constexpr float ADC1_CH4_SHUNT_RESISTANCE = 30.8;
diff --git a/src/boards/RIGv2/Radio/Radio.cpp b/src/boards/RIGv2/Radio/Radio.cpp
index 63c3d5e93..54ae3ad9f 100644
--- a/src/boards/RIGv2/Radio/Radio.cpp
+++ b/src/boards/RIGv2/Radio/Radio.cpp
@@ -150,7 +150,11 @@ void Radio::sendNack(const mavlink_message_t& msg)
Boardcore::MavlinkStatus Radio::getMavStatus()
{
- return mavDriver->getStatus();
+ if(mavDriver) {
+ return mavDriver->getStatus();
+ } else {
+ return {};
+ }
}
void Radio::handleMessage(const mavlink_message_t& msg)
diff --git a/src/entrypoints/RIGv2/rig-v2-entry.cpp b/src/entrypoints/RIGv2/rig-v2-entry.cpp
index d2d9d167e..b93bbab23 100644
--- a/src/entrypoints/RIGv2/rig-v2-entry.cpp
+++ b/src/entrypoints/RIGv2/rig-v2-entry.cpp
@@ -168,7 +168,6 @@ int main()
}
// Periodic statistics
- int i = 0;
while (true)
{
Thread::sleep(1000);
--
GitLab