Follower.h

Go to the documentation of this file.

/* Copyright (c) 2024 Skyward Experimental Rocketry
 * Authors: Emilio Corigliano, Niccolò Betto, Federico Lolli
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */
 
#pragma once
 
#include <algorithms/Algorithm.h>
#include <algorithms/NAS/NASState.h>
#include <diagnostic/PrintLogger.h>
#include <logger/Logger.h>
#include <sensors/SensorData.h>
#include <sensors/Vectornav/VN300/VN300Data.h>
 
#include <atomic>
#include <chrono>
#include <utils/ModuleManager/ModuleManager.hpp>
 
#include "FollowerData.h"
 
namespace Boardcore
{
 
static constexpr float YAW_GAIN_LIMIT =
    1.0;  
static constexpr float PITCH_GAIN_LIMIT =
    1.0;  
 
class Follower : public Algorithm
{
public:
    explicit Follower(std::chrono::milliseconds updatePeriod);
 
    bool init() override;
 
    void setAntennaCoordinates(const GPSData& gpsData);
 
    void setRocketNASOrigin(const GPSData& gpsData);
 
    void setLastRocketNasState(const NASState& nasState);
 
    void setLastAntennaAttitude(const VN300Data& attitudeData);
 
    bool setMaxGain(float yawGainNew, float pitchGainNew);
 
    FollowerState getState();
 
    AntennaAngles getTargetAngles();
 
private:
    void step() override;
 
    VN300Data getLastAntennaAttitude();
 
    NASState getLastRocketNasState();
 
    AntennaAngles rocketPositionToAntennaAngles(const NEDCoords& ned);
 
    void setState(const FollowerState& newState);
 
    Eigen::Vector2f getAntennaCoordinates();
 
    Eigen::Vector3f getRocketNASOrigin();
 
    // actuation update period [ms]
    std::chrono::milliseconds updatePeriod;
 
    // max number of retries for GPS data acquisition
    const uint8_t maxInitRetries = 120;
 
    bool antennaCoordinatesSet = false;
    bool rocketCoordinatesSet  = false;
    bool lastRocketNasStateSet = false;
    std::atomic<bool> firstAntennaAttitudeSet{false};
 
    VN300Data lastAntennaAttitude;
 
    NASState lastRocketNasState;
 
    // 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;
    /* 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;
 
    PrintLogger logger = Logging::getLogger("Follower");
 
    // General mutex for the follower
    miosix::FastMutex followerMutex;
 
    float yawGain   = YAW_GAIN_LIMIT;    
    float pitchGain = PITCH_GAIN_LIMIT;  
};
 
}  // namespace Boardcore