diff --git a/src/shared/algorithms/Follower/Follower.cpp b/src/shared/algorithms/Follower/Follower.cpp
index 46beec180682ff59bc351efa995101c257ab698b..bc5b45f8ed2fba457d47079d8f7f2667619edd65 100644
--- a/src/shared/algorithms/Follower/Follower.cpp
+++ b/src/shared/algorithms/Follower/Follower.cpp
@@ -50,23 +50,22 @@ float minimizeRotation(float angle)
return angle;
}
-Follower::Follower(std::chrono::milliseconds updatePeriod)
- : updatePeriod(static_cast<float>(updatePeriod.count()) / 1000),
- targetAngles({0, 0, 0})
+Follower::Follower(std::chrono::milliseconds updPeriod)
+ : updatePeriod{updPeriod}, targetAngles({0, 0, 0})
{
}
-void Follower::setAntennaCoordinates(const Boardcore::GPSData& gpsData)
+void Follower::setAntennaCoordinates(const GPSData& gpsData)
{
- Lock<FastMutex> lock(lastAntennaAttitudeMutex);
- antennaCoordinates = {gpsData.latitude, gpsData.longitude, gpsData.height};
+ Lock<FastMutex> lock(followerMutex);
+ antennaCoordinates = {gpsData.latitude, gpsData.longitude};
Boardcore::Logger::getInstance().log(LogAntennasCoordinates(gpsData));
antennaCoordinatesSet = true;
}
-void Follower::setRocketNASOrigin(const Boardcore::GPSData& gpsData)
+void Follower::setRocketNASOrigin(const GPSData& gpsData)
{
- Lock<FastMutex> lock(lastAntennaAttitudeMutex);
+ Lock<FastMutex> lock(followerMutex);
rocketNASOrigin = {gpsData.latitude, gpsData.longitude, gpsData.height};
Boardcore::Logger::getInstance().log(LogRocketCoordinates(gpsData));
rocketCoordinatesSet = true;
@@ -74,97 +73,107 @@ void Follower::setRocketNASOrigin(const Boardcore::GPSData& gpsData)
void Follower::setLastAntennaAttitude(const VN300Data& attitudeData)
{
- Lock<FastMutex> lock(lastAntennaAttitudeMutex);
- lastAntennaAttitude = attitudeData;
+ Lock<FastMutex> lock(followerMutex);
+ firstAntennaAttitudeSet = true;
+ lastAntennaAttitude = attitudeData;
}
VN300Data Follower::getLastAntennaAttitude()
{
- Lock<FastMutex> lock(lastAntennaAttitudeMutex);
+ Lock<FastMutex> lock(followerMutex);
return lastAntennaAttitude;
}
void Follower::setLastRocketNasState(const NASState& nasState)
{
- Lock<FastMutex> lock(lastRocketNasStateMutex);
- lastRocketNasState = nasState;
- firstAntennaAttitudeSet = true;
+ Lock<FastMutex> lock(followerMutex);
+ lastRocketNasState = nasState;
+ lastRocketNasStateSet = true;
}
NASState Follower::getLastRocketNasState()
{
- Lock<FastMutex> lock(lastRocketNasStateMutex);
+ Lock<FastMutex> lock(followerMutex);
return lastRocketNasState;
}
FollowerState Follower::getState()
{
- miosix::Lock<miosix::FastMutex> lock(stateMutex);
+ miosix::Lock<miosix::FastMutex> lock(followerMutex);
return state;
}
void Follower::setState(const FollowerState& newState)
{
- miosix::Lock<miosix::FastMutex> lock(stateMutex);
+ miosix::Lock<miosix::FastMutex> lock(followerMutex);
state = newState;
}
+AntennaAngles Follower::getTargetAngles()
+{
+ miosix::Lock<miosix::FastMutex> lock(followerMutex);
+ return targetAngles;
+}
+
bool Follower::init()
{
+ if (isInit)
+ return true;
if (!antennaCoordinatesSet || !rocketCoordinatesSet)
{
LOG_ERR(logger, "Antenna or rocket coordinates not set");
return false;
}
-
- // Antenna Coordinates
- Eigen::Vector2f antennaCoord{getAntennaCoordinates().head<2>()};
- // Rocket coordinates
- Eigen::Vector2f rocketCoord{getRocketNASOrigin().head<2>()};
-
- initialAntennaRocketDistance =
- Aeroutils::geodetic2NED(rocketCoord, antennaCoord);
-
- LOG_INFO(logger, "Initial antenna - rocket distance: [{}, {}] [m]\n",
- initialAntennaRocketDistance[0], initialAntennaRocketDistance[1]);
-
return true;
}
void Follower::step()
{
- NASState lastRocketNas = getLastRocketNasState();
+ AntennaAngles diffAngles;
+ VN300Data vn300;
+ NASState lastRocketNas;
+ NEDCoords rocketPosition;
- // Getting the position of the rocket wrt the antennas in NED frame
- NEDCoords rocketPosition = {lastRocketNas.n, lastRocketNas.e,
- lastRocketNas.d};
+ // Read the data for the step computation
+ {
+ Lock<FastMutex> lock(followerMutex);
- // Calculate the antenna target angles from the NED rocket coordinates
- targetAngles = rocketPositionToAntennaAngles(rocketPosition);
+ if (!firstAntennaAttitudeSet)
+ {
+ LOG_ERR(logger, "Antenna attitude not set\n");
+ return;
+ }
+ // TODO: See if needed to check the NAS or rather point to the NAS
+ // origin if missing
- // If attitude data has never been set, do not actuate the steppers
- if (!firstAntennaAttitudeSet)
- {
- LOG_ERR(logger, "Antenna attitude not set");
- return;
- }
+ lastRocketNas = lastRocketNasState;
+ vn300 = lastAntennaAttitude;
- VN300Data vn300 = getLastAntennaAttitude();
+ // Local variable checks and updates
+ // Getting the position of the rocket wrt the antennas in NED frame
+ rocketPosition = {lastRocketNas.n, lastRocketNas.e, lastRocketNas.d};
- // Calculate the amount to move from the current position
- AntennaAngles diffAngles{targetAngles.timestamp,
- targetAngles.yaw - vn300.yaw,
- targetAngles.pitch - vn300.pitch};
+ // Calculate the antenna target angles from the NED rocket coordinates
+ targetAngles = rocketPositionToAntennaAngles(rocketPosition);
+
+ // Calculate the amount to move from the current position
+ diffAngles = {targetAngles.timestamp, targetAngles.yaw - vn300.yaw,
+ targetAngles.pitch - vn300.pitch};
+ }
// Rotate in the shortest direction
- diffAngles.yaw = 0.1 * minimizeRotation(diffAngles.yaw);
- diffAngles.pitch = minimizeRotation(diffAngles.pitch);
+ diffAngles.yaw = YAW_GAIN * minimizeRotation(diffAngles.yaw);
+ diffAngles.pitch = PITCH_GAIN * minimizeRotation(diffAngles.pitch);
// Calculate angular velocity for moving the antennas toward position
float horizontalSpeed =
- std::abs((diffAngles.yaw * 1000) / (360 * updatePeriod));
+ std::abs((diffAngles.yaw) /
+ (360 * (static_cast<float>(updatePeriod.count()) / 1000)));
+ TRACE("[Follower] horizontalSpeed is: %f\n", horizontalSpeed);
float verticalSpeed =
- std::abs((diffAngles.pitch * 1000) / (360 * updatePeriod));
+ std::abs((diffAngles.pitch) /
+ (360 * (static_cast<float>(updatePeriod.count()) / 1000)));
+ TRACE("[Follower] Vertical speed is: %f\n", horizontalSpeed);
// Update the state of the follower
FollowerState newState;
@@ -173,7 +182,12 @@ void Follower::step()
newState.pitch = diffAngles.pitch;
newState.horizontalSpeed = horizontalSpeed;
newState.verticalSpeed = verticalSpeed;
- setState(newState);
+
+ // Write the new state for the follower
+ {
+ Lock<FastMutex> lockWrite(followerMutex);
+ state = newState;
+ }
#ifndef NDEBUG
std::cout << "[FOLLOWER] STEPPER " << "Angles: [" << newState.yaw << ", "
@@ -184,15 +198,15 @@ void Follower::step()
#endif
}
-Eigen::Vector3f Follower::getAntennaCoordinates()
+Eigen::Vector2f Follower::getAntennaCoordinates()
{
- Lock<FastMutex> lock(antennaCoordinatesMutex);
- return Eigen::Vector3f{antennaCoordinates};
+ Lock<FastMutex> lock(followerMutex);
+ return Eigen::Vector2f{antennaCoordinates};
}
Eigen::Vector3f Follower::getRocketNASOrigin()
{
- Lock<FastMutex> lock(rocketNASOriginMutex);
+ Lock<FastMutex> lock(followerMutex);
return Eigen::Vector3f{rocketNASOrigin};
}
@@ -200,18 +214,16 @@ AntennaAngles Follower::rocketPositionToAntennaAngles(
const NEDCoords& rocketNed)
{
// Antenna Coordinates
- Eigen::Vector2f antennaCoord = getAntennaCoordinates().head<2>();
+ Eigen::Vector2f antennaCoord = antennaCoordinates;
- // Rocket coordinates
- Eigen::Vector2f rocketCoord = getRocketNASOrigin().head<2>();
+ // Rocket coordinates, w/out altitude
+ Eigen::Vector2f rocketCoord = rocketNASOrigin.head<2>();
- initialAntennaRocketDistance =
- Aeroutils::geodetic2NED(rocketCoord, antennaCoord);
+ antennaRocketDistance = Aeroutils::geodetic2NED(rocketCoord, antennaCoord);
// NED coordinates of the rocket in the NED antenna frame
- NEDCoords ned = {rocketNed.n + initialAntennaRocketDistance.x(),
- rocketNed.e + initialAntennaRocketDistance.y(),
- rocketNed.d};
+ NEDCoords ned = {rocketNed.n + antennaRocketDistance.x(),
+ rocketNed.e + antennaRocketDistance.y(), rocketNed.d};
AntennaAngles angles;
angles.timestamp = TimestampTimer::getTimestamp();
diff --git a/src/shared/algorithms/Follower/Follower.h b/src/shared/algorithms/Follower/Follower.h
index 527144188bca05a2ae808cec7cf538026a252e92..850a63f90e8e1793d8c889d93aa07cf1c0813616 100644
--- a/src/shared/algorithms/Follower/Follower.h
+++ b/src/shared/algorithms/Follower/Follower.h
@@ -38,6 +38,15 @@
namespace Boardcore
{
+static constexpr float YAW_GAIN = 0.1;
+static constexpr float PITCH_GAIN = 1.0;
+
+/**
+ * @brief Follower class to output the yaw ad pitch necessary to track from the
+ * GPS origin the rocket. Computes the angle to follow the rocket using its NAS
+ * origin, NED position and velocity
+ *
+ */
class Follower : public Algorithm
{
public:
@@ -57,12 +66,14 @@ public:
/**
* @brief Setter for the GPS coordinates of the antenna.
+ * @note No checks for the GPS fix are done
*/
void setAntennaCoordinates(const GPSData& gpsData);
/**
* @brief Setter for the GPS coordinates of the rocket's NAS origin
* reference.
+ * @note No checks for the GPS fix are done
*/
void setRocketNASOrigin(const GPSData& gpsData);
@@ -88,7 +99,7 @@ public:
* @brief Getter for the target antenna position computed by the algorithm.
* @returns The target antenna positions.
*/
- AntennaAngles getTargetAngles() { return targetAngles; }
+ AntennaAngles getTargetAngles();
private:
/**
@@ -112,6 +123,8 @@ private:
/**
* @brief Calculates the target angles from the given NED coordinates that
* the antenna should point to.
+ *
+ * @note Called by a mutex-protected function
*/
AntennaAngles rocketPositionToAntennaAngles(const NEDCoords& ned);
@@ -122,49 +135,52 @@ private:
void setState(const FollowerState& newState);
/**
- * @brief Get for the GPS coordinates of the antenna.
+ * @brief Get for the [lat, lon] coordinates of the antenna.
*/
- Eigen::Vector3f getAntennaCoordinates();
+ Eigen::Vector2f getAntennaCoordinates();
/**
* @brief Get for the GPS coordinates of the rocket's NAS origin reference.
*/
Eigen::Vector3f getRocketNASOrigin();
- // actuation update period [s]
- float updatePeriod;
+ // actuation update period [ms]
+ std::chrono::milliseconds updatePeriod;
+ // Initialization flag
+ std::atomic<bool> isInit{false};
// max number of retries for GPS data acquisition
const uint8_t maxInitRetries = 120;
- bool antennaCoordinatesSet = false;
- bool rocketCoordinatesSet = false;
- bool firstAntennaAttitudeSet = false;
+ bool antennaCoordinatesSet = false;
+ bool rocketCoordinatesSet = false;
+ bool lastRocketNasStateSet = false;
+ std::atomic<bool> firstAntennaAttitudeSet{false};
VN300Data lastAntennaAttitude;
- miosix::FastMutex lastAntennaAttitudeMutex;
NASState lastRocketNasState;
- miosix::FastMutex lastRocketNasStateMutex;
- // GPS coordinates of the antenna [lat, lon, alt] [deg, deg, m]
- Eigen::Vector3f antennaCoordinates;
- miosix::FastMutex antennaCoordinatesMutex;
- // GPS coordinates of the NAS origin taken from reference origin [lat, lon,
- // alt] [deg, deg, m]
+ // TODO: See if assumption has sense...
+ /* GPS coordinates of the antenna [lat, lon] [deg, deg],
+ altitude is considered same as NAS Origin */
+ Eigen::Vector2f antennaCoordinates;
+ /* GPS coordinates of the NAS origin taken from reference origin [lat, lon,
+ alt] [deg, deg, m] */
Eigen::Vector3f rocketNASOrigin;
- miosix::FastMutex rocketNASOriginMutex;
- // Initial distance between the antenna and the rocket while in ramp [lat,
- // lon, alt] [deg, deg, m]
- Eigen::Vector2f initialAntennaRocketDistance;
+ /* Distance between the antenna and the rocket [lat,
+ lon, alt] [deg, deg, m] */
+ Eigen::Vector2f antennaRocketDistance;
// Target yaw and pitch of the system [deg, deg]
AntennaAngles targetAngles;
FollowerState state;
- miosix::FastMutex stateMutex;
PrintLogger logger = Logging::getLogger("Follower");
+
+ // General mutex for the follower
+ miosix::FastMutex followerMutex;
};
} // namespace Boardcore
diff --git a/src/shared/algorithms/Follower/FollowerData.h b/src/shared/algorithms/Follower/FollowerData.h
index d8e2c9df072b131ac13424c0b9117c087a813f4d..59a1b7c2ba9869ed94963fb831720df07852eb9b 100644
--- a/src/shared/algorithms/Follower/FollowerData.h
+++ b/src/shared/algorithms/Follower/FollowerData.h
@@ -42,9 +42,9 @@ struct AntennaAngles
uint64_t timestamp = 0;
float yaw; //!< Angle between the X axis (N axis) and the target position
- //!< on the XY plane (NE plane), positive anti-clockwise [deg]
+ //!< on the XY plane (NE plane), positive clockwise [deg]
float pitch; //!< Angle between the XY plane (NE plane) and the target
- //!< position [deg]
+ //!< position, positive UP [deg]
AntennaAngles() : timestamp{0}, yaw{0}, pitch{0} {};
AntennaAngles(uint64_t timestamp, float yaw, float pitch)
@@ -55,7 +55,9 @@ struct AntennaAngles
};
/**
- * @brief A structure for storing angles relative to the NED frame.
+ * @brief A structure for storing angles relative to the NED frame and the
+ * number of propagations that produce such angle, 0 if no propagation step has
+ * been used. Used for logging.
*/
struct AntennaAnglesLog : public AntennaAngles
{
@@ -84,6 +86,10 @@ struct AntennaAnglesLog : public AntennaAngles
}
};
+/**
+ * @brief State of the Follower algorithm, with the angles and speeds
+ *
+ */
struct FollowerState
{
uint64_t timestamp;
diff --git a/src/shared/algorithms/Propagator/Propagator.cpp b/src/shared/algorithms/Propagator/Propagator.cpp
index 634d1bf26fdeebcdd5dbb98ca5b023254c02d220..8fa10602dffb0ab12f8e11f5485cd8a857b98f91 100644
--- a/src/shared/algorithms/Propagator/Propagator.cpp
+++ b/src/shared/algorithms/Propagator/Propagator.cpp
@@ -24,6 +24,7 @@
#include <drivers/timer/TimestampTimer.h>
#include <utils/AeroUtils/AeroUtils.h>
+#include <utils/Debug.h>
using namespace Eigen;
@@ -52,15 +53,19 @@ void Propagator::step()
: oldState);
// Update Position propagating it with velocity
- state.x_prop = state.x_prop + state.v_prop * updatePeriod;
+ state.setXProp(state.getXProp() + state.getVProp() * updatePeriod);
state.nPropagations++;
state.timestamp = TimestampTimer::getTimestamp();
+
+ // Log propagator state
+ PropagatorState logState(state);
+ Boardcore::Logger::getInstance().log(logState);
}
void Propagator::setRocketNasState(const NASState& newRocketNasState)
{
miosix::Lock<miosix::FastMutex> lockState(stateMutex);
- miosix::Lock<miosix::FastMutex> lockNAS(nasStateMutex);
+ miosix::Lock<miosix::FastMutex> lock(nasStateMutex);
// Reset nPropagations to notify another received "real" packet
state.nPropagations = 0;
diff --git a/src/shared/algorithms/Propagator/Propagator.h b/src/shared/algorithms/Propagator/Propagator.h
index a8d43d4c0af70241f16a5e46c68aca5a41ea37d7..142fc41c21268b8abc42f209894bc95cfd23880b 100644
--- a/src/shared/algorithms/Propagator/Propagator.h
+++ b/src/shared/algorithms/Propagator/Propagator.h
@@ -23,6 +23,7 @@
#pragma once
#include <algorithms/Algorithm.h>
+#include <logger/Logger.h>
#include <miosix.h>
#include <chrono>
diff --git a/src/shared/algorithms/Propagator/PropagatorData.h b/src/shared/algorithms/Propagator/PropagatorData.h
index cab262da5bea6993482136dea1d1989c03f3bcd7..228f306f047a586811061fda71a2d51dbe16be1d 100644
--- a/src/shared/algorithms/Propagator/PropagatorData.h
+++ b/src/shared/algorithms/Propagator/PropagatorData.h
@@ -31,73 +31,113 @@ namespace Boardcore
{
/**
- * @brief Struct containing the state of the propagator. Stores the timestamp of
- * the propagation, the number of propagations since the last NAS rocket packet
- * and the propagated NAS state already divided in significant vectors so that
- * computations with the NAS state are easier.
- * It also exposes a method to retrieve the propagated NAS state as a NASState
- * structure.
+ * @brief State of the propagator, taking into account the prediction steps (0
+ * if true NAS state) and the propagated NAS
+ * @note Can be logged.
+ *
*/
struct PropagatorState
{
uint64_t timestamp; ///< Prediction timestamp [ms]
uint32_t nPropagations; ///< Predictions from last received NAS state
- Eigen::Vector3f x_prop; ///< Position propagation state NED [m]
- Eigen::Vector3f v_prop; ///< Speed propagation state NED [m]
- Eigen::Vector4f q_prop; ///< Quaternion propagation (scalar last)
- Eigen::Vector3f b_prop; ///< Gyroscope bias propagation
-
- PropagatorState()
- : timestamp(0), nPropagations(0), x_prop(0, 0, 0), v_prop(0, 0, 0),
- q_prop(0, 0, 0, 1), b_prop(0, 0, 0)
+ NASState nas;
+
+ PropagatorState() : timestamp(0), nPropagations(0), nas() {}
+
+ PropagatorState(uint64_t timestamp, uint32_t nPropagations,
+ NASState nasState)
+ : timestamp(timestamp), nPropagations(nPropagations), nas(nasState)
{
}
- PropagatorState(const PropagatorState& newState)
- : PropagatorState(newState.timestamp, newState.nPropagations,
- newState.x_prop, newState.v_prop, newState.q_prop,
- newState.b_prop)
+ static std::string header()
{
+ return "timestamp,nPropagations,n,e,d,vn,ve,vd,qx,qy,qz,qw,bx,by,bz\n";
}
- PropagatorState(uint64_t timestamp, uint32_t nPropagations,
- Eigen::Vector3f x_prop, Eigen::Vector3f v_prop,
- Eigen::Vector4f q_prop, Eigen::Vector3f b_prop)
- : timestamp(timestamp), nPropagations(nPropagations), x_prop(x_prop),
- v_prop(v_prop), q_prop(q_prop), b_prop(b_prop)
+ void print(std::ostream& os) const
{
+ os << timestamp << "," << nPropagations << "," << nas.n << "," << nas.e
+ << "," << nas.d << "," << nas.vn << "," << nas.ve << "," << nas.vd
+ << "," << nas.qx << "," << nas.qy << "," << nas.qz << "," << nas.qw
+ << "," << nas.bx << "," << nas.by << "," << nas.bz << "\n";
}
- PropagatorState(uint64_t timestamp, uint32_t nPropagations,
- NASState nasState)
- : timestamp(timestamp), nPropagations(nPropagations),
- x_prop(nasState.n, nasState.e, nasState.d),
- v_prop(nasState.vn, nasState.ve, nasState.vd),
- q_prop(nasState.qx, nasState.qy, nasState.qz, nasState.qw),
- b_prop(nasState.bx, nasState.by, nasState.bz)
+ NASState getNasState() const { return nas; }
+
+ /**
+ * @brief Getter for the vector of positions NED
+ *
+ * @return Eigen::Vector3f the NED position vector
+ */
+ Eigen::Vector3f getXProp() { return Eigen::Vector3f(nas.n, nas.e, nas.d); }
+
+ /**
+ * @brief Setter for the vector of positions NED
+ */
+ void setXProp(Eigen::Vector3f xProp)
{
+ nas.n = xProp(0);
+ nas.e = xProp(1);
+ nas.d = xProp(2);
}
- static std::string header()
+ /**
+ * @brief Getter for the vector of velocities NED
+ *
+ * @return Eigen::Vector3f the NED velocities vector
+ */
+ Eigen::Vector3f getVProp()
{
- return "timestamp,nPropagations,n,e,d,vn,ve,vd,qx,qy,qz,qw,bx,by,bz\n";
+ return Eigen::Vector3f(nas.vn, nas.ve, nas.vd);
}
- void print(std::ostream& os) const
+ /**
+ * @brief Setter for the vector of velocities NED
+ */
+ void setVProp(Eigen::Vector3f vProp)
{
- os << timestamp << "," << nPropagations << "," << x_prop(0) << ","
- << x_prop(1) << "," << x_prop(2) << "," << v_prop(0) << ","
- << v_prop(1) << "," << v_prop(2) << "," << q_prop(0) << ","
- << q_prop(1) << "," << q_prop(2) << "," << q_prop(3) << ","
- << b_prop(0) << "," << b_prop(1) << "," << b_prop(2) << "\n";
+ nas.vn = vProp(0);
+ nas.ve = vProp(1);
+ nas.vd = vProp(2);
}
- NASState getNasState() const
+ /**
+ * @brief Getter for the vector of quaternions
+ *
+ * @return Eigen::Vector3f the quaternions vector
+ */
+ Eigen::Vector4f getQProp()
+ {
+ return Eigen::Vector4f(nas.qx, nas.qy, nas.qz, nas.qw);
+ }
+
+ /**
+ * @brief Setter for the vector of quaternions
+ */
+ void setQProp(Eigen::Vector4f qProp)
+ {
+ nas.qx = qProp(0);
+ nas.qy = qProp(1);
+ nas.qz = qProp(2);
+ nas.qw = qProp(3);
+ }
+
+ /**
+ * @brief Getter for the vector of quaternions' bias
+ *
+ * @return Eigen::Vector3f the quaternions' bias vector
+ */
+ Eigen::Vector3f getBProp() { return Eigen::Vector3f(nas.n, nas.e, nas.d); }
+
+ /**
+ * @brief Setter for the vector of quaternions' bias
+ */
+ void setBProp(Eigen::Vector3f bProp)
{
- Eigen::Matrix<float, 13, 1> nasState;
- // cppcheck-suppress constStatement
- nasState << x_prop, v_prop, q_prop, b_prop;
- return NASState(timestamp, nasState);
+ nas.bx = bProp(0);
+ nas.by = bProp(1);
+ nas.bz = bProp(2);
}
};
diff --git a/src/shared/sensors/Vectornav/VN300/VN300.cpp b/src/shared/sensors/Vectornav/VN300/VN300.cpp
index d5243bced4c1d5de2f9fa2daf0c5d03c7b969272..e23dc78b6acc30f44c14eb309c7d645d0527565a 100644
--- a/src/shared/sensors/Vectornav/VN300/VN300.cpp
+++ b/src/shared/sensors/Vectornav/VN300/VN300.cpp
@@ -314,6 +314,12 @@ void VN300::buildBinaryDataReduced(const VN300Defs::BinaryDataReduced& rawData,
data.quaternionX = rawData.quaternionX;
data.quaternionY = rawData.quaternionY;
data.quaternionZ = rawData.quaternionZ;
+
+ // Accelerometer
+ data.accelerationTimestamp = timestamp;
+ data.accelerationX = rawData.accelX;
+ data.accelerationY = rawData.accelY;
+ data.accelerationZ = rawData.accelZ;
}
bool VN300::setAntennaA(VN300Defs::AntennaPosition antPos)
@@ -399,6 +405,7 @@ bool VN300::setBinaryOutput()
VN300Defs::BINARYGROUP_COMMON | VN300Defs::BINARYGROUP_GPS;
commonGroup = VN300Defs::COMMONGROUP_YAWPITCHROLL |
VN300Defs::COMMONGROUP_QUATERNION |
+ VN300Defs::COMMONGROUP_ACCEL |
VN300Defs::COMMONGROUP_ANGULARRATE;
gnssGroup = VN300Defs::GPSGROUP_FIX | VN300Defs::GPSGROUP_POSLLA;
diff --git a/src/shared/sensors/Vectornav/VN300/VN300Defs.h b/src/shared/sensors/Vectornav/VN300/VN300Defs.h
index 7e2b186d0ce0263e18806a2fe65acafce8336796..9a32dddee3060189f7d1d756470d54002aa12c3b 100755
--- a/src/shared/sensors/Vectornav/VN300/VN300Defs.h
+++ b/src/shared/sensors/Vectornav/VN300/VN300Defs.h
@@ -277,6 +277,9 @@ struct __attribute__((packed)) BinaryDataReduced
float angularX;
float angularY;
float angularZ;
+ float accelX;
+ float accelY;
+ float accelZ;
uint8_t gpsFix;
double latitude;
double longitude;