From 5daccdc87a7d3874e0aefe1bd2d8db827dd76640 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Nicol=C3=B2=20Caruso?= <niccolo.caruso@skywarder.eu>
Date: Tue, 14 Jan 2025 12:39:11 +0100
Subject: [PATCH] [ARP] SMA rework for reduce update and data checks
The active feedback and no feedback now are togheter managed in the update.
Also now the data is acquired once for VN300 or from stepper.
Also changes on the logic of the Hub and Follower, since now there are checks on the validity of NAS and ORIGIN elements which should have been received at least once
---
src/Groundstation/Automated/SMA/SMA.cpp | 131 +++++++-----------------
src/Groundstation/Automated/SMA/SMA.h | 1 +
2 files changed, 36 insertions(+), 96 deletions(-)
diff --git a/src/Groundstation/Automated/SMA/SMA.cpp b/src/Groundstation/Automated/SMA/SMA.cpp
index cf800147d..cbf1c5461 100644
--- a/src/Groundstation/Automated/SMA/SMA.cpp
+++ b/src/Groundstation/Automated/SMA/SMA.cpp
@@ -138,41 +138,45 @@ void SMA::setFatal() { fatalInit = true; };
void SMA::update()
{
GPSData rocketCoordinates, antennaCoordinates;
- VN300Data vn300Data;
+ VN300Data data;
+ auto steppers = getModule<Actuators>();
- Hub* hub = static_cast<Hub*>(getModule<Groundstation::HubBase>());
- auto* sensors = getModule<Sensors>();
- rocketCoordinates = hub->getRocketOrigin();
+ Hub* hub = static_cast<Hub*>(getModule<Groundstation::HubBase>());
+ auto* sensors = getModule<Sensors>();
- // Update the antenna position except in case of no feedback
- if (status.state != SMAState::FIX_ROCKET_NF &&
- status.state != SMAState::INIT_ERROR &&
- status.state != SMAState::ACTIVE_NF &&
- status.state != SMAState::ARM_READY &&
- status.state != SMAState::ACTIVE_NF)
+ // Update the antenna position if in feedback
+ if (status.state == SMAState::FEEDBACK)
{
// update antenna coordinates
- vn300Data = sensors->getVN300LastSample();
- if (vn300Data.gpsFix == 3)
+ data = sensors->getVN300LastSample();
+ if (data.gpsFix == 3)
{
// build the GPSData struct with the VN300 data
- antennaCoordinates.gpsTimestamp = vn300Data.insTimestamp;
- antennaCoordinates.latitude = vn300Data.latitude;
- antennaCoordinates.longitude = vn300Data.longitude;
- antennaCoordinates.height = vn300Data.altitude;
- antennaCoordinates.velocityNorth = vn300Data.velocityX;
- antennaCoordinates.velocityEast = vn300Data.velocityY;
- antennaCoordinates.velocityDown = vn300Data.velocityZ;
- antennaCoordinates.satellites = vn300Data.gpsFix;
- antennaCoordinates.fix = vn300Data.gpsFix;
+ antennaCoordinates.gpsTimestamp = data.insTimestamp;
+ antennaCoordinates.latitude = data.latitude;
+ antennaCoordinates.longitude = data.longitude;
+ antennaCoordinates.height = data.altitude;
+ antennaCoordinates.velocityNorth = data.velocityX;
+ antennaCoordinates.velocityEast = data.velocityY;
+ antennaCoordinates.velocityDown = data.velocityZ;
+ antennaCoordinates.satellites = data.gpsFix;
+ antennaCoordinates.fix = data.gpsFix;
// update follower with coordinates
follower.setAntennaCoordinates(antennaCoordinates);
}
}
+ else
+ {
+ // Fake attitude data, taking the stepper current positions as pitch and
+ // yaw
+ data.pitch = steppers->getCurrentDegPosition(StepperList::STEPPER_Y);
+ data.yaw = steppers->getCurrentDegPosition(StepperList::STEPPER_X);
+ }
// update follower with the rocket GPS data
- follower.setRocketNASOrigin(rocketCoordinates);
+ if (hub->getRocketOrigin(rocketCoordinates))
+ follower.setRocketNASOrigin(rocketCoordinates);
switch (status.state)
{
@@ -211,89 +215,26 @@ void SMA::update()
}
break;
}
+
// in active state, update the follower and propagator inner states
case SMAState::ACTIVE:
- {
- // retrieve the last NAS Rocket state
- if (hub->hasNasSet())
- {
- NASState nasState = hub->getRocketNasState();
-
- // update the propagator with the NAS state
- // and retrieve the propagated state
- propagator.setRocketNasState(nasState);
- }
- propagator.update(); // step the propagator
- PropagatorState predicted = propagator.getState();
-
- // update the follower with the propagated state
- follower.setLastRocketNasState(predicted.getNasState());
- follower.setLastAntennaAttitude(vn300Data);
- follower.update(); // step the follower
- FollowerState follow = follower.getState();
-
- // Log the target angles and propagations info
- AntennaAngles target = follower.getTargetAngles();
- Boardcore::Logger::getInstance().log(
- Boardcore::AntennaAnglesLog(target, predicted.nPropagations));
-
- // actuate the steppers
- auto steppers = getModule<Actuators>();
- steppers->setSpeed(StepperList::STEPPER_X, follow.horizontalSpeed);
- steppers->setSpeed(StepperList::STEPPER_Y, follow.verticalSpeed);
-
- ActuationStatus actuation =
- steppers->moveDeg(StepperList::STEPPER_X, follow.yaw);
- if (actuation != ActuationStatus::OK)
- {
- LOG_ERR(
- logger,
- "Step antenna - STEPPER_X could not move or reached move "
- "limit. Error: ",
- actuation, "\n");
- }
-
- actuation = steppers->moveDeg(StepperList::STEPPER_Y, follow.pitch);
- if (actuation != ActuationStatus::OK)
- {
- LOG_ERR(
- logger,
- "Step antenna - STEPPER_Y could not move or reached move "
- "limit. Error: ",
- actuation, "\n");
- }
-
- break;
- }
case SMAState::ACTIVE_NF:
{
- VN300Data fakeAttitudeData;
-
// retrieve the last NAS Rocket state
- if (hub->hasNasSet())
- {
- NASState nasState = hub->getRocketNasState();
+
+ NASState nasState;
+ if (hub->hasNewNasState() && hub->getLastRocketNasState(nasState))
// update the propagator with the NAS state
// and retrieve the propagated state
propagator.setRocketNasState(nasState);
- }
+
propagator.update(); // step the propagator
PropagatorState predicted = propagator.getState();
- auto steppers = getModule<Actuators>();
-
- // set the attitude as the current position of the steppers
- // FIXME this method of setting the attitude is too dirty
- // if the follower is updated something may break here
- fakeAttitudeData.pitch =
- steppers->getCurrentDegPosition(StepperList::STEPPER_Y);
- fakeAttitudeData.yaw =
- steppers->getCurrentDegPosition(StepperList::STEPPER_X);
-
// update the follower with the propagated state
follower.setLastRocketNasState(predicted.getNasState());
- follower.setLastAntennaAttitude(fakeAttitudeData);
+ follower.setLastAntennaAttitude(data);
follower.update(); // step the follower
FollowerState follow = follower.getState();
@@ -326,9 +267,9 @@ void SMA::update()
"limit. Error: ",
actuation, "\n");
}
-
- break;
}
+ break;
+
case SMAState::CALIBRATE:
{
if (!sensors->isCalibrating())
@@ -339,9 +280,7 @@ void SMA::update()
}
break;
default:
- {
break;
- }
}
}
@@ -795,7 +734,7 @@ State SMA::state_fix_rocket(const Event& event)
// init the follower before leaving the state
// (compute initial arp-rocket distance and bearing)
if (!follower.init())
- LOG_ERR(logger, "Follower initialization failed");
+ LOG_ERR(logger, "Follower initialization failed\n");
leds->setOn(LedColor::YELLOW);
return HANDLED;
diff --git a/src/Groundstation/Automated/SMA/SMA.h b/src/Groundstation/Automated/SMA/SMA.h
index d86934ac0..b73b2c66a 100644
--- a/src/Groundstation/Automated/SMA/SMA.h
+++ b/src/Groundstation/Automated/SMA/SMA.h
@@ -109,6 +109,7 @@ public:
/**
* @brief Setter for the multipliers of the steppers
*/
+ // TODO: See if has sense to remove it...
void setMultipliers(StepperList axis, float multiplier);
/**
--
GitLab