diff --git a/src/boards/Groundstation/Automated/Hub.cpp b/src/boards/Groundstation/Automated/Hub.cpp index 3a182058f4598e3a386ac9c5cf2d85bf9fb43354..6833d510b669be28fa412efa80bef79ec35eda69 100644 --- a/src/boards/Groundstation/Automated/Hub.cpp +++ b/src/boards/Groundstation/Automated/Hub.cpp @@ -286,6 +286,7 @@ void Hub::dispatchIncomingMsg(const mavlink_message_t& msg) gpsState.latitude = rocketST.ref_lat; gpsState.longitude = rocketST.ref_lon; gpsState.height = rocketST.ref_alt; + gpsState.fix = 3; setRocketOrigin(gpsState); diff --git a/src/boards/Groundstation/Automated/SMA/SMA.cpp b/src/boards/Groundstation/Automated/SMA/SMA.cpp index d04b7f9c62d97d84ea7ee09fb5ba16abf32a30f9..31d0b1bab660453b6ce021ca5a71b1b9cb2087fe 100644 --- a/src/boards/Groundstation/Automated/SMA/SMA.cpp +++ b/src/boards/Groundstation/Automated/SMA/SMA.cpp @@ -137,10 +137,32 @@ void SMA::setFatal() { fatalInit = true; }; void SMA::update() { - GPSData rocketCoordinates; + GPSData rocketCoordinates, antennaCoordinates; + VN300Data vn300Data; + Hub* hub = static_cast<Hub*>(getModule<Groundstation::HubBase>()); + auto* sensors = getModule<Sensors>(); rocketCoordinates = hub->getRocketOrigin(); + // update antenna coordinates + vn300Data = sensors->getVN300LastSample(); + if (vn300Data.fix_gps == 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.nedVelX; + antennaCoordinates.velocityEast = vn300Data.nedVelY; + antennaCoordinates.velocityDown = vn300Data.nedVelZ; + antennaCoordinates.satellites = vn300Data.fix_gps; + antennaCoordinates.fix = vn300Data.fix_gps; + + // update follower with coordinates + follower.setAntennaCoordinates(antennaCoordinates); + } + // update follower with the rocket GPS data follower.setRocketNASOrigin(rocketCoordinates); @@ -160,32 +182,8 @@ void SMA::update() // in fix_antennas state, wait for the GPS fix of ARP case SMAState::FIX_ANTENNAS: { - VN300Data vn300Data; - GPSData antennaPosition; - - auto* sensors = getModule<Sensors>(); - - vn300Data = sensors->getVN300LastSample(); - if (vn300Data.fix_gps == 3) + if (antennaCoordinates.fix == 3) { - // build the GPSData struct with the VN300 data - antennaPosition.gpsTimestamp = vn300Data.insTimestamp; - antennaPosition.latitude = vn300Data.latitude; - antennaPosition.longitude = vn300Data.longitude; - antennaPosition.height = vn300Data.altitude; - antennaPosition.velocityNorth = vn300Data.nedVelX; - antennaPosition.velocityEast = vn300Data.nedVelY; - antennaPosition.velocityDown = vn300Data.nedVelZ; - antennaPosition.satellites = vn300Data.fix_gps; - antennaPosition.fix = vn300Data.fix_gps; - - // update follower with coordinates - follower.setAntennaCoordinates(antennaPosition); - - LOG_INFO(Logging::getLogger("automated_antennas"), - "Antenna GPS position acquired !coord [{}, {}] [deg]", - antennaPosition.latitude, antennaPosition.longitude); - // fix found, now move to the next state EventBroker::getInstance().post(ARP_FIX_ANTENNAS, TOPIC_ARP); }