#include "StateMachine.h"
|
#include <modules/my_task_creator/TaskCreator.h>
|
void StateMachine::TaskStateMachine(void *pvParameters)
|
{
|
log_i("Task started");
|
vTaskDelay(100);
|
TaskCreator *mCreator = (TaskCreator *)pvParameters;
|
StateMachine *obj=&(mCreator->mStateMachine);
|
MavlinkTransmitter *mTransmitter=&(mCreator->mTransmitter);
|
DefaultParametrs *mParams=&(mCreator->mParams);
|
TrackerRxTx *mTracker=&(mCreator->mTracker);
|
TickType_t mStartTick,mStopTick;//=xTaskGetTickCount();
|
TickType_t mLastTick=xTaskGetTickCount();
|
TickType_t mLastTickGuided=mLastTick;
|
TickType_t mLastTickCalc=mLastTick;
|
String ModeName;
|
obj->mState=StateWait;
|
while (1)
|
{
|
TickType_t mCurrentTick=xTaskGetTickCount();
|
switch (obj->mState)
|
{
|
case StateWait:
|
{
|
ModeName="Wait";
|
|
uint16_t n=obj->GetWpCount();
|
if(obj->mSysID)
|
{
|
if(obj->mSysID!=obj->mSysID_old)
|
{
|
mTransmitter->SendText(SOFT_VER,MAV_SEVERITY_NOTICE);
|
obj->mSysID_old=obj->mSysID;
|
uint8_t m=mParams->GetValue("CTRL_TEST_MODE");
|
float alt=mParams->GetValue("CTRL_TEST_ALT");
|
obj->SetTestMode(m,alt);
|
float ky=mParams->GetValue("CTRL_K_YAW");
|
float ki=mParams->GetValue("CTRL_I_YAW");
|
float kd=mParams->GetValue("CTRL_D_YAW");
|
float kimax=mParams->GetValue("CTRL_I_MAX");
|
float kp=mParams->GetValue("CTRL_K_PITCH");
|
bool use=mParams->GetValue("CTRL_USE_KPITCH");
|
float speed=mParams->GetValue("CTRL_ATTACK_SPD");
|
obj->SetCamSrvCH(mParams->GetValue("CAM_SRV_CH"));
|
obj->SetCTRLSrvCH(mParams->GetValue("CTRL_SRV_CH"));
|
obj->SetCamSrvMax(mParams->GetValue("CAM_SRV_MAX"));
|
obj->SetCamSrvMin(mParams->GetValue("CAM_SRV_MIN"));
|
obj->SetCamAngCruise(mParams->GetValue("CAM_ANG_CRUISE"));
|
obj->SetCamAngMax(mParams->GetValue("CAM_ANG_MAX"));
|
obj->SetCamAngMin(mParams->GetValue("CAM_ANG_MIN"));
|
obj->SetFlapRC(mParams->GetValue("CTRL_FLAP_RC"));
|
obj->SetFlapVAL(mParams->GetValue("CTRL_FLAP_VAL"));
|
|
obj->SetTestSrvCH(mParams->GetValue("TEST_SRV_CH"));
|
obj->SetTestSrvAmp(mParams->GetValue("TEST_SRV_AMP"));
|
obj->SetTestSrvT(mParams->GetValue("TEST_SRV_T"));
|
obj->SetTestSrvTrim(mParams->GetValue("TEST_SRV_TRIM"));
|
obj->SetCamReverse(mParams->GetValue("CAM_SRV_REVERSE"));
|
|
obj->SetK(ky);
|
obj->SetI(ki);
|
obj->SetD(kd);
|
obj->SetImax(kimax);
|
obj->SetKpitch(kp);
|
obj->SetUseKpitch(use);
|
obj->SetVideoParam(mParams->GetValue("VIDEO_FOVX"),mParams->GetValue("VIDEO_FOVY"),
|
mParams->GetValue("VIDEO_WIDTH"),mParams->GetValue("VIDEO_HEIGHT"));
|
obj->SetTrackerEnable(mParams->GetValue("VIDEO_ENABLE"));
|
obj->SetAttackSpeed(speed);
|
obj->SetLoiterAlt(mParams->GetValue("CTRL_LOITER_ALT"));
|
obj->SetSafeSpeed(mParams->GetValue("CTRL_SAFE_SPD"));
|
obj->SetSafePitch(mParams->GetValue("CTRL_SAFE_PITCH"));
|
obj->SetSafeTime(mParams->GetValue("CTRL_SAFE_TIME"));
|
obj->SetCamSrvKI(mParams->GetValue("CAM_SRV_KI"));
|
obj->SetCamSrvKP(mParams->GetValue("CAM_SRV_KP"));
|
obj->SetCamPitchDelta(mParams->GetValue("CAM_PITCH_DELTA"));
|
obj->SetCamRateMax(mParams->GetValue("CAM_RATE_MAX"));
|
obj->SetTecsDalt(mParams->GetValue("CTRL_TECS_DALT"));
|
if(mParams->GetValue("CTRL_TWP_NUM")==-1)
|
{
|
mTransmitter->SendMissionReq();
|
mStartTick=xTaskGetTickCount();
|
log_i("Send MissionReq");
|
vTaskDelay(50);
|
}
|
}
|
else
|
{
|
if(mParams->GetValue("CTRL_TWP_NUM")==-1)
|
{
|
obj->mTargetWpNum=0;
|
|
mTransmitter->SubscribeMsg(MAVLINK_MSG_ID_ATTITUDE,20000);
|
vTaskDelay(10);
|
mTransmitter->SubscribeMsg(MAVLINK_MSG_ID_GLOBAL_POSITION_INT,20000);
|
vTaskDelay(10);
|
mTransmitter->SubscribeMsg(MAVLINK_MSG_ID_RC_CHANNELS,100000);
|
vTaskDelay(10);
|
if(obj->mTEST_SRV_CH)
|
{
|
obj->mState=StateTestSrv;
|
mTransmitter->SendCamPWM(obj->mTEST_SRV_CH,obj->mTEST_SRV_TRIM);
|
log_i("State chnged to StateTestSrv");
|
vTaskDelay(1000);
|
}
|
else
|
{
|
obj->mState=StateNavigation;
|
log_i("State chnged to StateNavigation");
|
}
|
break;
|
}
|
mStopTick=xTaskGetTickCount();
|
if(mStopTick-mStartTick>500)
|
{
|
mTransmitter->SendMissionReq();
|
mStartTick=xTaskGetTickCount();
|
log_i("Resend MissionReq");
|
}
|
vTaskDelay(50);
|
}
|
if(n>0)
|
{
|
obj->mState=StateReadWps;
|
obj->mCurWpRead=0;
|
log_i("Change state to StateReadWps");
|
vTaskDelay(50);
|
}
|
}
|
else
|
{
|
//log_i("Wait arm");
|
vTaskDelay(1000);
|
}
|
|
}
|
break;
|
case StateReadWps:
|
{
|
ModeName="ReadWps";
|
if(obj->mCurWpRead==obj->mWpReadedCount)
|
{
|
mTransmitter->SendRequestMissionItem(obj->mCurWpRead);
|
log_i("Send req get wp num=%d",obj->mCurWpRead);
|
vTaskDelay(100);
|
}
|
else
|
{
|
obj->mCurWpRead++;
|
if(obj->mCurWpRead>obj->mWpMaxNum)
|
{
|
obj->mCurWpRead--;
|
if(mParams->GetValue("CTRL_WP_REDY")>0)
|
{
|
|
obj->WpD=obj->mWpsMap[obj->mWpMaxNum-1];
|
|
obj->TargetWp=obj->mWpsMap[obj->mWpMaxNum];
|
obj->mTargetWpNum=obj->mWpMaxNum;
|
|
mTransmitter->SubscribeMsg(MAVLINK_MSG_ID_ATTITUDE,20000);
|
vTaskDelay(10);
|
mTransmitter->SubscribeMsg(MAVLINK_MSG_ID_GLOBAL_POSITION_INT,20000);
|
vTaskDelay(10);
|
mTransmitter->SubscribeMsg(MAVLINK_MSG_ID_RC_CHANNELS,100000);
|
vTaskDelay(10);
|
if(obj->mTEST_SRV_CH)
|
{
|
obj->mState=StateTestSrv;
|
mTransmitter->SendCamPWM(obj->mTEST_SRV_CH,obj->mTEST_SRV_TRIM);
|
log_i("State chnged to StateTestSrv");
|
vTaskDelay(1000);
|
}
|
else
|
{
|
obj->mState=StateNavigation;
|
log_i("State chnged to StateNavigation");
|
}
|
}
|
else
|
obj->mState=StateCalcWp;
|
|
log_i("All wp readed nums=%d",obj->mWpMaxNum);
|
}
|
vTaskDelay(10);
|
}
|
}
|
break;
|
case StateCalcWp:
|
{
|
ModeName="CalcWp";
|
uint8_t error=obj->CalckTargetWp(pvParameters);
|
if(error==StateMachine::CalcErrorNone)
|
{
|
obj->mTargetWpNum=obj->TargetWp.seq;
|
mTransmitter->SendMissionCount();
|
obj->mState=StateSendWps;
|
log_i("State changed to StateSendWps");
|
}
|
else
|
{
|
if(obj->mTEST_SRV_CH)
|
{
|
obj->mState=StateTestSrv;
|
mTransmitter->SendCamPWM(obj->mTEST_SRV_CH,obj->mTEST_SRV_TRIM);
|
log_i("State chnged to StateTestSrv");
|
vTaskDelay(1000);
|
}
|
else
|
{
|
obj->mState=StateNavigation;
|
log_i("State chnged to StateNavigation");
|
}
|
}
|
vTaskDelay(10);
|
}
|
break;
|
case StateSendWps:
|
{
|
ModeName="SendWps";
|
if(obj->mWpsSended)
|
{
|
|
log_i("State chnged to StateNavigation");
|
mTransmitter->SubscribeMsg(MAVLINK_MSG_ID_GLOBAL_POSITION_INT,20000);
|
vTaskDelay(50);
|
mTransmitter->SubscribeMsg(MAVLINK_MSG_ID_ATTITUDE,20000);
|
vTaskDelay(50);
|
mTransmitter->SubscribeMsg(MAVLINK_MSG_ID_RC_CHANNELS,50000);
|
vTaskDelay(50);
|
obj->mState=StateNavigation;
|
}
|
vTaskDelay(50);
|
}
|
break;
|
case StateNavigation:
|
{
|
ModeName="Navigation";
|
static uint16_t cnt=0;
|
if(mTracker->isNewData())
|
{
|
uint16_t x,y,count;
|
uint8_t v,mode;
|
mTracker->GetTrackerData(x,y,v,mode,count);
|
obj->SetTrackerData(x,y,v,mode,count);
|
}
|
if(mTracker->isNewCamData())
|
{
|
float val=mTracker->GetCamData();
|
obj->mCAM_TARGET=val;
|
obj->mNewCamData=true;
|
}
|
uint8_t res=obj->Correction();
|
switch (res)
|
{
|
case ResContinue:
|
mTracker->SetMavlikBuf(obj->mBufferOutStream,obj->mSendLen);
|
break;
|
case ResSwitchGuided:
|
{
|
mTransmitter->SendGuidedMode(obj->mBaseMode,obj->GuidedWp);
|
log_i("Set guided mode");
|
vTaskDelay(50);
|
mTransmitter->SendGuidedSpeed(obj->mAttackSpeed);
|
}
|
|
break;
|
case ResSendControl:
|
{
|
if(obj->mTECS_DALT>0)
|
mTransmitter->SendGuidedControll(obj->mQ0,obj->mQ1,obj->mQ2,obj->mQ3,obj->mGuidedAlt,true);
|
else
|
mTransmitter->SendGuidedControll(obj->mQ0,obj->mQ1,obj->mQ2,obj->mQ3,1,false);
|
mTracker->SetMavlikBuf(obj->mBufferOutStream,obj->mSendLen);
|
if(obj->mSendPWM)
|
{
|
obj->mSendPWM=false;
|
mTransmitter->SendCamPWM(obj->mCAM_SRV_CH,obj->mCamPWM);
|
}
|
if(obj->mFLAP_RC)
|
{
|
mTransmitter->SendFlapPWM(obj->mFLAP_RC,obj->mFLAP_PWM);
|
}
|
if(obj->mUnlock && obj->mCTRL_SRV_CH)
|
{
|
//Відправка PWM на відкриття замку
|
/*mTransmitter->SendCamPWM(obj->mCTRL_SRV_CH,2000);*/
|
|
mTransmitter->SendCamPWM(obj->mCTRL_SRV_CH,1000);
|
}
|
if((mCurrentTick-mLastTickGuided)>5000)
|
{
|
|
String p(obj->mCam_pitch*180.0/M_PI,1);
|
String y(obj->mCam_yaw*180.0/M_PI,1);
|
String distans(obj->mDistans,1);
|
String use(obj->mUseTracker);
|
String tmp("Nav UseTracker=");
|
tmp+=use;
|
tmp+=String(" d=");
|
tmp+=distans;
|
tmp+=String(" TPitch=");
|
tmp+=p;
|
tmp+=String(" TYaw=");
|
tmp+=y;
|
mTransmitter->SendText(tmp,MAV_SEVERITY_INFO);
|
|
/*String fovx(obj->mFovX,1);
|
String fovy(obj->mFovY,1);
|
String w(obj->mWidth,1);
|
String h(obj->mHeight,1);
|
String kx(obj->mKx,4);
|
String ky(obj->mKy,4);
|
String tmp1("test ");
|
tmp1+=fovx+String(" ")+fovy+String(" ")+w+String(" ")+h+String(" ")+kx+String(" ")+ky;
|
mTransmitter->SendText(tmp1,MAV_SEVERITY_INFO);*/
|
|
|
mLastTickGuided=mCurrentTick;
|
|
}
|
}
|
break;
|
case ResSwitchRtl:
|
{
|
mTransmitter->SendRtl();
|
String pich_error(obj->mPitchError,1);
|
String yaw_error(obj->mYawError,1);
|
String distans(obj->mDistans,1);
|
String tmp("NavEnd d=");
|
tmp+=distans;
|
tmp+=String(" pe=");
|
tmp+=pich_error;
|
tmp+=String(" ye=");
|
tmp+=yaw_error;
|
mTransmitter->SendText(tmp,MAV_SEVERITY_INFO);
|
log_i("Set rtl mode");
|
vTaskDelay(1000);
|
mTransmitter->SendText("Set rtl mode",MAV_SEVERITY_INFO);
|
}
|
break;
|
case ResSwitchCustomGuided:
|
{
|
mTransmitter->SendGuidedMode(obj->mBaseMode,obj->CustomGuidedWp);
|
log_i("Set guided mode");
|
vTaskDelay(50);
|
mTransmitter->SendGuidedSpeed(obj->mAttackSpeed);
|
}
|
break;
|
case ResSwitchLoiter:
|
{
|
mTransmitter->SendLoiter(obj->mLoiterAlt);
|
if(obj->mFLAP_RC)
|
{
|
mTransmitter->SendFlapPWM(obj->mFLAP_RC,1000);
|
}
|
vTaskDelay(50);
|
//mTransmitter->SendLoiterSpeed(obj->mLoiterSpeed);
|
log_i("Set loiter mode");
|
String pich_error(obj->mPitchError,1);
|
String yaw_error(obj->mYawError,1);
|
String distans(obj->mDistans,1);
|
String tmp("NavEnd d=");
|
tmp+=distans;
|
tmp+=String(" pe=");
|
tmp+=pich_error;
|
tmp+=String(" ye=");
|
tmp+=yaw_error;
|
mTransmitter->SendText(tmp,MAV_SEVERITY_INFO);
|
obj->mUnlock=false;
|
vTaskDelay(1000);
|
}
|
break;
|
case ResSendCamPWM:
|
{
|
mTransmitter->SendCamPWM(obj->mCAM_SRV_CH,obj->mCamPWM);
|
log_i("SendCamPWM pwm=%d ang=%.2f",obj->mCamPWM,obj->mCAM_CURRENT);
|
}
|
break;
|
case ResSwitchSafeMode:
|
{
|
obj->mEnterSafeMode=true;
|
obj->mState=StateSafeMode;
|
}
|
break;
|
default:
|
break;
|
}
|
|
|
uint32_t mCurrentTick1=xTaskGetTickCount();
|
uint32_t dt=mCurrentTick1-mCurrentTick;
|
cnt++;
|
if(cnt==500)
|
{
|
String tmp("Correction time=");
|
tmp+=dt;
|
mTransmitter->SendText(tmp,MAV_SEVERITY_INFO);
|
log_i("Correction time=%i",dt);
|
cnt=0;
|
}
|
|
if(dt<20)
|
{
|
vTaskDelay(20-dt);
|
}
|
|
}
|
break;
|
case StateTestSrv:
|
{
|
ModeName="StateTestSrv";
|
static uint64_t cnt=0;
|
if(obj->mTEST_SRV_T<0.1)
|
{
|
log_i("Error value TEST_SRV_T");
|
obj->mTEST_SRV_T=3.0;
|
|
}
|
float om=2.0f*M_PI/obj->mTEST_SRV_T;
|
float t=0.02f*cnt;
|
if(om*t>2.0f*M_PI)
|
cnt=0;
|
cnt++;
|
float val=sin(om*t)*obj->mTEST_SRV_AMP+obj->mTEST_SRV_TRIM;
|
if(val>2000)
|
val=2000;
|
if(val<1000)
|
val=1000;
|
mTransmitter->SendCamPWM(obj->mTEST_SRV_CH,val);
|
vTaskDelay(20);
|
//log_i("SendCamPWM pwm=%d ang=%.2f",obj->mCamPWM,obj->mCAM_CURRENT);
|
}
|
break;
|
case StateSafeMode:
|
{
|
ModeName="SafeMode";
|
uint8_t res=obj->SafeMode();
|
switch (res)
|
{
|
case ResContinue:
|
{
|
vTaskDelay(20);
|
}
|
break;
|
case ResSendWpAndControl:
|
{
|
if(obj->mTECS_DALT>0)
|
mTransmitter->SendGuidedControll(obj->mQ0,obj->mQ1,obj->mQ2,obj->mQ3,obj->mGuidedAlt,true);
|
else
|
mTransmitter->SendGuidedControll(obj->mQ0,obj->mQ1,obj->mQ2,obj->mQ3,1,false);
|
vTaskDelay(10);
|
mTransmitter->SendGuidedMode(obj->mBaseMode,obj->CustomGuidedWp);
|
vTaskDelay(10);
|
mTransmitter->SendGuidedSpeed(obj->mSafeSpeed);
|
vTaskDelay(10);
|
if(obj->mFLAP_RC)
|
{
|
mTransmitter->SendFlapPWM(obj->mFLAP_RC,1000);
|
}
|
vTaskDelay(20);
|
String pich_error(obj->mPitchError,1);
|
String yaw_error(obj->mYawError,1);
|
String distans(obj->mDistans,1);
|
String tmp("NavEndSafe d=");
|
tmp+=distans;
|
tmp+=String(" pe=");
|
tmp+=pich_error;
|
tmp+=String(" ye=");
|
tmp+=yaw_error;
|
mTransmitter->SendText(tmp,MAV_SEVERITY_INFO);
|
log_i("Set safe guided mode");
|
}
|
break;
|
case ResSwitchNavigation:
|
{
|
obj->mState=StateNavigation;
|
log_i("Switch form Safe to Navigation");
|
}
|
break;
|
}
|
}
|
break;
|
default:
|
break;
|
}
|
if((mCurrentTick-mLastTick)>12000)
|
{
|
String tmp("Navmode=");
|
tmp+=ModeName;
|
tmp+=String(" twp=");
|
tmp+=String(obj->mTargetWpNum);
|
tmp+=" TrMsgCnt=";
|
tmp+=String(obj->mTrackerMsgCnt);
|
mTransmitter->SendText(tmp,MAV_SEVERITY_INFO);
|
mLastTick=mCurrentTick;
|
}
|
}
|
|
}
|
StateMachine::StateMachine():mState(StateWait),
|
mWpMaxNum(0),
|
mWpsSended(false)
|
{
|
mYawPID=new AC_PID_Basic(3,0.1,0.01,0,5,0);
|
mYawPID->reset_filter();
|
mYawPID->reset_I();
|
}
|
|
void StateMachine::SetWp(mavlink_mission_item_t wp)
|
{
|
mMutex.Lock();
|
mWpsMap[wp.seq]=wp;
|
mWpReadedCount=wp.seq+1;
|
mMutex.Unlock();
|
//log_i("Readed wp num=%d",wp.seq);
|
}
|
mavlink_mission_item_t StateMachine::GetWp(uint16_t num)
|
{
|
mavlink_mission_item_t wp={0};
|
mMutex.Lock();
|
if(num<mWpsMap.size())
|
wp=mWpsMap[num];
|
mMutex.Unlock();
|
return wp;
|
}
|
|
|
void StateMachine::SetWpCount(uint16_t n)
|
{
|
mWpMaxNum=n;
|
}
|
int8_t StateMachine::CalckTargetWp(void *pvParameters)
|
{
|
#define Rz 6371000.0f
|
TaskCreator *mCreator = (TaskCreator *)pvParameters;
|
StateMachine *obj=&(mCreator->mStateMachine);
|
MavlinkTransmitter *mTransmitter=&(mCreator->mTransmitter);
|
DefaultParametrs *mParams=&(mCreator->mParams);
|
uint16_t MaxWpNum=obj->GetWpCount();
|
if(!MaxWpNum)
|
{
|
log_i("Calc error wps nums=%d",MaxWpNum);
|
return StateMachine::CalcErrorWpNums;
|
}
|
float TaggetWpNumF=mParams->GetValue("CTRL_TWP_NUM");
|
uint16_t TaggetWpNum;
|
if(TaggetWpNumF<0)
|
{
|
TaggetWpNum=MaxWpNum;
|
}
|
else
|
TaggetWpNum=TaggetWpNumF;
|
obj->TargetWp=obj->GetWp(TaggetWpNum);
|
mavlink_mission_item_t Wp2Target=obj->GetWp(TaggetWpNum-1);
|
|
|
float AttackAlt=mParams->GetValue("CTRL_ATTACK_ALT");
|
float AttackDist=mParams->GetValue("CTRL_ATTACK_DIST");
|
|
float Lat0=Wp2Target.x*M_PI/180.0;
|
float Lon0=Wp2Target.y*M_PI/180.0;
|
float Lat1=obj->TargetWp.x*M_PI/180.0;
|
float Lon1=obj->TargetWp.y*M_PI/180.0;
|
float Z1=obj->TargetWp.z;
|
|
float X1=(Lat1-Lat0)*Rz;
|
float Y1=(Lon1-Lon0)*Rz*cos(Lat0);
|
log_i("Target X1=%f Y1=%f Z1=%f",X1,Y1,Z1);
|
|
float lenVn=sqrt(X1*X1+Y1*Y1);
|
float X1n=X1/lenVn;
|
float Y1n=Y1/lenVn;
|
float t1;
|
if(fabs(X1)>0.1)
|
t1=X1/X1n;
|
else
|
t1=Y1/Y1n;
|
float td=t1-AttackDist;
|
log_i("Target t1=%f td=%f ",t1,td);
|
float Xd=X1n*td;
|
float Yd=Y1n*td;
|
float Zd=AttackAlt+Z1;
|
log_i("Attack Xd=%f Yd=%f Zd=%f",Xd,Yd,Zd);
|
|
float LatD=Lat0*180.0/M_PI+Xd*180.0/M_PI/Rz;
|
float LonD=Lon0*180.0/M_PI+Yd*180.0/M_PI/Rz/cos(Lat0);
|
|
obj->WpD=obj->TargetWp;
|
obj->WpD.x=LatD;
|
obj->WpD.y=LonD;
|
obj->WpD.z=Zd;
|
|
obj->TargetWp.seq=obj->TargetWp.seq+1;
|
|
|
obj->SetWp(obj->WpD);
|
|
obj->SetWp(obj->TargetWp);
|
|
obj->mWpMaxNum=obj->TargetWp.seq;
|
mTransmitter->SetWps(obj->mWpsMap);
|
return StateMachine::CalcErrorNone;
|
}
|
|
void StateMachine::SetRPY(float r, float p, float y)
|
{
|
mMutex.Lock();
|
mRoll=r;
|
mPitch=p;
|
mYaw=y;
|
mMutex.Unlock();
|
}
|
|
void StateMachine::SetPos(float lat, float lon, float alt)
|
{
|
mMutex.Lock();
|
mLat=lat;
|
mLon=lon;
|
mAlt=alt;
|
mMutex.Unlock();
|
}
|
|
uint8_t StateMachine::Correction()
|
{
|
static uint32_t cwpn=0;
|
static uint8_t cnt=0;
|
static bool in_corr=false;
|
float Lat0,Lon0,Lat1,Lon1,X1,Y1,D,Alt,yaw,roll,pitch,trackerX,trackerY;
|
float yaw_t;
|
float pitch_q;
|
float roll_q;
|
uint8_t trackerV=0;
|
uint8_t trackerMode=0;
|
bool trackerNew=false;
|
mY=mZ=mR=mX=0;
|
mMutex.Lock();
|
Lat0=mLat*M_PI/180.0;
|
Lon0=mLon*M_PI/180.0;
|
Alt=mAlt;
|
roll=mRoll;
|
pitch=mPitch;
|
yaw=mYaw;
|
if(mNewTrackerData)
|
{
|
trackerX=mTrackerX;
|
trackerY=mTrackerY;
|
trackerV=mTrackerValid;
|
trackerNew=true;
|
trackerMode=mTrackerMode;
|
|
mNewTrackerData=false;
|
|
}
|
mMutex.Unlock();
|
if(mUseCustomTrackerMode)
|
{
|
if(mCustomMode==PLANE_MODE_RTL || mCustomMode==PLANE_MODE_LOITER)
|
{
|
mUseCustomTrackerMode=false;
|
return ResContinue;
|
}
|
if(mTrackerMode==1)
|
{
|
if(mCustomMode!=PLANE_MODE_GUIDED)
|
{
|
mUseCustomTrackerMode=false;
|
mUseTracker=false;
|
return ResContinue;
|
}
|
if(mTestMode)
|
{
|
if(Alt<mTestAlt+20 && mCTRL_SRV_CH)
|
{
|
mUnlock=true;
|
}
|
if(Alt<mTestAlt)
|
{
|
log_i("Switch to loiter test alt");
|
mUseCustomTrackerMode=false;
|
mUseTracker=false;
|
|
if(Alt<mLoiterAltMin)
|
mLoiterAlt=mLoiterAltMin;
|
else
|
mLoiterAlt=Alt;
|
return ResSwitchLoiter;
|
}
|
}
|
if(trackerV)
|
mUseTracker=true;
|
if(trackerV&&trackerNew)
|
{
|
float camYaw=(trackerX-mWidth/2.0)*mKx;
|
float camPitch=(mHeight/2.0-trackerY)*mKy;
|
Dcm<float> mPlaneM=Dcm<float>(Euler<float>(roll,pitch,yaw));
|
Dcm<float> mCameraM;
|
if(mCAM_SRV_CH)
|
{
|
mCameraM=Dcm<float>(Euler<float>(0,(camPitch-mCAM_CURRENT)*M_PI/180,camYaw*M_PI/180));
|
CamYTrecker(camPitch,true);
|
}
|
else
|
{
|
mCameraM=Dcm<float>(Euler<float>(0,camPitch*M_PI/180,camYaw*M_PI/180));
|
}
|
Dcm<float> mToNED=mPlaneM*mCameraM;
|
Euler<float> ang=Euler<float>(mToNED);
|
mCam_pitch=ang.theta();
|
mCam_yaw=ang.psi();
|
}
|
yaw_t=mCam_yaw;
|
pitch_q=mCam_pitch;
|
|
mMutex.Lock();
|
roll_q=mYawPID->update_all(yaw_t,yaw,0.02,false)*M_PI/180.0;
|
mMutex.Unlock();
|
mYawError=mYawPID->get_error();
|
if(fabs(pitch_q-pitch)*180/M_PI>mCAM_PITCH_DELTA)
|
{
|
mRollMax=5;
|
mYawPID->reset_I();
|
}
|
else
|
{
|
if(mCAM_CURRENT>0 && mCAM_CURRENT<1.0f && mCAM_SRV_CH)
|
{
|
Ang2PWM(0);
|
mCAM_CURRENT=0;
|
mSendPWM=true;
|
}
|
mRollMax=40;
|
}
|
if(roll_q>mRollMax*M_PI/180.0)
|
roll_q=mRollMax*M_PI/180.0;
|
if(roll_q<-mRollMax*M_PI/180.0)
|
roll_q=-mRollMax*M_PI/180.0;
|
cnt++;
|
if(cnt==1000)
|
{
|
log_i("Use mode custom tracker=%d target yaw=%f,pitch=%f",mUseTracker,yaw_t*180.0/M_PI,pitch_q*180.0/M_PI);
|
cnt=0;
|
}
|
float phi=roll_q;
|
float theta=pitch_q;
|
float psi=0;
|
Eulerf et(phi,theta,psi);
|
Quatf qt(et);
|
mQ0=qt(0);
|
mQ1=qt(1);
|
mQ2=qt(2);
|
mQ3=qt(3);
|
mAttackMode=AttackTracker;
|
CreateMavlink(roll_q,pitch_q,yaw_t,0,0,0,mAttackMode);
|
mGuidedAlt=Alt-mTECS_DALT;
|
return ResSendControl;
|
}
|
else
|
{
|
log_i("Switch to loiter mode");
|
mUseCustomTrackerMode=false;
|
if(Alt<mLoiterAltMin)
|
mLoiterAlt=mLoiterAltMin;
|
else
|
mLoiterAlt=Alt;
|
|
mUseTracker=false;
|
return ResSwitchLoiter;
|
}
|
}
|
else
|
{
|
if(trackerMode==1 && trackerV && mCustomMode!=PLANE_MODE_RTL && mCustomMode!=PLANE_MODE_LOITER)
|
{
|
|
mUseTracker=true;
|
mUseCustomTrackerMode=true;
|
float camYaw=(trackerX-mWidth/2.0)*mKx;
|
float camPitch=(mHeight/2.0-trackerY)*mKy;
|
Dcm<float> mPlaneM=Dcm<float>(Euler<float>(roll,pitch,yaw));
|
Dcm<float> mCameraM;
|
if(mCAM_SRV_CH)
|
{
|
mCameraM=Dcm<float>(Euler<float>(0,(camPitch-mCAM_CURRENT)*M_PI/180,camYaw*M_PI/180));
|
}
|
else
|
{
|
mCameraM=Dcm<float>(Euler<float>(0,camPitch*M_PI/180,camYaw*M_PI/180));
|
}
|
Dcm<float> mToNED=mPlaneM*mCameraM;
|
Euler<float> ang=Euler<float>(mToNED);
|
mCam_pitch=ang.theta();
|
mCam_yaw=ang.psi();
|
Vector3f n(1,0,0);
|
Vector3f n1=mToNED*n;
|
if(fabs(n1(2))<1e-5)
|
{
|
/*mUseCustomTrackerMode=false;
|
log_i("Tracker erro guided point theta");
|
if(trackerV)
|
mUseTracker=false;
|
return ResContinue;*/
|
float X=cos(yaw)*1000.0f;
|
float Y=sin(yaw)*1000.0f;
|
float LatT=Lat0*180.0/M_PI+X*180.0/M_PI/Rz;
|
float LonT=Lon0*180.0/M_PI+Y*180.0/M_PI/Rz/cos(Lat0);
|
CustomGuidedWp=GuidedWp;
|
CustomGuidedWp.x=LatT;
|
CustomGuidedWp.y=LonT;
|
CustomGuidedWp.z=Alt;
|
log_i("Tracker custom mode switch to Guided");
|
mYawPID->reset_I();
|
mYawPID->reset_filter();
|
mUnlock=false;
|
return ResSwitchCustomGuided;
|
}
|
if(n1(2)>1e-5)
|
{
|
float t=Alt/(n1(2));
|
float X=n1(0)*t;
|
float Y=n1(1)*t;
|
float LatT=Lat0*180.0/M_PI+X*180.0/M_PI/Rz;
|
float LonT=Lon0*180.0/M_PI+Y*180.0/M_PI/Rz/cos(Lat0);
|
D=sqrt(X*X+Y*Y);
|
/*if(D>5000)
|
{
|
mUseCustomTrackerMode=false;
|
log_i("Tracker custom mode error distans>5000 ");
|
if(trackerV)
|
mUseTracker=false;
|
return ResContinue;
|
}*/
|
CustomGuidedWp=GuidedWp;
|
CustomGuidedWp.x=LatT;
|
CustomGuidedWp.y=LonT;
|
CustomGuidedWp.z=1;
|
log_i("Tracker custom mode switch to Guided");
|
mYawPID->reset_I();
|
mYawPID->reset_filter();
|
mUnlock=false;
|
return ResSwitchCustomGuided;
|
}
|
if(n1(2)<-1e-5)
|
{
|
float t=(-Alt)/(n1(2));
|
float X=n1(0)*t;
|
float Y=n1(1)*t;
|
float LatT=Lat0*180.0/M_PI+X*180.0/M_PI/Rz;
|
float LonT=Lon0*180.0/M_PI+Y*180.0/M_PI/Rz/cos(Lat0);
|
D=sqrt(X*X+Y*Y);
|
/*if(D>5000)
|
{
|
mUseCustomTrackerMode=false;
|
log_i("Tracker custom mode error distans>5000 ");
|
if(trackerV)
|
mUseTracker=false;
|
return ResContinue;
|
}*/
|
CustomGuidedWp=GuidedWp;
|
CustomGuidedWp.x=LatT;
|
CustomGuidedWp.y=LonT;
|
CustomGuidedWp.z=2*Alt;
|
log_i("Tracker custom mode switch to Guided");
|
mYawPID->reset_I();
|
mYawPID->reset_filter();
|
mUnlock=false;
|
return ResSwitchCustomGuided;
|
}
|
|
}
|
}
|
|
if(mTargetWpNum>1 && cwpn!=mCurrWpNum && mCurrWpNum==TargetWp.seq && mCustomMode==PLANE_MODE_AUTO)
|
{
|
Lat0=WpD.x*M_PI/180.0;
|
Lon0=WpD.y*M_PI/180.0;
|
Lat1=TargetWp.x*M_PI/180.0;
|
Lon1=TargetWp.y*M_PI/180.0;
|
X1=(Lat1-Lat0)*Rz;
|
Y1=(Lon1-Lon0)*Rz*cos(Lat0);
|
D=sqrt(X1*X1+Y1*Y1);
|
float dAlt=WpD.z-TargetWp.z;
|
mTanWp=dAlt/D;
|
mAngleWp=atan(mTanWp)*180.0/M_PI;
|
cwpn=mCurrWpNum;
|
GuidedWp=TargetWp;
|
GuidedWp.seq=0;
|
GuidedWp.current=2;
|
mUseTracker=false;
|
mKpitcKorr=mK_Pitch*M_PI/180.0/D;
|
log_i("Current target wp nums=%d",mCurrWpNum);
|
//WpD1.z=mAlt;
|
//mX=1000;
|
mYawPID->reset_I();
|
mYawPID->reset_filter();
|
mUnlock=false;
|
return ResSwitchGuided;
|
}
|
|
if(mTargetWpNum>1 && mCurrWpNum==TargetWp.seq && mCustomMode==PLANE_MODE_GUIDED)
|
{
|
mAttackMode=AttackWp;
|
if(trackerV)
|
{
|
mUseTracker=true;
|
|
}
|
Lat1=TargetWp.x*M_PI/180.0;
|
Lon1=TargetWp.y*M_PI/180.0;
|
X1=(Lat1-Lat0)*Rz;
|
Y1=(Lon1-Lon0)*Rz*cos(Lat0);
|
D=sqrt(X1*X1+Y1*Y1);
|
if(mTestMode)
|
{
|
if(Alt<mTestAlt+20 && mCTRL_SRV_CH)
|
{
|
mUnlock=true;
|
}
|
if(Alt<mTestAlt)
|
{
|
log_i("Switch to loiter test alt");
|
cwpn=0;
|
in_corr=false;
|
mUseTracker=false;
|
if(Alt<mLoiterAltMin)
|
mLoiterAlt=mLoiterAltMin;
|
else
|
mLoiterAlt=Alt;
|
return ResSwitchSafeMode;//ResSwitchLoiter;
|
|
}
|
}
|
|
if(!in_corr)
|
{
|
mAngleWp=-atan2(Alt-TargetWp.z,D);
|
mTanWp=(Alt-TargetWp.z)/D;
|
in_corr=true;
|
}
|
float ErrorAlt=Alt-TargetWp.z;
|
mAltError=ErrorAlt;
|
mDistans=D;
|
|
yaw_t=atan2(Y1,X1);
|
|
if(mUseKpitch)
|
pitch_q=-atan2(Alt-TargetWp.z,D)+mKpitcKorr*D;//mAngleWp-mK_Pitch*ErrorAlt*M_PI/180;
|
else
|
pitch_q=-atan2(Alt-TargetWp.z,D);
|
|
|
if(mUseTracker&&mEnableTracker)
|
{
|
mAttackMode=AttackWpTracker;
|
if(trackerV&&trackerNew)
|
{
|
float camYaw=(trackerX-mWidth/2.0)*mKx;
|
float camPitch=(mHeight/2.0-trackerY)*mKy;
|
Dcm<float> mPlaneM=Dcm<float>(Euler<float>(roll,pitch,yaw));
|
Dcm<float> mCameraM;
|
if(mCAM_SRV_CH)
|
{
|
mCameraM=Dcm<float>(Euler<float>(0,(camPitch-mCAM_CURRENT)*M_PI/180,camYaw*M_PI/180));
|
CamYTrecker(camPitch,true);
|
}
|
else
|
{
|
mCameraM=Dcm<float>(Euler<float>(0,camPitch*M_PI/180,camYaw*M_PI/180));
|
}
|
Dcm<float> mToNED=mPlaneM*mCameraM;
|
Euler<float> ang=Euler<float>(mToNED);
|
mCam_pitch=ang.theta();
|
mCam_yaw=ang.psi();
|
}
|
yaw_t=mCam_yaw;
|
pitch_q=mCam_pitch;
|
}
|
else
|
{
|
mCam_yaw=yaw_t;
|
mCam_pitch=pitch_q;
|
}
|
mPitchError=(pitch_q-pitch)*180.0/M_PI;
|
/*if(yaw_t<0)
|
yaw_t+=2*M_PI;
|
float error=yaw_t-yaw;
|
if(error>(M_PI+2.0*M_PI/180.0))
|
error=-2*M_PI+error;
|
if(error<(-M_PI-2.0*M_PI/180.0))
|
error=2*M_PI+error;
|
mYawError=error*180.0/M_PI;
|
float roll_q=mK_Yaw*(error);
|
if(roll_q>30.0*M_PI/180.0)
|
roll_q=30.0*M_PI/180.0;
|
if(roll_q<-30.0*M_PI/180.0)
|
roll_q=-30*M_PI/180.0;*/
|
mMutex.Lock();
|
roll_q=mYawPID->update_all(yaw_t,yaw,0.02,false)*M_PI/180.0;
|
mYawError=mYawPID->get_error();
|
mMutex.Unlock();
|
if(fabs(pitch_q-pitch)*180/M_PI>mCAM_PITCH_DELTA)
|
{
|
mRollMax=5;
|
mYawPID->reset_I();
|
}
|
else
|
{
|
if(mCAM_CURRENT>0 && mCAM_CURRENT<1.0f && mCAM_SRV_CH)
|
{
|
Ang2PWM(0);
|
mCAM_CURRENT=0;
|
mSendPWM=true;
|
}
|
mRollMax=40;
|
}
|
if(roll_q>mRollMax*M_PI/180.0)
|
roll_q=mRollMax*M_PI/180.0;
|
if(roll_q<-mRollMax*M_PI/180.0)
|
roll_q=-mRollMax*M_PI/180.0;
|
|
cnt++;
|
if(cnt==1000)
|
{
|
log_i("Use tracker=%d target yaw=%f,pitch=%f",mUseTracker,yaw_t*180.0/M_PI,pitch_q*180.0/M_PI);
|
cnt=0;
|
}
|
float phi=roll_q;
|
float theta=pitch_q;
|
float psi=0;
|
Eulerf et(phi,theta,psi);
|
Quatf qt(et);
|
mQ0=qt(0);
|
mQ1=qt(1);
|
mQ2=qt(2);
|
mQ3=qt(3);
|
CreateMavlink(roll_q,pitch_q,yaw_t,X1,Y1,mAltError,mAttackMode);
|
mGuidedAlt=Alt-mTECS_DALT;
|
return ResSendControl;
|
|
}
|
|
if(mCAM_SRV_CH)
|
{
|
if(trackerNew&&mEnableTracker)
|
{
|
if(trackerV)
|
{
|
float camPitch=(mHeight/2.0-trackerY)*mKy;
|
mUseTracker=true;
|
/*mCAM_CURRENT-=camPitch*mCAM_SRV_KI;
|
if(mCAM_CURRENT<mCAM_ANG_MIN)
|
mCAM_CURRENT=mCAM_ANG_MIN;
|
if(mCAM_CURRENT>mCAM_ANG_MAX)
|
mCAM_CURRENT=mCAM_ANG_MAX;
|
Ang2PWM(mCAM_CURRENT);*/
|
CamYTrecker(camPitch,false);
|
|
return ResSendCamPWM;
|
}
|
else
|
{
|
mUseTracker=false;
|
}
|
}
|
CreateMavlink(0,mCAM_CURRENT*M_PI/180.0f,0,0,0,0,CameraManual);
|
|
if(mNewCamData&&!mUseTracker)
|
{
|
mNewCamData=false;
|
float ang;
|
float target=100.0f-mCAM_TARGET;
|
if(target<0)
|
target=0;
|
float dang=mCAM_ANG_MAX-mCAM_ANG_MIN;
|
ang=mCAM_ANG_MIN+target*dang/100.0f;
|
if(fabs(mCAM_CURRENT-ang)>0.1)
|
{
|
Ang2PWM(ang);
|
mCAM_CURRENT=ang;
|
return ResSendCamPWM;
|
}
|
else
|
{
|
return ResContinue;
|
}
|
|
}
|
else
|
{
|
if(mCAM_CURRENT<0.0f)
|
{
|
mCAM_CURRENT=mCAM_ANG_CRUISE;
|
Ang2PWM(mCAM_CURRENT);
|
log_i("Set camera to CAM_ANG_CRUISE PWM=%d",mCamPWM);
|
return ResSendCamPWM;
|
}
|
}
|
}
|
else
|
{
|
CreateMavlink(0,mCAM_CURRENT*M_PI/180.0f,0,0,0,0,mAttackMode);
|
}
|
cwpn=0;
|
in_corr=false;
|
//mUseTracker=false;
|
mAttackMode=AttackNone;
|
return ResContinue;
|
}
|
void StateMachine::CamYTrecker(float camPitch,bool on_attack)
|
{
|
if(on_attack && camPitch<0)
|
return;
|
|
if(fabs(camPitch)>1e-5)
|
{
|
float errorI=camPitch*mCAM_SRV_KI;
|
if(errorI>mCAM_RATE_MAX)
|
errorI=mCAM_RATE_MAX;
|
if(errorI<-mCAM_RATE_MAX)
|
errorI=-mCAM_RATE_MAX;
|
mCAM_CURRENT-=errorI;
|
|
float errorP=camPitch*mCAM_SRV_KP;
|
if(errorI+errorP>mCAM_RATE_MAX)
|
errorP=mCAM_RATE_MAX-errorI;
|
if(errorI+errorP<-mCAM_RATE_MAX)
|
errorP=-mCAM_RATE_MAX-errorI;
|
|
float ang=mCAM_CURRENT-errorP;
|
|
if(mCAM_CURRENT<mCAM_ANG_MIN)
|
mCAM_CURRENT=mCAM_ANG_MIN;
|
if(mCAM_CURRENT>mCAM_ANG_MAX)
|
mCAM_CURRENT=mCAM_ANG_MAX;
|
if(ang>mCAM_ANG_MAX)
|
ang=mCAM_ANG_MAX;
|
if(ang<mCAM_ANG_MIN)
|
ang=mCAM_ANG_MIN;
|
|
|
Ang2PWM(ang);
|
mSendPWM=true;
|
}
|
|
}
|
|
|
void StateMachine::Ang2PWM(float v)
|
{
|
float K=(mCAM_SRV_MAX-mCAM_SRV_MIN)/(mCAM_ANG_MAX-mCAM_ANG_MIN);
|
float pwm;
|
if(mCAM_SRV_REVERSE)
|
{
|
pwm=mCAM_SRV_MAX-K*(v-mCAM_ANG_MIN);
|
}
|
else
|
{
|
pwm=mCAM_SRV_MIN+K*(v-mCAM_ANG_MIN);
|
}
|
mCamPWM=roundf(pwm);
|
if(mCamPWM>mCAM_SRV_MAX)
|
mCamPWM=mCAM_SRV_MAX;
|
if(mCamPWM<mCAM_SRV_MIN)
|
mCamPWM=mCAM_SRV_MIN;
|
}
|
|
void StateMachine::SetTrackerData(float x,float y,uint8_t v,uint8_t mode,uint16_t cnt)
|
{
|
mMutex.Lock();
|
mTrackerX=x;
|
mTrackerY=y;
|
mTrackerValid=v;
|
mTrackerMode=mode;
|
mTrackerMsgCnt=cnt;
|
mNewTrackerData=true;
|
mMutex.Unlock();
|
}
|
|
void StateMachine::CreateMavlink(float r,float p,float y,float X,float Y,float Z,float mode)
|
{
|
mavlink_message_t msg;
|
mavlink_vision_position_delta_t pos;
|
pos.angle_delta[0]=r;
|
pos.angle_delta[1]=p;
|
pos.angle_delta[2]=y;
|
pos.position_delta[0]=X;
|
pos.position_delta[1]=Y;
|
pos.position_delta[2]=Z;
|
pos.confidence=mode;
|
mavlink_msg_vision_position_delta_encode(1,1,&msg,&pos);
|
mSendLen = mavlink_msg_to_send_buffer(mBufferOutStream, &msg);
|
mMavlinkNew=true;
|
|
}
|
|
void StateMachine::SetFlapVAL(float val)
|
{
|
mMutex.Lock();
|
mFLAP_VAL=val;
|
if(val<0)
|
val=fabs(val);
|
if(val>1000)
|
val=1000;
|
mFLAP_PWM=1000+val;
|
mMutex.Unlock();
|
}
|
|
uint8_t StateMachine::SafeMode()
|
{
|
float Lat0,Lon0,Lat1,Lon1,X1,Y1,D,Alt,yaw,roll,pitch;
|
|
mY=mZ=mR=mX=0;
|
mMutex.Lock();
|
Lat0=mLat*M_PI/180.0;
|
Lon0=mLon*M_PI/180.0;
|
Alt=mAlt;
|
roll=mRoll;
|
pitch=mPitch;
|
yaw=mYaw;
|
mMutex.Unlock();
|
if(mEnterSafeMode)
|
{
|
mEnterSafeMode=false;
|
|
float phi=0;
|
float theta=mSafePitch*M_PI/180.0f;
|
float psi=0;
|
Eulerf et(phi,theta,psi);
|
Quatf qt(et);
|
mQ0=qt(0);
|
mQ1=qt(1);
|
mQ2=qt(2);
|
mQ3=qt(3);
|
|
float d=mSafeSpeed*mSafeTime*cos(theta);
|
float dAlt=mSafeSpeed*mSafeTime*sin(theta);
|
|
float X=cos(yaw)*d;
|
float Y=sin(yaw)*d;
|
float LatT=Lat0*180.0/M_PI+X*180.0/M_PI/Rz;
|
float LonT=Lon0*180.0/M_PI+Y*180.0/M_PI/Rz/cos(Lat0);
|
CustomGuidedWp=GuidedWp;
|
CustomGuidedWp.x=LatT;
|
CustomGuidedWp.y=LonT;
|
CustomGuidedWp.z=Alt+dAlt;
|
mGuidedAlt=Alt+dAlt;
|
return ResSendWpAndControl;
|
}
|
if(mTargetWpNum>1 && mCurrWpNum!=TargetWp.seq && mCustomMode!=PLANE_MODE_GUIDED)
|
{
|
return ResSwitchNavigation;
|
}
|
return ResContinue;
|
}
|
