gnc.h

Go to the documentation of this file.

 
#ifndef GNC_H
#define GNC_H
 
#include "waypoint_list.h"
#include "aircraft/aircraft.h"
#include <easy3d/core/vec.h>
#include "common.h"
 
class GNC 
{
public:
    GNC();
    void setWaypoints(const WaypointList& waypoints);
    void update(const Aircraft& aircraft, float dt);
 
 
    // New additions: For Trim, linearisation etc
    bool computeTrim(Aircraft& aircraft, double Va, double gamma, double R);
    static double trimObjective(const std::vector<double>& angles, std::vector<double>& grad, void* data);
    static Eigen::VectorXd computeXdot(Aircraft& ac);
    bool linearizeAtTrim(Aircraft& drone);
    void computeLinearModel(Aircraft& aircraft,
                              const Eigen::VectorXd& Xtrim,
                              const Eigen::VectorXd& Utrim,
                              Eigen::MatrixXd& A_full,
                              Eigen::MatrixXd& B_full,
                              Eigen::MatrixXd& A_lat,
                              Eigen::MatrixXd& B_lat,
                              Eigen::MatrixXd& A_lon,
                              Eigen::MatrixXd& B_lon);
 
 
    struct TrimData
    {
        double Va;
        double gamma;
        double R;
        double phi_fixed;
        double beta_fixed;
        Aircraft* aircraft;
    };
 
    const TrimData& getTrimData() const { return trimData; }
    const Eigen::MatrixXd& getA() const { return A; }
    const Eigen::MatrixXd& getB() const { return B; }
    const Eigen::MatrixXd& getA_lat() const { return A_lat; }
    const Eigen::MatrixXd& getB_lat() const { return B_lat; }
    const Eigen::MatrixXd& getA_lon() const { return A_lon; }
    const Eigen::MatrixXd& getB_lon() const { return B_lon; }
 
 
 
private:
    friend class Simviewer;
    WaypointList waypoints_;    
 
    TrimData trimData;
    Eigen::MatrixXd A, B, A_lat, B_lat, A_lon, B_lon;
};
 
#endif // GNC_H