bool APMrover_base::init(void* pKiss)
{
CHECK_F(this->ActionBase::init(pKiss)==false);
Kiss* pK = (Kiss*)pKiss;
pK->m_pInst = this;
Kiss* pCC;
APMrover_PID cPID;
pCC = pK->o("steer");
CHECK_F(pCC->empty());
F_ERROR_F(pCC->v("P", &cPID.m_P));
F_INFO(pCC->v("I", &cPID.m_I));
F_INFO(pCC->v("Imax", &cPID.m_Imax));
F_INFO(pCC->v("D", &cPID.m_D));
F_INFO(pCC->v("dT", &cPID.m_dT));
m_pidSteer = cPID;
pCC = pK->o("thrust");
CHECK_F(pCC->empty());
F_ERROR_F(pCC->v("P", &cPID.m_P));
F_INFO(pCC->v("I", &cPID.m_I));
F_INFO(pCC->v("Imax", &cPID.m_Imax));
F_INFO(pCC->v("D", &cPID.m_D));
F_INFO(pCC->v("dT", &cPID.m_dT));
m_pidThrust = cPID;
//init controls
m_lastHeartbeat = 0;
m_iHeartbeat = 0;
return true;
}
bool RC_visualFollow::init(void* pKiss)
{
CHECK_F(!this->ActionBase::init(pKiss));
Kiss* pK = (Kiss*)pKiss;
m_roll.reset();
m_pitch.reset();
m_yaw.reset();
m_alt.reset();
F_ERROR_F(pK->v("targetX", &m_roll.m_targetPos));
F_ERROR_F(pK->v("targetY", &m_pitch.m_targetPos));
F_INFO(pK->v("ROIsizeFrom", &m_ROIsizeFrom));
F_INFO(pK->v("ROIsizeTo", &m_ROIsizeTo));
F_INFO(pK->v("ROIsizeStep", &m_ROIsizeStep));
//setup UI
Kiss* pC;
pC = pK->o("assist");
if (!pC->empty())
{
m_pUIassist = new UI();
F_FATAL_F(m_pUIassist->init(pC));
}
pC = pK->o("drawRect");
if (!pC->empty())
{
m_pUIdrawRect = new UI();
F_FATAL_F(m_pUIdrawRect->init(pC));
}
pK->m_pInst = this;
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;
}
bool _Mavlink::init(void* pKiss)
{
CHECK_F(!this->_ThreadBase::init(pKiss));
Kiss* pK = (Kiss*) pKiss;
pK->m_pInst = this;
Kiss* pCC = pK->o("input");
CHECK_F(pCC->empty());
m_pSerialPort = new SerialPort();
CHECK_F(!m_pSerialPort->init(pCC));
//init param
m_systemID = 1;
m_componentID = MAV_COMP_ID_PATHPLANNER;
m_type = MAV_TYPE_ONBOARD_CONTROLLER;
m_targetComponentID = 0;
m_msg.sysid = 0;
m_msg.compid = 0;
m_status.packet_rx_drop_count = 0;
return true;
}
bool RC_base::init(void* pKiss)
{
CHECK_F(this->ActionBase::init(pKiss)==false);
Kiss* pK = (Kiss*)pKiss;
pK->m_pInst = this;
RC_PID cPID;
RC_CHANNEL RC;
Kiss* pCC;
pCC = pK->o("roll");
CHECK_F(pCC->empty());
F_ERROR_F(pCC->v("P", &cPID.m_P));
F_ERROR_F(pCC->v("I", &cPID.m_I));
F_ERROR_F(pCC->v("Imax", &cPID.m_Imax));
F_ERROR_F(pCC->v("D", &cPID.m_D));
F_ERROR_F(pCC->v("dT", &cPID.m_dT));
m_pidRoll = cPID;
F_ERROR_F(pCC->v("pwmL", &RC.m_pwmL));
F_ERROR_F(pCC->v("pwmH", &RC.m_pwmH));
F_ERROR_F(pCC->v("pwmN", &RC.m_pwmN));
F_ERROR_F(pCC->v("pwmCh", &RC.m_iCh));
m_rcRoll = RC;
pCC = pK->o("pitch");
CHECK_F(pCC->empty());
F_ERROR_F(pCC->v("P", &cPID.m_P));
F_ERROR_F(pCC->v("I", &cPID.m_I));
F_ERROR_F(pCC->v("Imax", &cPID.m_Imax));
F_ERROR_F(pCC->v("D", &cPID.m_D));
F_ERROR_F(pCC->v("dT", &cPID.m_dT));
m_pidPitch = cPID;
F_ERROR_F(pCC->v("pwmL", &RC.m_pwmL));
F_ERROR_F(pCC->v("pwmH", &RC.m_pwmH));
F_ERROR_F(pCC->v("pwmN", &RC.m_pwmN));
F_ERROR_F(pCC->v("pwmCh", &RC.m_iCh));
m_rcPitch = RC;
pCC = pK->o("alt");
CHECK_F(pCC->empty());
F_ERROR_F(pCC->v("P", &cPID.m_P));
F_ERROR_F(pCC->v("I", &cPID.m_I));
F_ERROR_F(pCC->v("Imax", &cPID.m_Imax));
F_ERROR_F(pCC->v("D", &cPID.m_D));
F_ERROR_F(pCC->v("dT", &cPID.m_dT));
m_pidAlt = cPID;
F_ERROR_F(pCC->v("pwmL", &RC.m_pwmL));
F_ERROR_F(pCC->v("pwmH", &RC.m_pwmH));
F_ERROR_F(pCC->v("pwmN", &RC.m_pwmN));
F_ERROR_F(pCC->v("pwmCh", &RC.m_iCh));
m_rcAlt = RC;
pCC = pK->o("yaw");
CHECK_F(pCC->empty());
F_ERROR_F(pCC->v("P", &cPID.m_P));
F_ERROR_F(pCC->v("I", &cPID.m_I));
F_ERROR_F(pCC->v("Imax", &cPID.m_Imax));
F_ERROR_F(pCC->v("D", &cPID.m_D));
F_ERROR_F(pCC->v("dT", &cPID.m_dT));
m_pidYaw = cPID;
F_ERROR_F(pCC->v("pwmL", &RC.m_pwmL));
F_ERROR_F(pCC->v("pwmH", &RC.m_pwmH));
F_ERROR_F(pCC->v("pwmN", &RC.m_pwmN));
F_ERROR_F(pCC->v("pwmCh", &RC.m_iCh));
m_rcYaw = RC;
return true;
}