/home/toby/moos-ivp/MOOS-2375-Oct0611/NavigationAndControl/MOOSTaskLib/PilotTask.cpp

Go to the documentation of this file.

//
//   MOOS - Mission Oriented Operating Suite 
//  
//   A suit of Applications and Libraries for Mobile Robotics Research 
//   Copyright (C) 2001-2005 Massachusetts Institute of Technology and 
//   Oxford University. 
//    
//   This software was written by Paul Newman and others
//   at MIT 2001-2002 and Oxford University 2003-2005.
//   email: pnewman@robots.ox.ac.uk. 
//      
//   This file is part of a  MOOS Basic (Common) Application. 
//        
//   This program is free software; you can redistribute it and/or 
//   modify it under the terms of the GNU General Public License as 
//   published by the Free Software Foundation; either version 2 of the 
//   License, or (at your option) any later version. 
//          
//   This program is distributed in the hope that it will be useful, 
//   but WITHOUT ANY WARRANTY; without even the implied warranty of 
//   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 
//   General Public License for more details. 
//            
//   You should have received a copy of the GNU General Public License 
//   along with this program; if not, write to the Free Software 
//   Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 
//   02111-1307, USA. 
//
// PilotTask.cpp: implementation of the CPilotTask class.
//

#include "PilotTask.h"
#define SET_POINT_TIMEOUT 20.0

// Construction/Destruction
#define DEFAULT_SPEED_TO_THRUST_FACTOR 30.0
CPilotTask::CPilotTask()
{
    m_bInitialised = false;
    m_dfSpeedToThrustFactor = DEFAULT_SPEED_TO_THRUST_FACTOR;
}

CPilotTask::~CPilotTask()
{

}

//returns false if we haven't received data in a while..bad news!
bool CPilotTask::RegularMailDelivery(double dfTimeNow)
{
      if(m_YawDOF.IsStale(dfTimeNow,GetStartTime()))
      {
          DebugNotify(MOOSFormat("%s Not Rx'ing NAV_YAW\n",GetName().c_str()));
          return false;
      }
      if(m_DepthDOF.IsStale(dfTimeNow,GetStartTime()))
      {
          DebugNotify(MOOSFormat("%s Not Rx'ing NAV_DEPTH\n",GetName().c_str()));
          return false;
      }
      if(m_YawSetPoint.IsStale(dfTimeNow,GetStartTime(),SET_POINT_TIMEOUT))
      {
          DebugNotify(MOOSFormat("%s Not Rx'ing PILOT_DESIRED_YAW for more than %f s\n",GetName().c_str(),SET_POINT_TIMEOUT));
          return false;
      }
      if(m_SpeedSetPoint.IsStale(dfTimeNow,GetStartTime(),SET_POINT_TIMEOUT))
      {
          DebugNotify(MOOSFormat("%s Not Rx'ing PILOT_DESIRED_SPEED for more than %f s\n",GetName().c_str(),SET_POINT_TIMEOUT));
          return false;
      }
      if(m_DepthSetPoint.IsStale(dfTimeNow,GetStartTime(),SET_POINT_TIMEOUT))
      {
          DebugNotify(MOOSFormat("%s Not Rx'ing PILOT_DESIRED_DEPTH for more than %f s\n",GetName().c_str(),SET_POINT_TIMEOUT));
          return false;
      }



      return true;

}

bool CPilotTask::GetNotifications(MOOSMSG_LIST & List)
{
   if(m_bActive)
   {
   }
   
   return CMOOSBehaviour::GetNotifications(List);
}


bool CPilotTask::Run(CPathAction &DesiredAction)
{

    if(!m_bInitialised)
    {
        Initialise();
    }
    
    if(m_YawDOF.IsValid())
    {

        double dfError = MOOS_ANGLE_WRAP(m_YawDOF.GetError());

        double dfCmd= 0;


        // 1 RUDDER CONTROL
        //this is for loggin purposes only
        m_YawPID.SetGoal(m_YawDOF.GetDesired());

        if(m_YawPID.Run(dfError,m_YawDOF.GetErrorTime(),dfCmd))
        {

            //OK we need to change something
            DesiredAction.Set(  ACTUATOR_RUDDER,
                                -(dfCmd),
                                m_nPriority,
                                m_sName.c_str());            
            
            //MOOSTrace("just wrote rudder = %f\n",dfCmd);

        }

        //2 THRUST CONTROL
        if(m_SpeedSetPoint.IsValid())
        {
            //OK we need to change something
            DesiredAction.Set(  ACTUATOR_THRUST,
                                m_SpeedSetPoint.GetCurrent(),
                                m_nPriority,
                                m_sName.c_str());                

            //MOOSTrace("just wrote thrust = %f\n",m_ThrustSetPoint.GetCurrent());

        }

        //3 ELEVATOR CONTROL
        if(m_DepthDOF.IsValid() && m_PitchDOF.IsValid())
        {
            double dfError = m_DepthDOF.GetError();
            
            double dfCmd= 0;
            
            //this is for logging purposes only
            m_ZPID.SetGoal(m_DepthDOF.GetDesired());
            
            
            if(m_ZPID.Run(dfError,
                m_DepthDOF.GetErrorTime(),
                dfCmd,m_PitchDOF.GetCurrent(),
                m_PitchDOF.GetErrorTime()))
            {
                //OK we need to change something
                DesiredAction.Set(  ACTUATOR_ELEVATOR,
                    dfCmd,
                    m_nPriority,
                    m_sName.c_str());                

                //MOOSTrace("just wrote elevator = %f\n",dfCmd);

            }
        }

    }
    return true;
}

bool CPilotTask::OnNewMail(MOOSMSG_LIST &NewMail)
{
    CMOOSMsg Msg;
    
    //get yaw
    if(PeekMail(NewMail,"NAV_YAW",Msg))
    { 
        if(!Msg.IsSkewed(GetTimeNow()))
        {           
            m_YawDOF.SetCurrent(Msg.GetDouble(),Msg.m_dfTime);
        }
    }

    //depth
    if(PeekMail(NewMail,"NAV_DEPTH",Msg))
    {
        if(!Msg.IsSkewed(GetTimeNow()))
        {
            m_DepthDOF.SetCurrent(Msg.GetDouble(),Msg.m_dfTime);
        }
    }

    //pitch
    if(PeekMail(NewMail,"NAV_PITCH",Msg))
    {
        if(!Msg.IsSkewed(GetTimeNow()))
        {
            m_PitchDOF.SetCurrent(Msg.GetDouble(),Msg.m_dfTime);
        }
    }

    if(PeekMail(NewMail,"PILOT_DESIRED_YAW",Msg))
    {
        if(!Msg.IsSkewed(GetTimeNow()))
        {
            if(MOOSStrCmp(Msg.GetSource(),m_sYawSetPointSourceProcess))
            {
                m_YawDOF.SetDesired(Msg.GetDouble());            
                m_YawSetPoint.SetCurrent(Msg.GetDouble(),Msg.GetTime());
            }
            else
            {
                static double dfT =MOOSTime();
                if(MOOSTime()-dfT>2.0)
                {
                    DebugNotify(MOOSFormat("Process %s : no Pilot task permission",Msg.GetSource().c_str()));
                    dfT = MOOSTime();
                }
            }
        }
    }

    if(PeekMail(NewMail,"PILOT_DESIRED_DEPTH",Msg))
    {
        if(!Msg.IsSkewed(GetTimeNow()))
        {
            if(MOOSStrCmp(Msg.GetSource(),m_sDepthSetPointSourceProcess))
            {
                m_DepthDOF.SetDesired(Msg.GetDouble());            
                m_DepthSetPoint.SetCurrent(Msg.GetDouble(),Msg.GetTime());
            }
            else
            {
                static double dfT =MOOSTime();
                if(MOOSTime()-dfT>2.0)
                {
                    DebugNotify(MOOSFormat("Process %s : no Pilot task permission",Msg.GetSource().c_str()));
                    dfT = MOOSTime();
                }
            }
        }
    }

    if(PeekMail(NewMail,"PILOT_DESIRED_SPEED",Msg))
    {
        if(!Msg.IsSkewed(GetTimeNow()))
        {
            if(MOOSStrCmp(Msg.GetSource(),m_sSpeedSetPointSourceProcess))
            {
                double dfT = MOOSClamp<double>(m_dfSpeedToThrustFactor*Msg.GetDouble(),0,100);
                m_SpeedSetPoint.SetCurrent(dfT,Msg.GetTime());
//                MOOSTrace("Just Set thrust SP to %f\n",Msg.GetDouble());
            }
            else
            {
                static double dfT =MOOSTime();
                if(MOOSTime()-dfT>2.0)
                {

                    DebugNotify(MOOSFormat("Process %s : no Pilot task permission",Msg.GetSource().c_str()));
                    dfT = MOOSTime();
                }
            }
        }
    }

    //always call base class version
    CMOOSBehaviour::OnNewMail(NewMail);

    return true;
}

bool CPilotTask::GetRegistrations(STRING_LIST &List)
{

    List.push_front("NAV_YAW");
    List.push_front("NAV_PITCH");
    List.push_front("NAV_DEPTH");
    List.push_front("PILOT_DESIRED_YAW");
    List.push_front("PILOT_DESIRED_DEPTH");
    List.push_front("PILOT_DESIRED_SPEED");

    //always call base class version
    CMOOSBehaviour::GetRegistrations(List);

    return true;
}


bool CPilotTask::SetParam(string sParam, string sVal)
{
    MOOSToUpper(sParam);
    MOOSToUpper(sVal);


    if(!CMOOSBehaviour::SetParam(sParam,sVal))
    {
        //this is for us...
        if(MOOSStrCmp(sParam,"YawControllingProcess"))
        {
            m_sYawSetPointSourceProcess = sVal;
        }        
        //this is for us...
        else if(MOOSStrCmp(sParam,"SpeedControllingProcess"))
        {
            m_sSpeedSetPointSourceProcess = sVal;
        }        
        //this is for us...
        else if(MOOSStrCmp(sParam,"DepthControllingProcess"))
        {
            m_sDepthSetPointSourceProcess = sVal;
        }            
        else if(MOOSStrCmp(sParam,"SpeedToThrustFactor"))
        {
            m_dfSpeedToThrustFactor = atof(sVal.c_str());
        }
        else
        {
            //hmmm - it wasn't for us at all: base class didn't understand either
            MOOSTrace("Param \"%s\" not understood!\n",sParam.c_str());
            return false;
        }
    }

    return true;

}

bool CPilotTask::Initialise()
{
    if(m_sYawSetPointSourceProcess=="")
    {
        return MOOSFail("%s : \"YawControllingProcess\" not set\n",GetName().c_str());
    }
    
    if(m_sDepthSetPointSourceProcess=="")
    {
        return MOOSFail("%s : \"DepthControllingProcess\" not set\n",GetName().c_str());
    }
    if(m_sSpeedSetPointSourceProcess=="")
    {
        return MOOSFail("%s : \"SpeedControllingProcess\" not set\n",GetName().c_str());
    }



    //set a pitch driven depth controller
    m_ZPID.SetAsDepthController(true);


    m_bInitialised = true;
    return true;
}