#ifndef STATEMASHINE_H
|
#define STATEMASHINE_H
|
|
#include <Arduino.h>
|
#include <map>
|
#include <utility>
|
#include <vector>
|
#include <modules/my_mutex/Mutex.h>
|
#include <modules/my_math/matrix/math.hpp>
|
#include <modules/mavlink/v2.0/ardupilotmega/mavlink.h>
|
#include <modules/my_state_machine/AC_PID_Basic.h>
|
using namespace matrix;
|
|
class StateMachine
|
{
|
public:
|
StateMachine();
|
static void TaskStateMachine(void *pvParameters);
|
enum{
|
StateWait,
|
StateReadWps,
|
StateCalcWp,
|
StateSendWps,
|
StateNavigation,
|
StateTargetAttack,
|
StateTestSrv,
|
StateSafeMode,
|
};
|
enum{
|
CalcErrorNone,
|
CalcErrorDist,
|
CalcErrorAngle,
|
CalcErrorAlt,
|
CalcErrorWpNums,
|
|
};
|
enum{
|
ResContinue,
|
ResSwitchGuided,
|
ResSendControl,
|
ResSwitchRtl,
|
ResSwitchCustomGuided,
|
ResSwitchLoiter,
|
ResSendCamPWM,
|
ResSendCamPWM_Control,
|
ResSwitchSafeMode,
|
ResSendWpAndControl,
|
ResSwitchNavigation,
|
};
|
|
void SetWp(mavlink_mission_item_t wp);
|
mavlink_mission_item_t GetWp(uint16_t num);
|
uint16_t GetWpCount(){return mWpMaxNum;}
|
void SetWpCount(uint16_t n);
|
void SetWpsSended(bool ok){mWpsSended=ok;}
|
void SetRPY(float r,float p,float y);
|
void SetPos(float lat,float lon,float alt);
|
void SetCurrWpNum(uint16_t n){mCurrWpNum=n;}
|
void SetBaseMode(uint8_t bm,uint8_t cm){mBaseMode=bm;mCustomMode=cm;}
|
void SetTestMode(uint8_t mode,float alt){mTestMode=mode;mTestAlt=alt;}
|
void SetKpitch(float kpitch){mK_Pitch=kpitch;}
|
void SetK(float k){mMutex.Lock();mYawPID->kP(k);mMutex.Unlock();};
|
void SetI(float k){mMutex.Lock();mYawPID->kI(k);mMutex.Unlock();};
|
void SetD(float k){mMutex.Lock();mYawPID->kD(k);mMutex.Unlock();};
|
void SetImax(float k){mMutex.Lock();mYawPID->imax(k);mMutex.Unlock();};
|
void SetUseKpitch(bool ok){mUseKpitch=ok;}
|
void SetVideoParam(float fovx,float fovy,float w,float h){mFovX=fovx;mFovY=fovy;mWidth=w;mHeight=h;mKx=fovx/w;mKy=fovy/h;}
|
void SetTrackerData(float x,float y,uint8_t v,uint8_t mode,uint16_t cnt);
|
void SetTrackerEnable(bool ok){mEnableTracker=ok;}
|
void SetAttackSpeed(float speed){mAttackSpeed=speed;}
|
void SetCamSrvCH(float ch){mMutex.Lock();mCAM_SRV_CH=ch;mMutex.Unlock();}
|
void SetCamSrvMax(float max){mMutex.Lock();mCAM_SRV_MAX=max;mMutex.Unlock();}
|
void SetCamSrvMin(float min){mMutex.Lock();mCAM_SRV_MIN=min;mMutex.Unlock();}
|
void SetCamAngMax(float max){mMutex.Lock();mCAM_ANG_MAX=max;mMutex.Unlock();}
|
void SetCamAngMin(float min){mMutex.Lock();mCAM_ANG_MIN=min;mMutex.Unlock();}
|
void SetCamAngCruise(float cr){mMutex.Lock();mCAM_ANG_CRUISE=cr;mMutex.Unlock();}
|
void SetFlapRC(float ch){mMutex.Lock();mFLAP_RC=ch;mMutex.Unlock();}
|
void SetCTRLSrvCH(float ch){mMutex.Lock();mCTRL_SRV_CH=ch;mMutex.Unlock();}
|
void SetFlapVAL(float val);
|
void SetTestSrvCH(float ch){mMutex.Lock();mTEST_SRV_CH=ch;mMutex.Unlock();}
|
void SetTestSrvAmp(float a){mMutex.Lock();mTEST_SRV_AMP=a;mMutex.Unlock();}
|
void SetTestSrvT(float t){mMutex.Lock();mTEST_SRV_T=t;mMutex.Unlock();}
|
void SetTestSrvTrim(float trim){mMutex.Lock();mTEST_SRV_TRIM=trim;mMutex.Unlock();}
|
void SetLoiterAlt(float alt){mMutex.Lock();mLoiterAltMin=alt;mMutex.Unlock();}
|
void SetSysID(uint8_t id){mSysID=id;}
|
void SetCamReverse(uint8_t r){mCAM_SRV_REVERSE=r;}
|
void SetSafeSpeed(float spd){mMutex.Lock();mSafeSpeed=spd;mMutex.Unlock();}
|
void SetSafePitch(float p){mMutex.Lock();mSafePitch=p;mMutex.Unlock();}
|
void SetSafeTime(float t){mMutex.Lock();mSafeTime=t;mMutex.Unlock();}
|
void SetCamSrvKI(float ki){mMutex.Lock();mCAM_SRV_KI=ki;mMutex.Unlock();}
|
void SetCamSrvKP(float kp){mMutex.Lock();mCAM_SRV_KP=kp;mMutex.Unlock();}
|
void SetCamPitchDelta(float dp){mMutex.Lock();mCAM_PITCH_DELTA=dp;mMutex.Unlock();}
|
void SetCamRateMax(float rate){mMutex.Lock();mCAM_RATE_MAX=rate*0.033f;mMutex.Unlock();}
|
void SetTecsDalt(float dDlat){mMutex.Lock();mTECS_DALT=dDlat;mMutex.Unlock();}
|
|
private:
|
static int8_t CalckTargetWp(void *pvParameters);
|
uint8_t Correction();
|
uint8_t SafeMode();
|
void CreateMavlink(float r,float p,float y,float X,float Y,float Z,float mode);
|
void Ang2PWM(float v);
|
void CamYTrecker(float camPitch,bool on_attack);
|
enum{
|
AttackNone,
|
AttackWp,
|
AttackWpTracker,
|
AttackTracker,
|
CameraManual
|
};
|
Mutex mMutex;
|
u_int8_t mState;
|
uint16_t mWpMaxNum;
|
uint16_t mCurWpRead=0;
|
uint16_t mWpReadedCount=0;
|
//bool mSystemArmed;
|
//bool mSystemArmedLast;
|
bool mWpsSended;
|
std::map<uint16_t,mavlink_mission_item_t> mWpsMap;
|
float mRoll,mPitch,mYaw;
|
float mLat,mLon,mAlt;
|
uint16_t mCurrWpNum=0;
|
mavlink_mission_item_t WpD,TargetWp,GuidedWp,CustomGuidedWp;
|
float mTanWp=0;
|
float mAngleWp=0;
|
int16_t mX,mY,mZ,mR;
|
uint8_t mBaseMode=0;
|
uint8_t mCustomMode=0;
|
uint8_t mTestMode=0;
|
float mTestAlt=100;
|
float mQ0,mQ1,mQ2,mQ3;
|
float mK_Yaw=2.0;
|
float mI_Yaw=0.0;
|
float mD_Yaw=0.0;
|
float mI_Max=0;
|
float mK_Pitch=1.5;
|
float mAltError=0;
|
float mDistans=0;
|
uint8_t mTargetWpNum=0;
|
bool mUseKpitch=0.0;
|
float mFovX,mFovY,mWidth,mHeight,mKx,mKy;
|
float mTrackerX=0;
|
float mTrackerY=0;
|
uint8_t mTrackerValid=0;
|
uint8_t mTrackerMode=0;
|
bool mNewTrackerData=false;
|
bool mUseTracker=false;
|
bool mUseCustomTrackerMode=false;
|
bool mEnableTracker=false;
|
float mCam_yaw=0;
|
float mCam_pitch=0;
|
float mPitchError=0;
|
float mYawError=0;
|
float mKpitcKorr=0;
|
uint16_t mTrackerMsgCnt=0;
|
float mLoiterAlt=100;
|
float mAttackSpeed=30.0;
|
float mSafeSpeed=30.0;
|
float mSafePitch=0;
|
float mSafeTime=10;
|
uint8_t mBufferOutStream[MAVLINK_MAX_PACKET_LEN];
|
bool mMavlinkNew=false;
|
int mSendLen=0;
|
uint8_t mAttackMode=0;
|
AC_PID_Basic *mYawPID;
|
uint8_t mCAM_SRV_CH=0;
|
uint8_t mCTRL_SRV_CH=0;
|
float mCAM_SRV_MAX;
|
float mCAM_SRV_MIN;
|
float mCAM_ANG_CRUISE;
|
float mCAM_ANG_MAX;
|
float mCAM_ANG_MIN;
|
float mCAM_TARGET=-500.0f;
|
float mCAM_CURRENT=-500.0f;
|
float mCAM_SRV_KI=0.1;
|
float mCAM_SRV_KP=0.5;
|
float mCAM_PITCH_DELTA=5.0;
|
float mCAM_RATE_MAX=45.0*0.033f;
|
uint8_t mCAM_SRV_REVERSE=0;
|
bool mNewCamData=false;
|
uint16_t mCamPWM=0;
|
float mRollMax=40;
|
uint8_t mFLAP_RC=0;
|
uint16_t mFLAP_VAL=0;
|
uint16_t mFLAP_PWM=0;
|
bool mUnlock=false;
|
uint8_t mTEST_SRV_CH=0;
|
u_int16_t mTEST_SRV_AMP=0;
|
float mTEST_SRV_T=0;
|
u_int16_t mTEST_SRV_TRIM=1500;
|
float mLoiterAltMin=100;
|
uint8_t mSysID=0;
|
uint8_t mSysID_old=0;
|
bool mSendPWM=false;
|
bool mEnterSafeMode=true;
|
float mGuidedAlt=1.0f;
|
float mTECS_DALT=10.0f;
|
};
|
#endif
|
