diff --git a/src/boards/RIGv2/Actuators/Actuators.cpp b/src/boards/RIGv2/Actuators/Actuators.cpp
index 8908ac082ea849e709770d756e9514d4ac945a04..cacfb260fc161b03300ce0e91220a0159ca4a362 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 afe032ba23c55bd0f952fd6239008591e27c0868..9fb5cdf627dba1a05edffe747c6f28c1f69f2641 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 530fefe365f28f746c0cee5a313ce51d2d4d75a4..eb588426c2036fa1df5cf5a8d9c76fc836e598a8 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 63c3d5e93911f82519ae8dff8114a550ec8796a1..54ae3ad9f5c4dbe05e30fffd49f5a60e21b56c28 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 d2d9d167e206b540c96a125c6003ebcac1ab90d0..b93bbab2329a5c323510b8e5ad53289ad1a04843 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);