// %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.cpp
//
//  author:     Gildas Bayard, Guillaume Sanahuja
//              Copyright Heudiasyc UMR UTC/CNRS 7253
//
//  version:    $Id: $
//
//  purpose:    meta class for UAV
//
//
/*********************************************************************/

#include "UavStateMachine.h"
#include "Uav.h"
#include <DataPlot1D.h>
#include <GridLayout.h>
#include <Tab.h>
#include <TabWidget.h>
#include <PushButton.h>
#include <SpinBox.h>
#include <DoubleSpinBox.h>
#include <X4X8Multiplex.h>
#include <Bldc.h>
#include <Ahrs.h>
#include <MetaUsRangeFinder.h>
#include <ControlLaw.h>
#include <Pid.h>
#include <PidThrust.h>
#include <NestedSat.h>
#include <MetaDualShock3.h>
#include <AhrsData.h>
#include <BatteryMonitor.h>
#include <FrameworkManager.h>
#include <Vector3D.h>
#include <Vector2D.h>
#include <cvmatrix.h>
#include <stdio.h>
#include <TrajectoryGenerator1D.h>

using namespace std;
using namespace flair::core;
using namespace flair::gui;
using namespace flair::sensor;
using namespace flair::actuator;
using namespace flair::filter;
using namespace flair::meta;

UavStateMachine::UavStateMachine(Uav *inUav, uint16_t ds3Port)
    : Thread(getFrameworkManager(), "UavStateMachine", 50), uav(inUav),
      failSafeMode(true), flagConnectionLost(false), flagBatteryLow(false),
      flagZTrajectoryFinished(false) {
  altitudeState = AltitudeState_t::Stopped;
  uav->UseDefaultPlot();

  Tab *uavTab = new Tab(getFrameworkManager()->GetTabWidget(), "uav", 0);
  buttonslayout = new GridLayout(uavTab->NewRow(), "buttons");
  button_kill = new PushButton(buttonslayout->NewRow(), "kill");
  button_start_log = new PushButton(buttonslayout->NewRow(), "start_log");
  button_stop_log = new PushButton(buttonslayout->LastRowLastCol(), "stop_log");
  button_take_off = new PushButton(buttonslayout->NewRow(), "take_off");
  button_land = new PushButton(buttonslayout->LastRowLastCol(), "land");

  Tab *lawTab = new Tab(getFrameworkManager()->GetTabWidget(), "control laws");
  TabWidget *tabWidget = new TabWidget(lawTab->NewRow(), "laws");
  setupLawTab = new Tab(tabWidget, "Setup");
  graphLawTab = new Tab(tabWidget, "Graphes");

  uRoll = new NestedSat(setupLawTab->At(0, 0), "u_roll");
  uRoll->ConvertSatFromDegToRad();
  uRoll->UseDefaultPlot(graphLawTab->NewRow());

  uPitch = new NestedSat(setupLawTab->At(0, 1), "u_pitch");
  uPitch->ConvertSatFromDegToRad();
  uPitch->UseDefaultPlot(graphLawTab->LastRowLastCol());

  uYaw = new Pid(setupLawTab->At(0, 2), "u_yaw");
  uYaw->UseDefaultPlot(graphLawTab->LastRowLastCol());

  uZ = new PidThrust(setupLawTab->At(1, 2), "u_z");
  uZ->UseDefaultPlot(graphLawTab->LastRowLastCol());

  getFrameworkManager()->AddDeviceToLog(uZ);
  uZ->AddDeviceToLog(uRoll);
  uZ->AddDeviceToLog(uPitch);
  uZ->AddDeviceToLog(uYaw);

  joy = new MetaDualShock3(getFrameworkManager(), "dualshock3", ds3Port, 30);
  uav->GetAhrs()->AddPlot(joy->GetReferenceOrientation(), DataPlot::Blue);

  altitudeMode = AltitudeMode_t::Manual;
  orientationMode = OrientationMode_t::Manual;
  thrustMode = ThrustMode_t::Default;
  torqueMode = TorqueMode_t::Default;

  GroupBox *reglagesGroupbox =
      new GroupBox(uavTab->NewRow(), "takeoff/landing");
  desiredTakeoffAltitude =
      new DoubleSpinBox(reglagesGroupbox->NewRow(), "desired takeoff altitude",
                        " m", 0, 5, 0.1, 2, 1);
  desiredLandingAltitude =
      new DoubleSpinBox(reglagesGroupbox->LastRowLastCol(),
                        "desired landing altitude", " m", 0, 1, 0.1, 1);
  altitudeTrajectory =
      new TrajectoryGenerator1D(uavTab->NewRow(), "alt cons", "m");
  uav->GetMetaUsRangeFinder()->GetZPlot()->AddCurve(
      altitudeTrajectory->Matrix()->Element(0), DataPlot::Green);
  uav->GetMetaUsRangeFinder()->GetVzPlot()->AddCurve(
      altitudeTrajectory->Matrix()->Element(1), DataPlot::Green);
}

UavStateMachine::~UavStateMachine() {}

void UavStateMachine::AddDeviceToControlLawLog(const IODevice *device) {
  uZ->AddDeviceToLog(device);
}

void UavStateMachine::AddDataToControlLawLog(const core::io_data *data) {
  uZ->AddDataToLog(data);
}

const TargetController *UavStateMachine::GetJoystick(void) const { return joy; }

const Quaternion &UavStateMachine::GetCurrentQuaternion(void) const {
  return currentQuaternion;
}

const Vector3D &UavStateMachine::GetCurrentAngularSpeed(void) const {
  return currentAngularSpeed;
}

const Uav *UavStateMachine::GetUav(void) const { return uav; }

void UavStateMachine::AltitudeValues(float &altitude,
                                     float &verticalSpeed) const {
  FailSafeAltitudeValues(altitude, verticalSpeed);
}

void UavStateMachine::FailSafeAltitudeValues(float &altitude,
                                             float &verticalSpeed) const {
  altitude = uav->GetMetaUsRangeFinder()->z();
  verticalSpeed = uav->GetMetaUsRangeFinder()->Vz();
}

void UavStateMachine::Run() {
  WarnUponSwitches(true);
  uav->StartSensors();

  if (getFrameworkManager()->ErrorOccured() == true) {
    SafeStop();
  }

  while (!ToBeStopped()) {
    SecurityCheck();

    // get controller inputs
    CheckJoystick();
    CheckPushButton();

    if (IsPeriodSet()) {
      WaitPeriod();
    } else {
      WaitUpdate(uav->GetAhrs());
    }
    needToComputeDefaultTorques = true;
    needToComputeDefaultThrust = true;

    SignalEvent(Event_t::EnteringControlLoop);

    ComputeOrientation();
    ComputeAltitude();

    // compute thrust and torques to apply
    ComputeTorques();
    ComputeThrust(); // logs are added to uz, so it must be updated at last

    // Set torques for roll, pitch and yaw angles (value between -1 and 1). Set
    // thrust (value between 0 and 1)
    uav->GetUavMultiplex()->SetRoll(-currentTorques.roll);
    uav->GetUavMultiplex()->SetPitch(-currentTorques.pitch);
    uav->GetUavMultiplex()->SetYaw(-currentTorques.yaw);
    uav->GetUavMultiplex()->SetThrust(-currentThrust); // on raisonne en negatif
                                                       // sur l'altitude, a
                                                       // revoir avec les
                                                       // equations
    uav->GetUavMultiplex()->SetRollTrim(joy->RollTrim());
    uav->GetUavMultiplex()->SetPitchTrim(joy->PitchTrim());
    uav->GetUavMultiplex()->SetYawTrim(0);
    uav->GetUavMultiplex()->Update(GetTime());
  }

  WarnUponSwitches(false);
}

void UavStateMachine::ComputeOrientation(void) {
  if (failSafeMode) {
    GetDefaultOrientation()->GetQuaternionAndAngularRates(currentQuaternion,
                                                          currentAngularSpeed);
  } else {
    GetOrientation()->GetQuaternionAndAngularRates(currentQuaternion,
                                                   currentAngularSpeed);
  }
}

const AhrsData *UavStateMachine::GetOrientation(void) const {
  return GetDefaultOrientation();
}

const AhrsData *UavStateMachine::GetDefaultOrientation(void) const {
  return uav->GetAhrs()->GetDatas();
}

void UavStateMachine::ComputeAltitude(void) {
  if (failSafeMode) {
    FailSafeAltitudeValues(currentAltitude, currentVerticalSpeed);
  } else {
    AltitudeValues(currentAltitude, currentVerticalSpeed);
  }
}

void UavStateMachine::ComputeReferenceAltitude(float &refAltitude,
                                               float &refVerticalVelocity) {
  if (altitudeMode == AltitudeMode_t::Manual) {
    GetDefaultReferenceAltitude(refAltitude, refVerticalVelocity);
  } else {
    GetReferenceAltitude(refAltitude, refVerticalVelocity);
  }
}

void UavStateMachine::GetDefaultReferenceAltitude(float &refAltitude,
                                                  float &refVerticalVelocity) {
  float zc, dzc;

  switch (altitudeState) {
  // initiate a takeoff: increase motor speed in open loop (see ComputeThrust)
  // until we detect a take off of 0.03m (hard coded value) above the ground.
  case AltitudeState_t::TakingOff: {
    if (currentAltitude > groundAltitude + 0.03) {
      altitudeTrajectory->StartTraj(currentAltitude,
                                    desiredTakeoffAltitude->Value());
      altitudeState = AltitudeState_t::Stabilized;
      SignalEvent(Event_t::Stabilized);
    }
    break;
  }
  // landing, only check if we reach desired landing altitude
  case AltitudeState_t::StartLanding: {
    if (altitudeTrajectory->Position() == desiredLandingAltitude->Value()) {
      // The Uav target altitude has reached its landing value (typically 0)
      // but the real Uav altitude may not have reach this value yet because of
      // command delay. Moreover, it
      // may never exactly reach this value if the ground is not perfectly
      // leveled (critical case: there's a
      // deep and narrow hole right in the sensor line of sight). That's why we
      // have a 2 phases landing strategy.
      altitudeState = AltitudeState_t::FinishLanding;
      SignalEvent(Event_t::FinishLanding);
      joy->SetLedOFF(1); // DualShock3::led1
    }
  }
  // stabilized: check if z trajectory is finished
  case AltitudeState_t::Stabilized: {
    if (!altitudeTrajectory->IsRunning() && !flagZTrajectoryFinished) {
      SignalEvent(Event_t::ZTrajectoryFinished);
      flagZTrajectoryFinished = true;
    }
    if (flagZTrajectoryFinished && desiredTakeoffAltitude->ValueChanged()) {
      flagZTrajectoryFinished = false;
      altitudeTrajectory->StartTraj(currentAltitude,
                                    desiredTakeoffAltitude->Value());
      joy->SetZRef(0);
    }
  }
  }

  // Récupère les consignes (du joystick dans l'implémentation par défaut). La
  // consigne joystick est une vitesse ("delta_z", dzc). le zc est calculé par
  // la manette
  zc = joy->ZRef(); // a revoir, la position offset devrait se calculer dans le
                    // generator
  dzc = joy->DzRef();

  // z control law
  altitudeTrajectory->SetPositionOffset(zc);
  altitudeTrajectory->SetSpeedOffset(dzc);

  altitudeTrajectory->Update(GetTime());
  refAltitude = altitudeTrajectory->Position();
  refVerticalVelocity = altitudeTrajectory->Speed();
}

void UavStateMachine::GetReferenceAltitude(float &refAltitude,
                                           float &refVerticalVelocity) {
  Thread::Warn("Default GetReferenceAltitude method is not overloaded => "
               "switching back to safe mode\n");
  EnterFailSafeMode();
};

void UavStateMachine::ComputeThrust(void) {
  if (altitudeMode == AltitudeMode_t::Manual) {
    currentThrust = ComputeDefaultThrust();
  } else {
    currentThrust = ComputeCustomThrust();
  }
}

float UavStateMachine::ComputeDefaultThrust(void) {
  if (needToComputeDefaultThrust) {
    // compute desired altitude
    float refAltitude, refVerticalVelocity;
    ComputeReferenceAltitude(refAltitude, refVerticalVelocity);

    switch (altitudeState) {
    case AltitudeState_t::TakingOff: {
      // The progressive increase in motor speed is used to evaluate the motor
      // speed that compensate the uav weight. This value
      // will be used as an offset for altitude control afterwards
      uZ->OffsetStepUp();
      break;
    }
    case AltitudeState_t::StartLanding:
    case AltitudeState_t::Stabilized: {
      float p_error = currentAltitude - refAltitude;
      float d_error = currentVerticalSpeed - refVerticalVelocity;
      uZ->SetValues(p_error, d_error);
      break;
    }
    // decrease motor speed in open loop until value offset_g , uav should have
    // already landed or be very close to at this point
    case AltitudeState_t::FinishLanding: {
      if (uZ->OffsetStepDown() == false) {
        StopMotors();
      }
      break;
    }
    }
    uZ->Update(GetTime());

    savedDefaultThrust = uZ->Output();
    needToComputeDefaultThrust = false;
  }

  return savedDefaultThrust;
}

float UavStateMachine::ComputeCustomThrust(void) {
  Thread::Warn("Default GetThrust method is not overloaded => switching back "
               "to safe mode\n");
  EnterFailSafeMode();
  return ComputeDefaultThrust();
}

const AhrsData *UavStateMachine::ComputeReferenceOrientation(void) {
  if (orientationMode == OrientationMode_t::Manual) {
    return GetDefaultReferenceOrientation();
  } else {
    return GetReferenceOrientation();
  }
}

const AhrsData *UavStateMachine::GetDefaultReferenceOrientation(void) const {
  // We directly control yaw, pitch, roll angles
  return joy->GetReferenceOrientation();
}

const AhrsData *UavStateMachine::GetReferenceOrientation(void) {
  Thread::Warn("Default GetReferenceOrientation method is not overloaded => "
               "switching back to safe mode\n");
  EnterFailSafeMode();
  return GetDefaultReferenceOrientation();
}

void UavStateMachine::ComputeTorques(void) {
  if (torqueMode == TorqueMode_t::Default) {
    ComputeDefaultTorques(currentTorques);
  } else {
    ComputeCustomTorques(currentTorques);
  }
}

void UavStateMachine::ComputeDefaultTorques(Euler &torques) {
  if (needToComputeDefaultTorques) {
    const AhrsData *refOrientation = ComputeReferenceOrientation();
    Quaternion refQuaternion;
    Vector3D refAngularRates;
    refOrientation->GetQuaternionAndAngularRates(refQuaternion,
                                                 refAngularRates);
    Euler refAngles = refQuaternion.ToEuler();
    Euler currentAngles = currentQuaternion.ToEuler();

    uYaw->SetValues(currentAngles.YawDistanceFrom(refAngles.yaw),
                    currentAngularSpeed.z - refAngularRates.z);
    uYaw->Update(GetTime());
    torques.yaw = uYaw->Output();

    uPitch->SetValues(refAngles.pitch, currentAngles.pitch,
                      currentAngularSpeed.y);
    uPitch->Update(GetTime());
    torques.pitch = uPitch->Output();

    uRoll->SetValues(refAngles.roll, currentAngles.roll, currentAngularSpeed.x);
    uRoll->Update(GetTime());
    torques.roll = uRoll->Output();

    savedDefaultTorques = torques;
    needToComputeDefaultTorques = false;
  } else {
    torques = savedDefaultTorques;
  }
}

void UavStateMachine::ComputeCustomTorques(Euler &torques) {
  Thread::Warn("Default ComputeCustomTorques method is not overloaded => "
               "switching back to safe mode\n");
  EnterFailSafeMode();
  ComputeDefaultTorques(torques);
}

void UavStateMachine::TakeOff(void) {
  flagZTrajectoryFinished = false;

  if (altitudeState == AltitudeState_t::Stopped) { // &&
    // GetBatteryMonitor()->IsBatteryLow()==false)
    // The uav always takes off in fail safe mode
    EnterFailSafeMode();
    joy->SetLedOFF(4); // DualShock3::led4
    joy->SetLedOFF(1); // DualShock3::led1
    joy->Rumble(0x70);
    joy->SetZRef(0);

    uZ->SetOffset(); // positionne l'offset de compensation de la gravité à sa
                     // valeur initiale (station sol)

    uav->GetUavMultiplex()->LockUserInterface();
    // Active les moteurs. Pouvoir les désactiver permet de pouvoir observer les
    // consignes moteurs
    // sans les faire tourner effectivement (en déplaçant à la main le drone)
    uav->GetBldc()->SetEnabled(true);
    groundAltitude = currentAltitude;
    altitudeState = AltitudeState_t::TakingOff;

    SignalEvent(Event_t::TakingOff);
  } else {
    joy->ErrorNotify();
  }
}

void UavStateMachine::Land(void) {
  if (altitudeMode != AltitudeMode_t::Manual) {
    SetAltitudeMode(AltitudeMode_t::Manual);
  }
  if (altitudeState == AltitudeState_t::Stabilized) {
    joy->SetLedOFF(4); // DualShock3::led4
    joy->Rumble(0x70);

    altitudeTrajectory->StopTraj();
    joy->SetZRef(0);
    altitudeTrajectory->StartTraj(
        currentAltitude,
        desiredLandingAltitude->Value()); // shouldn't it be groundAltitude?
    SignalEvent(Event_t::StartLanding);
    altitudeState = AltitudeState_t::StartLanding;
  } else {
    joy->ErrorNotify();
  }
}

void UavStateMachine::SignalEvent(Event_t event) {
  switch (event) {
  case Event_t::StartLanding:
    Thread::Info("Altitude: entering 'StartLanding' state\n");
    break;
  case Event_t::Stopped:
    Thread::Info("Altitude: entering 'Stopped' state\n");
    break;
  case Event_t::TakingOff:
    Thread::Info("Altitude: taking off\n");
    break;
  case Event_t::Stabilized:
    Thread::Info("Altitude: entering 'Stabilized' state\n");
    break;
  case Event_t::FinishLanding:
    Thread::Info("Altitude: entering 'FinishLanding' state\n");
    break;
  case Event_t::EmergencyStop:
    Thread::Info("Emergency stop!\n");
    break;
  }
}

void UavStateMachine::EmergencyStop(void) {
  if (altitudeState != AltitudeState_t::Stopped) {
    StopMotors();
    EnterFailSafeMode();
    joy->Rumble(0x70);
    SignalEvent(Event_t::EmergencyStop);
  }
}

void UavStateMachine::StopMotors(void) {
  joy->FlashLed(1, 10, 10); // DualShock3::led1
  uav->GetBldc()->SetEnabled(false);
  uav->GetUavMultiplex()->UnlockUserInterface();
  SignalEvent(Event_t::Stopped);
  altitudeState = AltitudeState_t::Stopped;
  uav->GetAhrs()->UnlockUserInterface();

  uZ->SetValues(0, 0);
  uZ->Reset();
}

GridLayout *UavStateMachine::GetButtonsLayout(void) const {
  return buttonslayout;
}

void UavStateMachine::SecurityCheck(void) {
  MandatorySecurityCheck();
  ExtraSecurityCheck();
}

void UavStateMachine::MandatorySecurityCheck(void) {
  if (getFrameworkManager()->ConnectionLost() && !flagConnectionLost) {
    flagConnectionLost = true;
    Thread::Err("Connection lost\n");
    EnterFailSafeMode();
    if (altitudeState == AltitudeState_t::Stopped) {
      SafeStop();
    } else {
      Land();
    }
  }
  if ((altitudeState == AltitudeState_t::TakingOff ||
       altitudeState == AltitudeState_t::Stabilized) &&
      uav->GetBatteryMonitor()->IsBatteryLow() && !flagBatteryLow) {
    flagBatteryLow = true;
    Printf("Battery Low\n");
    EnterFailSafeMode();
    Land();
  }
}

void UavStateMachine::CheckJoystick(void) {
  GenericCheckJoystick();
  ExtraCheckJoystick();
}

void UavStateMachine::GenericCheckJoystick(void) {
  static bool isEmergencyStopButtonPressed = false;
  static bool isTakeOffButtonPressed = false;
  static bool isSafeModeButtonPressed = false;

  if (joy->IsButtonPressed(1)) { // select
    if (!isEmergencyStopButtonPressed) {
      isEmergencyStopButtonPressed = true;
      Thread::Info("Emergency stop from joystick\n");
      EmergencyStop();
    }
  } else
    isEmergencyStopButtonPressed = false;

  if (joy->IsButtonPressed(0)) { // start
    if (!isTakeOffButtonPressed) {
      isTakeOffButtonPressed = true;
      switch (altitudeState) {
      case AltitudeState_t::Stopped:
        TakeOff();
        break;
      case AltitudeState_t::Stabilized:
        Land();
        break;
      default:
        joy->ErrorNotify();
        break;
      }
    }
  } else
    isTakeOffButtonPressed = false;

  // cross
  // gsanahuj:conflict with Majd programs.
  // check if l1,l2,r1 and r2 are not pressed
  // to allow a combination in user program
  if (joy->IsButtonPressed(5) && !joy->IsButtonPressed(6) &&
      !joy->IsButtonPressed(7) && !joy->IsButtonPressed(9) &&
      !joy->IsButtonPressed(10)) {
    if (!isSafeModeButtonPressed) {
      isSafeModeButtonPressed = true;
      Thread::Info("Entering fail safe mode\n");
      EnterFailSafeMode();
    }
  } else
    isSafeModeButtonPressed = false;
}

void UavStateMachine::CheckPushButton(void) {
  GenericCheckPushButton();
  ExtraCheckPushButton();
}

void UavStateMachine::GenericCheckPushButton(void) {
  if (button_kill->Clicked() == true)
    SafeStop();
  if (button_take_off->Clicked() == true)
    TakeOff();
  if (button_land->Clicked() == true)
    Land();
  if (button_start_log->Clicked() == true)
    getFrameworkManager()->StartLog();
  if (button_stop_log->Clicked() == true)
    getFrameworkManager()->StopLog();
}

void UavStateMachine::EnterFailSafeMode(void) {
  SetAltitudeMode(AltitudeMode_t::Manual);
  SetOrientationMode(OrientationMode_t::Manual);
  SetThrustMode(ThrustMode_t::Default);
  SetTorqueMode(TorqueMode_t::Default);

  GetDefaultOrientation()->GetQuaternionAndAngularRates(currentQuaternion,
                                                        currentAngularSpeed);
  joy->SetYawRef(currentQuaternion);
  uYaw->Reset();
  uPitch->Reset();
  uRoll->Reset();

  failSafeMode = true;
  SignalEvent(Event_t::EnteringFailSafeMode);
}

bool UavStateMachine::ExitFailSafeMode(void) {
  // only exit fail safe mode if in Stabilized altitude state
  // gsanahuj: pour la demo inaugurale on ne peut pas etre en failsafe
  // le ruban perturbe l'us
  /*
    if (altitudeState!=AltitudeState_t::Stabilized) {
        return false;
    } else*/ {
    failSafeMode = false;
    return true;
  }
}

bool UavStateMachine::SetTorqueMode(TorqueMode_t const &newTorqueMode) {
  if ((newTorqueMode == TorqueMode_t::Custom) && (failSafeMode)) {
    if (!ExitFailSafeMode())
      return false;
  }
  // When transitionning from Custom to Default torque mode, we should reset the
  // default control laws
  if ((torqueMode == TorqueMode_t::Custom) &&
      (newTorqueMode == TorqueMode_t::Default)) {
    uYaw->Reset();
    uPitch->Reset();
    uRoll->Reset();
  }
  torqueMode = newTorqueMode;
  return true;
}

bool UavStateMachine::SetAltitudeMode(AltitudeMode_t const &newAltitudeMode) {
  if ((newAltitudeMode == AltitudeMode_t::Custom) && (failSafeMode)) {
    if (!ExitFailSafeMode())
      return false;
  }
  altitudeMode = newAltitudeMode;
  GotoAltitude(desiredTakeoffAltitude->Value());

  return true;
}

bool UavStateMachine::GotoAltitude(float desiredAltitude) {
  if (altitudeMode != AltitudeMode_t::Manual) {
    return false;
  }
  altitudeTrajectory->StartTraj(uav->GetMetaUsRangeFinder()->z(),
                                desiredAltitude);
  return true;
}

bool UavStateMachine::SetOrientationMode(
    OrientationMode_t const &newOrientationMode) {
  if ((newOrientationMode == OrientationMode_t::Custom) && (failSafeMode)) {
    if (!ExitFailSafeMode())
      return false;
  }
  // When transitionning from Custom to Manual mode we must reset to yaw
  // reference to the current absolute yaw angle,
  // overwise the Uav will abruptly change orientation
  if ((orientationMode == OrientationMode_t::Custom) &&
      (newOrientationMode == OrientationMode_t::Manual)) {
    joy->SetYawRef(currentQuaternion);
  }
  orientationMode = newOrientationMode;
  return true;
}

bool UavStateMachine::SetThrustMode(ThrustMode_t const &newThrustMode) {
  if ((newThrustMode == ThrustMode_t::Custom) && (failSafeMode)) {
    if (!ExitFailSafeMode())
      return false;
  }
  thrustMode = newThrustMode;
  return true;
}