int ResolvMode(int cur_mode)
{
double tmp;
if(CHECK_H(cur_mode)&&CHECK_T(cur_mode))
{
Mid_h = Humidex - Temper;
Mid_e = Mid_h/0.5555 + 10;
Mid_e = Mid_e/6.11;
tmp = log(Mid_e);
Dewpoint = 5417.7530/(19.833625-tmp)-273.16;
return DONE;
}
//gets here means, must have Dewpoint
tmp = 5417.7530*Dewpoint/(pow(273.16, 2)+Dewpoint*273.16);
Mid_e = 6.11*pow(EXP, tmp);
Mid_h = 0.5555*(Mid_e-10);
if(CHECK_T(cur_mode))//now, we have Dewpoint
Humidex = Temper + Mid_h;
if(CHECK_H(cur_mode))
Temper = Humidex - Mid_h;
return DONE;
}
bool ActionBase::link(void)
{
CHECK_F(!this->BASE::link());
Kiss* pK = (Kiss*)m_pKiss;
string iName="";
F_INFO(pK->v("_Automaton", &iName));
m_pAM = (_Automaton*) (pK->root()->getChildInstByName(&iName));
CHECK_T(m_pAM==NULL);
iName="";
CHECK_T(pK->v("activeState", &iName)==false);
m_iActiveState = m_pAM->getStateIdx(&iName);
return true;
}
bool HM_follow::draw(Frame* pFrame, vInt4* pTextPos)
{
NULL_F(pFrame);
Mat* pMat = pFrame->getCMat();
putText(*pMat,
"HM: rpmL=" + i2str(m_pHM->m_motorPwmL) + ", rpmR="
+ i2str(m_pHM->m_motorPwmR),
cv::Point(pTextPos->m_x, pTextPos->m_y), FONT_HERSHEY_SIMPLEX, 0.5,
Scalar(0, 255, 0), 1);
pTextPos->m_y += pTextPos->m_w;
CHECK_T(m_pTarget==NULL);
circle(*pMat, Point(m_pTarget->m_bbox.midX(), m_pTarget->m_bbox.midY()), 10,
Scalar(0, 0, 255), 2);
return true;
}
bool _AutoPilot::init(void* pKiss)
{
CHECK_F(!this->_ThreadBase::init(pKiss));
Kiss* pK = (Kiss*)pKiss;
pK->m_pInst = this;
//create action instance
Kiss* pCC = pK->o("action");
CHECK_T(pCC->empty());
Kiss** pItr = pCC->getChildItr();
int i = 0;
while (pItr[i])
{
Kiss* pAction = pItr[i];
i++;
bool bInst = false;
F_INFO(pAction->v("bInst", &bInst));
if (!bInst)continue;
if (m_nAction >= N_ACTION)LOG(FATAL);
ActionBase** pA = &m_pAction[m_nAction];
m_nAction++;
//Add action modules below
ADD_ACTION(RC_visualFollow);
ADD_ACTION(APMcopter_landing);
ADD_ACTION(APMcopter_guided);
ADD_ACTION(HM_base);
ADD_ACTION(HM_follow);
ADD_ACTION(APMrover_base);
ADD_ACTION(APMrover_follow);
//Add action modules above
LOG_E("Unknown action class: "+pAction->m_class);
}
return true;
}
