bool _Automaton::init(void* pKiss)
{
CHECK_F(!this->_ThreadBase::init(pKiss));
Kiss* pK = (Kiss*)pKiss;
pK->m_pInst = this;
//create state instances
Kiss** pItr = pK->getChildItr();
int i = 0;
while (pItr[i])
{
Kiss* pState = pItr[i++];
CHECK_F(m_nState >= N_STATE);
F_ERROR_F(pState->v("state", &m_pStateName[m_nState]));
m_nState++;
}
string startState = "";
F_ERROR_F(pK->v("startState", &startState));
m_iState = getStateIdx(&startState);
CHECK_F(m_iState<0);
return true;
}
// Runs update logic
void KissFileSampleApp::update()
{
// Check if track is playing and has a PCM buffer available
if (mTrack->isPlaying() && mTrack->isPcmBuffering())
{
// Get buffer
mBuffer = mTrack->getPcmBuffer();
if (mBuffer && mBuffer->getInterleavedData())
{
// Get sample count
uint32_t mSampleCount = mBuffer->getInterleavedData()->mSampleCount;
if (mSampleCount > 0)
{
// Initialize analyzer, if needed
if (!mFftInit)
{
mFftInit = true;
mFft.setDataSize(mSampleCount);
}
// Analyze data
if (mBuffer->getInterleavedData()->mData != 0)
mFft.setData(mBuffer->getInterleavedData()->mData);
}
}
}
}
bool APMrover_base::link(void)
{
CHECK_F(!this->ActionBase::link());
Kiss* pK = (Kiss*)m_pKiss;
string iName = "";
F_INFO(pK->v("_Mavlink", &iName));
m_pMavlink = (_Mavlink*) (pK->root()->getChildInstByName(&iName));
return true;
}
bool _Bullseye::link(void)
{
CHECK_F(!this->_ThreadBase::link());
Kiss* pK = (Kiss*)m_pKiss;
//link instance
string iName = "";
F_ERROR_F(pK->v("_Stream",&iName));
m_pStream = (_StreamBase*)(pK->root()->getChildInstByName(&iName));
return true;
}
bool _Universe::init(void* pKiss)
{
CHECK_F(this->_ThreadBase::init(pKiss) == false);
Kiss* pK = (Kiss*)pKiss;
pK->m_pInst = this;
F_ERROR_F(pK->v("frameLifetime", &m_frameLifeTime));
F_ERROR_F(pK->v("probMin", &m_objProbMin));
F_ERROR_F(pK->v("posDisparity", &m_disparity));
return true;
}
bool HM_follow::link(void)
{
CHECK_F(this->ActionBase::link()==false);
Kiss* pK = (Kiss*)m_pKiss;
string iName = "";
F_INFO(pK->v("HM_base", &iName));
m_pHM = (HM_base*) (pK->parent()->getChildInstByName(&iName));
F_INFO(pK->v("_Universe", &iName));
m_pUniv = (_Universe*) (pK->root()->getChildInstByName(&iName));
return true;
}
//*********************************************************************************
void BeatDetectorApp::Init( uint32_t mSampleCount )
{
mFftInit = true;
mFft.setDataSize(mSampleCount);
int num_bins = mFft.getBinSize();
CSoundAnalyzer::StaticInit(num_bins, samples_per_frame);
for(int i=0; i<5; i++)
{
float i_f = i/(float)5 - 0.4f;
m_Cirlces.push_back(Circle(Color(1, 0, 0), 0, Vec2f(0, getWindowHeight() * i_f)));
}
}
bool _ROITracker::link(void)
{
NULL_F(m_pKiss);
Kiss* pK = (Kiss*)m_pKiss;
//link instance
string iName = "";
F_ERROR_F(pK->v("_Stream",&iName));
m_pStream = (_StreamBase*)(pK->root()->getChildInstByName(&iName));
//TODO: link variables to Automaton
return true;
}
bool _SSD::init(void* pKiss)
{
CHECK_F(!this->_ThreadBase::init(pKiss));
Kiss* pK = (Kiss*)pKiss;
pK->m_pInst = this;
//Setup Caffe Classifier
string caffeDir = "";
string modelFile;
string trainedFile;
string meanFile;
string labelFile;
string presetDir = "";
F_INFO(pK->root()->o("APP")->v("presetDir", &presetDir));
F_INFO(pK->v("dir", &caffeDir));
F_FATAL_F(pK->v("modelFile", &modelFile));
F_FATAL_F(pK->v("trainedFile", &trainedFile));
F_FATAL_F(pK->v("meanFile", &meanFile));
F_FATAL_F(pK->v("labelFile", &labelFile));
F_INFO(pK->v("minConfidence", &m_confidence_threshold));
setup(caffeDir + modelFile, caffeDir + trainedFile, caffeDir + meanFile, presetDir + labelFile);
LOG_I("Initialized");
m_pFrame = new Frame();
return true;
}
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 _Flow::init(void* pKiss)
{
CHECK_F(!this->_ThreadBase::init(pKiss));
Kiss* pK = (Kiss*)pKiss;
pK->m_pInst = this;
string presetDir = "";
string labelFile;
F_INFO(pK->root()->o("APP")->v("presetDir", &presetDir));
F_INFO(pK->v("bDepth", &m_bDepth));
F_INFO(pK->v("width", &m_width));
F_INFO(pK->v("height", &m_height));
F_INFO(pK->v("flowMax", &m_flowMax));
F_INFO(pK->v("colorFile", &labelFile));
m_pDepth = new Frame();
m_pFarn = cuda::FarnebackOpticalFlow::create();
m_pGrayFrames = new FrameGroup();
m_pGrayFrames->init(2);
m_GDMat = GpuMat(m_height, m_width, CV_32FC1, Scalar(0));
return true;
// m_labelColor = imread(presetDir+labelFile, 1);
// m_pGpuLUT = cuda::createLookUpTable(m_labelColor);
// m_pSeg = new CamFrame();
// m_flowMat = GpuMat(SMALL_WIDTH, SMALL_HEIGHT, CV_32FC2);
}
// Draw
void KissFileSampleApp::draw()
{
// Clear screen
gl::clear(Color(0.0f, 0.0f, 0.0f));
// Check init flag
if (mFftInit)
{
// Get data
float * mFreqData = mFft.getAmplitude();
float * mTimeData = mFft.getData();
int32_t mDataSize = mFft.getBinSize();
// Get dimensions
float mScale = ((float)getWindowWidth() - 20.0f) / (float)mDataSize;
float mWindowHeight = (float)getWindowHeight();
// Use polylines to depict time and frequency domains
PolyLine<Vec2f> mFreqLine;
PolyLine<Vec2f> mTimeLine;
// Iterate through data
for (int32_t i = 0; i < mDataSize; i++)
{
// Do logarithmic plotting for frequency domain
double mLogSize = log((double)mDataSize);
float x = (float)(log((double)i) / mLogSize) * (double)mDataSize;
float y = math<float>::clamp(mFreqData[i] * (x / mDataSize) * log((double)(mDataSize - i)), 0.0f, 2.0f);
// Plot points on lines
mFreqLine.push_back(Vec2f(x * mScale + 10.0f, -y * (mWindowHeight - 20.0f) * 1.25f + (mWindowHeight - 10.0f)));
mTimeLine.push_back(Vec2f(i * mScale + 10.0f, mTimeData[i] * (mWindowHeight - 20.0f) * 0.3f + (mWindowHeight * 0.15f + 10.0f)));
}
// Draw signals
gl::draw(mFreqLine);
gl::draw(mTimeLine);
}
}
bool _SSD::link(void)
{
CHECK_F(!this->_ThreadBase::link());
Kiss* pK = (Kiss*)m_pKiss;
string iName = "";
F_ERROR_F(pK->v("_Stream",&iName));
m_pStream = (_StreamBase*)(pK->root()->getChildInstByName(&iName));
F_ERROR_F(pK->v("_Universe",&iName));
m_pUniverse = (_Universe*)(pK->root()->getChildInstByName(&iName));
for(int i=0; i<labels_.size(); i++)
{
m_pUniverse->addObjClass(&labels_.at(i), 0);
}
return true;
}
bool _AutoPilot::link(void)
{
CHECK_F(!this->BASE::link());
Kiss* pK = (Kiss*)m_pKiss;
int i;
for(i=0; i<m_nAction; i++)
{
ActionBase* pA = m_pAction[i];
F_ERROR_F(pA->link());
}
string iName="";
F_INFO(pK->v("_Automaton", &iName));
m_pAM = (_Automaton*) (pK->root()->getChildInstByName(&iName));
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;
}
//*************************************************************************
void BeatDetectorApp::update()
{
if(write_frames)
{
if(curr_sample < p_sample->m_SampleCount)
{
// Initialize analyzer, if needed
if (!mFftInit)
{
Init(samples_per_frame);
}
mFft.setData(p_sample->mp_Buffer + (curr_sample));
curr_sample += samples_per_frame;
CSoundAnalyzer::Get().ProcessData(mFft.getAmplitude(), mFft.getData());
}
else
{
shutdown();
}
}
else
{
// Check if track is playing and has a PCM buffer available
if (mTrack->isPlaying() && mTrack->isPcmBuffering())
{
// Get buffer
mBuffer = mTrack->getPcmBuffer();
if (mBuffer && mBuffer->getInterleavedData())
{
// Get sample count
uint32_t mSampleCount = mBuffer->getChannelData(CHANNEL_FRONT_LEFT)->mSampleCount;
if (mSampleCount > 0)
{
// Initialize analyzer, if needed
if (!mFftInit)
{
Init(samples_per_frame);
}
// Analyze data
if (mBuffer->getChannelData(CHANNEL_FRONT_LEFT)->mData != 0)
mFft.setData(mBuffer->getChannelData(CHANNEL_FRONT_LEFT)->mData);
CSoundAnalyzer::Get().ProcessData(mFft.getAmplitude(), mFft.getData());
}
}
}
}
}
bool RC_visualFollow::link(void)
{
CHECK_F(!this->ActionBase::link());
Kiss* pK = (Kiss*)m_pKiss;
string iName = "";
F_INFO(pK->v("_RC", &iName));
m_pRC = (_RC*) (pK->root()->getChildInstByName(&iName));
F_INFO(pK->v("RC_base", &iName));
m_pRCconfig = (RC_base*) (pK->root()->getChildInstByName(&iName));
F_ERROR_F(pK->v("ROItracker", &iName));
m_pROITracker = (_ROITracker*) (pK->root()->getChildInstByName(&iName));
return true;
}
bool _Bullseye::init(void* pKiss)
{
CHECK_F(!this->_ThreadBase::init(pKiss));
Kiss* pK = (Kiss*)pKiss;
pK->m_pInst = this;
//format params
F_ERROR_F(pK->v("areaRatio", &m_areaRatio));
F_ERROR_F(pK->v("minSize", &m_minMarkerSize));
F_INFO(pK->v("method", &m_method));
F_INFO(pK->v("medBlueKsize", &m_kSize));
F_INFO(pK->v("HoughMinDist", &m_houghMinDist));
F_INFO(pK->v("HoughParam1", &m_houghParam1));
F_INFO(pK->v("HoughParam2", &m_houghParam2));
F_INFO(pK->v("HoughMinR", &m_houghMinR));
F_INFO(pK->v("HoughMaxR", &m_houghMaxR));
m_pFrame = new Frame();
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 HM_follow::init(void* pKiss)
{
CHECK_F(this->ActionBase::init(pKiss) == false);
Kiss* pK = (Kiss*)pKiss;
pK->m_pInst = this;
F_INFO(pK->v("targetX", &m_destX));
F_INFO(pK->v("targetY", &m_destY));
F_INFO(pK->v("targetArea", &m_destArea));
F_INFO(pK->v("targetClass", &m_targetClass));
F_INFO(pK->v("speedP", &m_speedP));
F_INFO(pK->v("steerP", &m_steerP));
F_INFO(pK->v("filterWindow", &m_filterWindow));
m_pTargetX->startMedian(m_filterWindow);
m_pTargetY->startMedian(m_filterWindow);
m_pTargetArea->startMedian(m_filterWindow);
return true;
}
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_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;
}
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;
}
