DroneController.h

Go to the documentation of this file.

#pragma once

#ifndef __DRONECONTROLLER_H
#define __DRONECONTROLLER_H




#include "TooN/se3.h"
#include <queue>
#include "geometry_msgs/Twist.h"
#include "uga_tum_ardrone/filter_state.h"

class ControlNode;

struct ControlCommand
{
        inline ControlCommand() {roll = pitch = yaw = gaz = 0;}
        inline ControlCommand(double roll, double pitch, double yaw, double gaz)
        {
                this->roll = roll;
                this->pitch = pitch;
                this->yaw = yaw;
                this->gaz = gaz;
        }
        double yaw, roll, pitch, gaz;
};


struct DronePosition
{
public:
        double yaw;
        TooN::Vector<3> pos;
        inline DronePosition(TooN::Vector<3> pos, double yaw)
                : yaw(yaw), pos(pos) {}
        inline DronePosition(){ yaw=0; pos=TooN::makeVector(0,0,0);}
};

class DroneController
{
private:
        ControlCommand lastSentControl;

        // currentTarget.
        DronePosition target;
        bool targetValid;

        // used for integral term
        TooN::Vector<4> targetNew;      // 0=target has been reached before
                                                                // 1=target is new

        // need to keep track of integral terms
        TooN::Vector<4> i_term;
        TooN::Vector<4> last_err;
        TooN::Vector<4> speedAverages;

        double lastTimeStamp;
        double targetSetAtClock;
        ControlCommand hoverCommand;



        // filled with info (on update)
        bool  ptamIsGood;
        double scaleAccuracy;
        void calcControl(TooN::Vector<4> new_err, TooN::Vector<4> d_error, double yaw, double pitch, double roll);

public:

        // generates and sends a new control command to the drone, based on the currently active command ant the drone's position.
        ControlCommand update(uga_tum_ardrone::filter_stateConstPtr);

        ControlNode* node;

        // for logging, gets filled with recent infos on control.
        TooN::Vector<30> logInfo;

        // adds a waypoint
        void setTarget(DronePosition newTarget);
        void clearTarget();
        DronePosition getCurrentTarget();
        ControlCommand getLastControl();

        // gets last error
        TooN::Vector<4> getLastErr();

        DroneController(void);
        ~DroneController(void);




    // damped spring control parameters, settable by dynamic reconfigure

    double K_direct;        // spring strength for yaw and linear Z
    double K_rp;            // spring strength for roll and pitch

    double droneMassInKilos;
    double max_rp_radians;  // maximum roll and pitch of the drone in radians (not the ardrone_autonomy parameter)

        double rise_fac;
        double agressiveness;
        double max_gaz_rise;
        double max_gaz_drop;
        double max_yaw;
        double max_rp;
        double xy_damping_factor;
};
#endif /* __DRONECONTROLLER_H */