UavStateMachine.h

// %flair:license{
// This file is part of the Flair framework distributed under the
// CECILL-C License, Version 1.0.
// %flair:license}
//  created:    2014/04/29
//  filename:   UavStateMachine.h
//
//  author:     Gildas Bayard, Guillaume Sanahuja
//              Copyright Heudiasyc UMR UTC/CNRS 7253
//
//  version:    $Id: $
//
//  purpose:    state machine for UAV
//
//
/*********************************************************************/

#ifndef UAVSTATEMACHINE_H
#define UAVSTATEMACHINE_H

#include <Thread.h>
#include <Vector2D.h>
#include <Vector3D.h>
#include <Euler.h>
#include <Quaternion.h>

namespace flair {
  namespace core {
    class FrameworkManager;
    class AhrsData;
    class io_data;
  }
  namespace gui {
    class PushButton;
    class GridLayout;
    class Tab;
    class DoubleSpinBox;
  }
  namespace filter {
    class ControlLaw;
    class NestedSat;
    class Pid;
    class PidThrust;
    class TrajectoryGenerator1D;
  }
  namespace sensor {
    class TargetController;
    class AltitudeSensor;
  }
  namespace meta {
    class MetaDualShock3;
    class Uav;
  }
}

namespace flair {
namespace meta {

class UavStateMachine : public core::Thread {
protected:
  enum class AltitudeMode_t { Manual, Custom };
  const AltitudeMode_t &GetAltitudeMode(void) const { return altitudeMode; }
  bool SetAltitudeMode(const AltitudeMode_t &altitudeMode);

  // uses TrajectoryGenerator1D *altitudeTrajectory to go to desiredAltitude
  // available in mode AltitudeMode_t::Manual
  // return true if goto is possible
  bool GotoAltitude(float desiredAltitude);

  enum class OrientationMode_t { Manual, Custom };
  const OrientationMode_t &GetOrientationMode(void) const {
    return orientationMode;
  }
  bool SetOrientationMode(const OrientationMode_t &orientationMode);

  enum class ThrustMode_t { Default, Custom };
  const ThrustMode_t &GetThrustMode() const { return thrustMode; }
  bool SetThrustMode(const ThrustMode_t &thrustMode);

  enum class TorqueMode_t { Default, Custom };
  const TorqueMode_t &GetTorqueMode(void) const { return torqueMode; }
  bool SetTorqueMode(const TorqueMode_t &torqueMode);

  enum class Event_t {
    EnteringFailSafeMode,
    EnteringControlLoop,
    StartLanding,
    FinishLanding,
    Stopped,
    TakingOff,
    EmergencyStop,
    Stabilized, // as soon as uav is 3cm far from the ground
    ZTrajectoryFinished,
    StartLog,
    StopLog,
  };

  UavStateMachine(sensor::TargetController* controller);
  ~UavStateMachine();

  const core::Quaternion &GetCurrentQuaternion(void) const;

  const core::Vector3Df &GetCurrentAngularSpeed(void) const;

  void Land(void);
  void EmergencyLand(void);
  void TakeOff(void);
  void EmergencyStop(void);

  virtual void SignalEvent(Event_t event);

  virtual const core::AhrsData *GetOrientation(void) const;
  const core::AhrsData *GetDefaultOrientation(void) const;

  virtual void AltitudeValues(float &z, float &dz) const; // in uav coordinate!
  void EnterFailSafeMode(void);
  bool ExitFailSafeMode(void);
  void FailSafeAltitudeValues(float &z, float &dz) const; // in uav coordinate!
  
  void SetFailSafeAltitudeSensor(sensor::AltitudeSensor *altitudeSensor);

  gui::GridLayout *GetButtonsLayout(void) const;
  virtual void ExtraSecurityCheck(void){};
  virtual void ExtraCheckJoystick(void){};
  virtual void ExtraCheckPushButton(void){};

  void GetDefaultReferenceAltitude(float &refAltitude,
                                   float &refVerticalVelocity);
  virtual void GetReferenceAltitude(float &refAltitude,
                                    float &refVerticalVelocity);
  // float GetDefaultThrustOffset(void);
  const core::AhrsData *GetDefaultReferenceOrientation(void) const;
  virtual const core::AhrsData *GetReferenceOrientation(void);

  virtual void ComputeCustomTorques(core::Euler &torques);

  void ComputeDefaultTorques(core::Euler &torques);

  // const core::Euler &GetTorques() const;

  virtual float ComputeCustomThrust(void);

  float ComputeDefaultThrust(void);

  // float GetThrust() const;

  void AddDeviceToControlLawLog(const core::IODevice *device);

  void AddDataToControlLawLog(const core::io_data *data);
        
  virtual void LowBatteryAction(void);
        
  void LowBatteryDefaultAction(void);

  sensor::TargetController *GetTargetController(void) const;
  MetaDualShock3 *GetJoystick(void) const;
  
  filter::NestedSat *GetURoll(void);
  filter::NestedSat *GetUPitch(void);
  filter::Pid *GetUYaw(void);
  filter::PidThrust *GetUZ(void);
  filter::TrajectoryGenerator1D *GetAltitudeTrajectory(void);
  
  gui::Tab *uavTab,*setupLawTab, *graphLawTab;

private:
  enum class AltitudeState_t {
    Stopped,      
    TakingOff,    
    Stabilized,   
    StartLanding, 
    FinishLanding 
  };
  AltitudeState_t altitudeState;
  void ProcessAltitudeFiniteStateMachine();
  void ComputeReferenceAltitude(float &refAltitude, float &refVerticalVelocity);

  float groundAltitude; // effective altitude when the uav leaves the ground
  float currentAltitude, currentVerticalSpeed;

  bool failSafeMode;
  void SecurityCheck(void);
  void MandatorySecurityCheck(void);
  void CheckJoystick();
  void GenericCheckJoystick();
  void CheckPushButton(void);
  void GenericCheckPushButton(void);
  void Run(void) override;
  void StopMotors(void);
  bool IsValuePossible(float value,std::string desc);

  Uav *uav;
  MetaDualShock3 *joy;
  sensor::TargetController *controller;

  core::Quaternion currentQuaternion;
  core::Vector3Df currentAngularSpeed;

  const core::AhrsData *ComputeReferenceOrientation(void);

  void ComputeOrientation(void);
  void ComputeAltitude(void);

  void ComputeTorques(void);
  core::Euler currentTorques, savedDefaultTorques;
  bool needToComputeDefaultTorques;

  void ComputeThrust(void);
  float currentThrust, savedDefaultThrust;
  bool needToComputeDefaultThrust;

  gui::PushButton *button_quit, *button_take_off, *button_land,
      *button_start_log, *button_stop_log;
  gui::GridLayout *buttonslayout;
  gui::DoubleSpinBox *desiredTakeoffAltitude, *desiredLandingAltitude;
  AltitudeMode_t altitudeMode;
  OrientationMode_t orientationMode;
  ThrustMode_t thrustMode;
  TorqueMode_t torqueMode;
  bool flagBatteryLow;
  bool flagConnectionLost;
  bool flagCriticalSensorLost;
  bool flagZTrajectoryFinished;
  bool safeToFly;
  filter::NestedSat *uRoll, *uPitch;
  filter::Pid *uYaw;
  filter::PidThrust *uZ;
  filter::TrajectoryGenerator1D *altitudeTrajectory;
  sensor::AltitudeSensor *failSafeAltitudeSensor;
};
}; // end namespace meta
}; // end namespace flair
#endif // UAVSTATEMACHINE_H