diff --git a/src/Groundstation/Automated/SMA/SMA.cpp b/src/Groundstation/Automated/SMA/SMA.cpp index cf800147d9ef7acb260e13af196f9765fca17d35..cbf1c546101a1ab253b978cfa1f6497c29f7300a 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 d86934ac0c05112fd3ab9bc6a516b53b3bda84ee..b73b2c66ad6c45b089b1c678d99d6da5a771cb0c 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); /**