bool Monster::init(float x, float y, float width, float height)
{
if(!Hero::init(x, y, width, height))
{
return false;
}
initParam();
return true;
}
bool Article::init(float x, float y, float width, float height)
{
if (!LifeObject::init(x, y, width, height))
{
return false;
}
initParam();
return true;
}
bool Monster::init(float x, float y, b2Vec2* points, int count)
{
if(!Hero::init(x, y, points, count))
{
return false;
}
initParam();
return true;
}
/// ////////////////////////
/// main関数
int main(int argc, char** argv)
{
axisThick = axisThick_;
cubeSize_ = cubeSize/4.;
/// Mat dataPoints
char fname[255];
sprintf(fname,"%s/%s/%s",filedir,dir,filename);
rows = readfileLine(fname);
dataPoints = csvread(fname,rows,cols); //csvから点群座標取得
/// ファイル書き込み
sprintf(fname,"%s/%s/Distance_%s",filedir,dir,filename);
errno_t error;
error = fopen_s(&fp, fname, "w");
if(error != 0){
cout << "ファイルが開けません " << fname << endl;
exit(1);
}
fprintf(fp,"距離,点1x,点1y,点1z,点2x,点2y,点2z\n");
/// OpenGL
FLAG = (int *)malloc(sizeof(int) * rows);
for(int i=0;i<rows;i++){
FLAG[i] = 0;
}
/// OpenGL
// GLUT initialize
initFlag();
initParam();
Initialize();
//window1
glutInit(&argc, argv);
glutInitWindowPosition(0, 0);
glutInitWindowSize(window_w, window_h);
glutInitDisplayMode( GLUT_RGBA | GLUT_DEPTH | GLUT_DOUBLE );
window1 = glutCreateWindow("Window1");
glutDisplayFunc(disp);
glutMouseFunc(mouse);
glutMotionFunc(drag);
glutPassiveMotionFunc(passive);
glutMouseWheelFunc ( MouseWheel ) ;//ホイールコールバック
glutIdleFunc(myGlutIdle);
glutKeyboardFunc(glut_keyboard);
glutIdleFunc(animate);
glClearColor(0.0, 0.0, 0.0, 0.5); //背景色
//描写開始
glutMainLoop();
fclose(fp);
return 0;
}
bool Article::init(float x, float y, b2Vec2* points, int count)
{
if (!LifeObject::init(x, y, points, count))
{
return false;
}
initParam();
return true;
}
PSV_ChartItem::PSV_ChartItem(const QMap<int, QVariant> ¶m, QGraphicsItem *parent)
: PSV_Item(param,parent)
// , m_isStaHidden(false)
, m_isCurrentMessHidden(true)
, m_isFisrtCurve_left(true)
, m_isFisrtCurve_right(true)
, m_isFisrtCurve_curP(&m_isFisrtCurve_left)
, m_curveZValue(1)
, m_max_y_left(10)
, m_min_y_left(0)
, m_max_y_right(1)
, m_min_y_right(-1)
, m_max_y_curP(&m_max_y_left)
, m_min_y_curP(&m_min_y_left)
, m_margin_up(15)
, m_margin_down(0)
, m_margin_left(5)
, m_margin_right(20)
, m_margin_total_up(50)
, m_margin_total_down(80)
, m_margin_total_left(50)
, m_margin_total_right(80)
, m_staMaxWidth(0)
, m_max_x(100.0)
, m_min_x(0.0)
, m_titleItem(NULL)
, m_currentMesItem(NULL)
, m_leftAxisItem(NULL)
, m_rightAxisItem(NULL)
, m_upAxisItem(NULL)
, m_downAxisItem(NULL)
, m_vLineItem(NULL)
, m_hLineItem(NULL)
, m_boundingItem(NULL)
{
initParam();
m_isAdvancetable = true;//根据advance自动 刷新
//====================
setAcceptHoverEvents(true);
}
void MazeData::init()
{
int row = 0;
int col = 0;
int len = 0;
char line[1024]={'\0'};
_input.getline(line, sizeof(line));
col = strlen(line);
if (col % 2 == 0) {
cerr << "line should be odd length.\n";
exit(1);
}
while (!_input.eof()) {
len = strlen(line);
if (len != col) {
cerr << "lines should be of the same length.\n";
exit(1);
}
char *item = new char[len+1];
strncpy(item, line, len);
_data.push_back(item);
_input.getline(line, sizeof(line));
row++;
}
if (row % 2 == 0) {
cerr << "should be odd numbers of lines.\n";
exit(1);
}
initParam();
checkInvalidChar();
changeToGraph();
}
//////////main関数/////////////
int main(int argc, char** argv){
cubeSize_ = cubeSize/2.;
int i=0, j=0, k=0;
clock_t start_time_total,end_time_total;
clock_t start_time[fileTotal];
clock_t end_time[fileTotal];
//char * filename[fileTotal];
//並列用
char * model_filename[fileTotal];
char * data_filename[fileTotal];
//Mat shape[fileTotal];
Mat shape_reg[fileTotal];
//Mat shape_temp[fileTotal];
Mat shape_fixed[fileTotal];
//並列用
Mat model_shape[fileTotal];
Mat data_shape[fileTotal];
Mat shape_temp[fileTotal][fileTotal];
Mat my_model_corr[fileTotal];
int myIndex[fileTotal][rows];
float myDist[fileTotal][rows];
RT<float> my_rt[fileTotal];
Mat_<float> model_mean;
//modelファイルのデータ数
//model_rows = 16128;
//dataファイルのデータ数
//data_rows = 16128;
start_time_total = clock();
cout << "-------------" << endl;
cout << "ICP Algorithm" << endl;
cout << "-------------" << endl;
#pragma omp parallel for
for(fileCount=0;fileCount<fileTotal;fileCount++)
{
#pragma region // --- 点群のCSVファイルをcv::Matに取り込む ---
if(fileCount>=1){
///model
//csvファイル名
model_filename[fileCount] = (char *)malloc(sizeof(char *) * 100);
//sprintf(model_filename[fileCount],"%s/%s/%d.csv",filedir,dir,fileCount);
sprintf(model_filename[fileCount],"%s/%s/points%02d.csv",filedir,dir,fileCount);
//csvファイルのデータ数
model_rows[fileCount] = rows;
//CSVファイル読み込み
model_shape[fileCount] = csvread(model_filename[fileCount], model_rows[fileCount], cols);
//コンソールにファイル名表示
//cout << "model点群データファイル名 " << model_filename[fileCount] << endl;
}
///data
//csvファイル名
data_filename[fileCount] = (char *)malloc(sizeof(char *) * 100);
//sprintf(data_filename[fileCount],"%s/%s/%d.csv",filedir,dir,(fileCount+1));
sprintf(data_filename[fileCount],"%s/%s/points%02d.csv",filedir,dir,(fileCount+1));
//csvファイルのデータ数
data_rows[fileCount] = rows;
//CSVファイル読み込み
data_shape[fileCount] = csvread(data_filename[fileCount], data_rows[fileCount], cols);
//コンソールにファイル名表示
cout << "点群データファイル名 " << data_filename[fileCount] << endl;
#pragma endregion
if(fileCount>=1){
#pragma region // --- ICPによるレジストレーション ---
#if 1 // --- ICP実行する ---
//実行時間計測開始
start_time[fileCount] = clock();
cout << "\t標準ICP開始" << endl;
//ICP with flann search and unit quaternion method
//cout << "kd-tree探索＋クォータニオンにより[R/t]を推定します" << endl << endl;
ClosestPointFlann model_shape_flann (model_shape[fileCount]);
RT_L2 <float, SolveRot_eigen<float>> rt_solver;
ICP <ClosestPointFlann> icp (model_shape_flann, rt_solver);
icp.set(data_shape[fileCount]);
icp.reg(100, 1.0e-6);
//実行時間計測終了
end_time[fileCount] = clock();
//cout << "icp result : [R/t] =" << endl << (icp.rt) << endl << endl;
cout << "\t" << data_filename[fileCount] << " icp error =" << icp.dk << endl;
cout << "\t" << data_filename[fileCount] << " 実行時間 = " << (float)(end_time[fileCount] - start_time[fileCount])/CLOCKS_PER_SEC << "秒" << endl << endl;
//データをローカル変数に格納
//my_model_corr[fileCount] = Mat::zeros(rows, cols, CV_32F);
my_model_corr[fileCount].create(rows, cols, CV_32F);
icp.model_corr.copyTo(my_model_corr[fileCount]);
icp.rt.copyTo(my_rt[fileCount]);
for(int k=0;k<data_rows[fileCount];k++){
myIndex[fileCount][k] = icp.index[k];
myDist[fileCount][k] = icp.distance[k];
}
#else // --- ICP実行しない場合 ---
shape_reg[fileCount] = data_shape[fileCount];
#endif
#pragma endregion
}else{
shape_reg[fileCount] = data_shape[fileCount];
}
}
#pragma region // --- 座標変換 ---
//平均値の計算
reduce(shape_reg[0], model_mean, 0, CV_REDUCE_AVG);
#pragma omp parallel for private(i,j,k)
for(fileCount=0;fileCount<fileTotal;fileCount++)
{
if(fileCount>=1){
//得られたrtをdatashapeに適用
//その前にshape_tempの初期化
for(k=0;k<fileTotal;k++)
{
shape_temp[fileCount][k] = cv::Mat::zeros(data_rows[fileCount], cols, CV_32F);
}
shape_temp[fileCount][fileCount] = data_shape[fileCount];
for(k=0;k<fileCount;k++)
{
shape_temp[fileCount][fileCount-(k+1)] = my_rt[(fileCount-k)].transform(shape_temp[fileCount][fileCount-k]);
}
shape_reg[fileCount] = shape_temp[fileCount][0];
}
shape_fixed[fileCount] = shape_reg[fileCount] - repeat(model_mean, shape_reg[fileCount].rows, 1);
/*
//メモリ割り当て
points[fileCount] = (GLfloat *)malloc(sizeof(float)*data_rows[fileCount]*cols);
//座標値をGLpointsに入れる
for(i=0;i<data_rows[fileCount];i++){
for(j=0;j<cols;j++){
points[fileCount][i*cols+j] = shape_fixed[fileCount].at<float>(i,j);
}
}*/
#pragma endregion
}
#pragma region // --- OpenGLにデータ渡す ---
//メモリ割り当て
allpoints = (GLfloat *)malloc(sizeof(float)*rows*fileTotal*cols);
for(fileCount=0;fileCount<fileTotal;fileCount++)
{
//座標値をallpointsに入れる
for(int i=0;i<rows;i++){
for(int j=0;j<cols;j++){
allpoints[fileCount*rows*cols+i*cols+j] = shape_fixed[fileCount].at<float>(i,j);
}
}
}
#pragma endregion
#pragma region // --- カメラRTの計算 ---
Mat cameraRT[fileTotal];
Mat cameraR[fileTotal];
Mat cameraT[fileTotal];
cameraRT[0] = Mat::eye(4,4,CV_32F);
cameraR[0] = Mat::eye(3,3,CV_32F);
cameraT[0] = Mat::zeros(1,3,CV_32F);
for(i=1;i<fileTotal;i++){
cameraRT[i] = Mat::eye(4,4,CV_32F);
cameraR[i] = Mat::eye(3,3,CV_32F);
cameraT[i] = Mat::zeros(1,3,CV_32F);
Mat r = my_rt[i].operator()(Range(0,3),Range(0,3));
cameraR[i] = cameraR[i-1]*r.t();
Mat t = my_rt[i].operator()(Range(3,4),Range(0,3));
cameraT[i] = t*cameraR[i-1].t() + cameraT[i-1];
cameraRT[i].at<float>(0,0) = cameraR[i].at<float>(0,0);
cameraRT[i].at<float>(0,1) = cameraR[i].at<float>(0,1);
cameraRT[i].at<float>(0,2) = cameraR[i].at<float>(0,2);
cameraRT[i].at<float>(1,0) = cameraR[i].at<float>(1,0);
cameraRT[i].at<float>(1,1) = cameraR[i].at<float>(1,1);
cameraRT[i].at<float>(1,2) = cameraR[i].at<float>(1,2);
cameraRT[i].at<float>(2,0) = cameraR[i].at<float>(2,0);
cameraRT[i].at<float>(2,1) = cameraR[i].at<float>(2,1);
cameraRT[i].at<float>(2,2) = cameraR[i].at<float>(2,2);
cameraRT[i].at<float>(3,0) = cameraT[i].at<float>(0,0);
cameraRT[i].at<float>(3,1) = cameraT[i].at<float>(0,1);
cameraRT[i].at<float>(3,2) = cameraT[i].at<float>(0,2);
}
#pragma endregion
// --- データ出力 ---
#if FILEOUTPUT
///////////////////////////////
// 全ての点群(shape_fixed)をまとめて書き出し
// pcd
//
FILE *outfp;
char outfilename[100];
sprintf(outfilename,"%s/%s/result_xyz.pcd",outdir,dir);
outfp = fopen(outfilename,"w");
if(outfp == NULL){
printf("%sファイルが開けません\n",outfilename);
return -1;
}
int red = 255*256*256;
int green = 255*256*256 + 255*256;
int white = 255*256*256 + 255*256 + 255;
fprintf(outfp,"# .PCD v.7 - Point Cloud Data file format\nVERSION .7\nFIELDS x y z rgb\nSIZE 4 4 4 4\nTYPE F F F F\nCOUNT 1 1 1 1\nWIDTH %d\nHEIGHT 1\nVIEWPOINT 0 0 0 1 0 0 0\nPOINTS %d\nDATA ascii\n", rows*fileTotal, rows*fileTotal);
for(i=0;i<fileTotal;i++){
for(j=0;j<data_rows[i];j++){
fprintf(outfp,"%f %f %f %d\n", shape_reg[i].at<float>(j,0), shape_reg[i].at<float>(j,1), shape_reg[i].at<float>(j,2), green+(int)floor(255.*(i+1)/fileTotal));
}
}
fclose(outfp);
///////////////////////////////
// 全ての点群(shape_fixed)をまとめて書き出し
// csv
//
sprintf(outfilename,"%s/%s/allpoints.csv",outdir,dir);
outfp = fopen(outfilename,"w");
if(outfp == NULL){
printf("%sファイルが開けません\n",outfilename);
return -1;
}
for(i=0;i<fileTotal;i++){
for(j=0;j<data_rows[i];j++){
fprintf(outfp,"%f %f %f\n", shape_reg[i].at<float>(j,0), shape_reg[i].at<float>(j,1), shape_reg[i].at<float>(j,2));
}
}
fclose(outfp);
///////////////////////////////
// 全ての点群(shape_fixed)をまとめて書き出し
// result_xyz.csv
//
sprintf(outfilename,"%s/%s/result_xyz_icp.csv",outdir,dir);
outfp = fopen(outfilename,"w");
if(outfp == NULL){
printf("%sファイルが開けません\n",outfilename);
return -1;
}
for(i=0;i<fileTotal;i++){
for(j=0;j<data_rows[i];j++){
fprintf(outfp,"%f,%f,%f\n", shape_reg[i].at<float>(j,0), shape_reg[i].at<float>(j,1), shape_reg[i].at<float>(j,2));
}
}
fclose(outfp);
//////////////////////////////////
// Corr(対応点), Index（対応点の要素番号）, Distance（対応点間距離）の書き出し
//
FILE *outfp_corr;
char outfilename_corr[100];
for(fileCount=1;fileCount<fileTotal;fileCount++){
///Indexファイル
sprintf(outfilename_corr,"%s/%s/index%02d.csv",outdir,dir,(fileCount));
outfp_corr = fopen(outfilename_corr,"w");
if(outfp_corr == NULL){
printf("%sファイルが開けません\n",outfilename_corr);
return -1;
}
for(j=0;j<data_rows[fileCount];j++){
fprintf(outfp_corr,"%d\n", myIndex[fileCount][j]);
}
fclose(outfp_corr);
///Distanceファイル
sprintf(outfilename_corr,"%s/%s/dist%02d.csv",outdir,dir,(fileCount));
outfp_corr = fopen(outfilename_corr,"w");
if(outfp_corr == NULL){
printf("%sファイルが開けません\n",outfilename_corr);
return -1;
}
for(j=0;j<data_rows[fileCount];j++){
fprintf(outfp_corr,"%f\n", myDist[fileCount][j]);
}
fclose(outfp_corr);
}
for(fileCount=0;fileCount<fileTotal;fileCount++){
if(fileCount<(fileTotal-1)){
///Corr点群ファイル
sprintf(outfilename_corr,"%s/%s/corr%02d.csv",outdir,dir,(fileCount+1));
outfp_corr = fopen(outfilename_corr,"w");
if(outfp_corr == NULL){
printf("%sファイルが開けません\n",outfilename_corr);
return -1;
}
for(j=0;j<data_rows[fileCount];j++){
//fprintf(outfp_corr,"%f %f %f\n", my_model_corr[fileCount].at<float>(j,0), my_model_corr[fileCount].at<float>(j,1), my_model_corr[fileCount].at<float>(j,2));
fprintf(outfp_corr,"%f %f %f\n", shape_reg[fileCount].at<float>(myIndex[fileCount+1][j],0), shape_reg[fileCount].at<float>(myIndex[fileCount+1][j],1), shape_reg[fileCount].at<float>(myIndex[fileCount+1][j],2));
}
fclose(outfp_corr);
}else{
///Corr点群ファイル
sprintf(outfilename_corr,"%s/%s/corr%02d.csv",outdir,dir,(fileCount+1));
outfp_corr = fopen(outfilename_corr,"w");
if(outfp_corr == NULL){
printf("%sファイルが開けません\n",outfilename_corr);
return -1;
}
for(j=0;j<data_rows[fileCount];j++){
//fprintf(outfp_corr,"%f %f %f\n", my_model_corr[fileCount].at<float>(j,0), my_model_corr[fileCount].at<float>(j,1), my_model_corr[fileCount].at<float>(j,2));
fprintf(outfp_corr,"%f %f %f\n", shape_reg[fileCount].at<float>(j,0), shape_reg[fileCount].at<float>(j,1), shape_reg[fileCount].at<float>(j,2));
}
fclose(outfp_corr);
}
}
/////////////////////
// RTの書き出し
//
//my_rt[0]に恒等変換を代入
//Mat rt0 = (Mat_<float>(4,4) << 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
Mat rt0 = Mat::eye(4,4,CV_32F);
rt0.copyTo(my_rt[0]);
// Open File Storage
char rtfilename[100];
sprintf(rtfilename,"%s/%s/rt.xml",outdir,dir);
cv::FileStorage cvfs(rtfilename,CV_STORAGE_WRITE);
cv::WriteStructContext ws(cvfs, "mat_rt", CV_NODE_SEQ); // create node
for(int i=0; i<fileTotal; i++){
cv::write(cvfs,"",cameraRT[i]);
}
cvfs.release();
#endif
//--- OpenGLで表示 ---
#if GLVIEW
// --- GLUT initialize ---
initFlag();
initParam();
//window1
glutInit(&argc, argv);
glutInitWindowPosition(0, 0);
glutInitWindowSize(window_w, window_h);
glutInitDisplayMode( GLUT_RGBA | GLUT_DEPTH | GLUT_DOUBLE );
window1 = glutCreateWindow("Window1");
glutMouseFunc(mouse);
glutMotionFunc(drag);
glutPassiveMotionFunc(passive);
glutMouseWheelFunc ( MouseWheel ) ;//ホイールコールバック
glutDisplayFunc(disp);
glutIdleFunc(myGlutIdle);
glutKeyboardFunc(glut_keyboard);
glutIdleFunc(animate);
glClearColor(0.0, 0.0, 0.0, 0.5); //背景色
glutMainLoop();
#endif
//実行時間計測終了
end_time_total = clock();
cout << "-------------" << endl;
cout << " Finish " << endl;
cout << "-------------" << endl;
cout << "プログラム実行時間 = " << (float)(end_time_total - start_time_total)/CLOCKS_PER_SEC << "秒" << endl << endl;
//cvNamedWindow ("WaitKey", CV_WINDOW_AUTOSIZE);
//cvWaitKey(0);
return 0;
}
/*************************************************
Function: OnInitDialog
Desc:
Input:
Return: SUCCESS 成功
FAIL 失败
**************************************************/
BOOL CDialogSetting::OnInitDialog()
{
CDialog::OnInitDialog();
CString tempa,tempb;
int i;
PARAM_STRU pm = getParams();
tempa.Format("%d",pm.brightness);
//str = getCStringFromConfig("CAMS","Brightness");
c_Brightness.SetWindowTextA(tempa);
for(i=1; i<=100; i++) {
tempb.Format("%d",i);
c_Brightness.AddString(tempb);
if(tempa == tempb) {
c_Brightness.SetCurSel(i);
}
tempb="";
}
setCStringToConfig("CAMS","Brightness",tempa);
tempa="";
tempa.Format("%d",pm.contrast);
//str = getCStringFromConfig("CAMS","Contrast");
c_Contrast.SetWindowTextA(tempa);
for(i=1; i<=100; i++) {
tempb.Format("%d",i);
c_Contrast.AddString(tempb);
if(tempa == tempb) {
c_Contrast.SetCurSel(i);
}
tempb="";
}
setCStringToConfig("CAMS","Contrast",tempa);
tempa="";
tempa.Format("%d",pm.sharpness);
//str = getCStringFromConfig("CAMS","Sharpness");
c_Sharpness.SetWindowTextA(tempa);
for(i=1; i<=100; i++) {
tempb.Format("%d",i);
c_Sharpness.AddString(tempb);
if(tempa == tempb) {
c_Sharpness.SetCurSel(i);
}
tempb="";
}
setCStringToConfig("CAMS","Sharpness",tempa);
tempa="";
tempa.Format("%d",pm.saturation);
//str = getCStringFromConfig("CAMS","Saturation");
c_Saturation.SetWindowTextA(tempa);
for(i=1; i<=100; i++) {
tempb.Format("%d",i);
c_Saturation.AddString(tempb);
if(tempa == tempb) {
c_Saturation.SetCurSel(i);
}
tempb="";
}
setCStringToConfig("CAMS","Saturation",tempa);
tempa="";
//str.Format("%d",pm.exposuremode);
if(pm.exposuremode ==0) {
tempa = "手动曝光";
} else {
tempa = "自动曝光";
}
//str = getCStringFromConfig("CAMS","ExposureMode");
c_ExpMode.SetWindowTextA(tempa);
c_ExpMode.AddString(_T("自动曝光"));
c_ExpMode.AddString(_T("手动曝光"));
if(tempa == _T("自动曝光")) {
c_ExpMode.SetCurSel(0);
} else if(tempa == _T("手动曝光")) {
c_ExpMode.SetCurSel(1);
}
setCStringToConfig("CAMS","ExposureMode",tempa);
tempa="";
tempa.Format("%d",pm.exposuretime);
//str = getCStringFromConfig("CAMS","ExposureTime");
c_ExpTime.SetWindowTextA(tempa);
for(i=1; i<=100; i++) {
tempb.Format("%d",i*100);
c_ExpTime.AddString(tempb);
if(tempa == tempb) {
c_ExpTime.SetCurSel(i);
}
tempb="";
}
setCStringToConfig("CAMS","ExposureTime",tempa);
tempa="";
if(pm.daynight ==0) {
tempa = "白天";
} else if(pm.daynight ==1) {
tempa = "夜晚";
} else if(pm.daynight ==2) {
tempa = "自动";
} else if(pm.daynight ==3) {
tempa = "定时";
} else if(pm.daynight ==4) {
tempa = "报警输入触发";
}
//str.Format("%d",pm.daynight);
//str = getCStringFromConfig("CAMS","DayNight");
c_DayNight.SetWindowTextA(tempa);
c_DayNight.AddString(_T("白天"));
c_DayNight.AddString(_T("夜晚"));
c_DayNight.AddString(_T("自动"));
c_DayNight.AddString(_T("定时"));
c_DayNight.AddString(_T("报警输入触发"));
if(tempa == _T("白天")) {
c_DayNight.SetCurSel(0);
} else if(tempa == _T("夜晚")) {
c_DayNight.SetCurSel(1);
} else if(tempa == _T("自动")) {
c_DayNight.SetCurSel(2);
} else if(tempa == _T("定时")) {
c_DayNight.SetCurSel(3);
} else if(tempa == _T("报警输入触发")) {
c_DayNight.SetCurSel(4);
}
setCStringToConfig("CAMS","DayNight",tempa);
tempa="";
initParam();
return TRUE; // return TRUE unless you set the focus to a control
}
int main(void) {
uint8_t currState;
uint8_t targetState;
uint16_t readbcLength;
uint32_t nwkId;
uint8_t errorCheckInterval = 100;
// Stop watchdog timer to prevent time out reset
WDTCTL = WDTPW + WDTHOLD;
// Initialize the MCU and board peripherals
halBoardInit();
halBoardStartXT1();
halBoardSetSystemClock(SYSCLK_16MHZ);
halButtonsInit(BUTTON_ALL);
halLcdInit();
halLcdBackLightInit();
halLcdSetContrast(90);
halLcdSetBackLight(10);
halLcdClearScreen();
halLcdPrintLine(" CC85XX SLAVE ", 0, OVERWRITE_TEXT );
halLcdPrintLine(" ", 1, OVERWRITE_TEXT );
halLcdPrintLine("S1: Power toggle ", 2, OVERWRITE_TEXT );
halLcdPrintLine("S2: Pairing start", 3, OVERWRITE_TEXT );
uifLcdPrintJoystickInfo();
// Wipe remote control information
memset(&ehifRcSetDataParam, 0x00, sizeof(ehifRcSetDataParam));
// Initialize EHIF IO
ehifIoInit();
// Reset into the application
ehifSysResetPin(true);
currState = CC85XX_STATE_ALONE;
targetState = CC85XX_STATE_ACTIVE;
// Get the last used network ID from CC85XX non-volatile storage
initParam();
ehifCmdParam.nvsGetData.index = 0;
ehifCmdExecWithRead(EHIF_EXEC_ALL, EHIF_CMD_NVS_GET_DATA,
sizeof(EHIF_CMD_NVS_GET_DATA_PARAM_T), &ehifCmdParam,
sizeof(EHIF_CMD_NVS_GET_DATA_DATA_T), &ehifCmdData);
nwkId = ehifCmdData.nvsGetData.data;
// Handle illegal default network IDs that may occur first time after programming
if ((nwkId == 0x00000000) || (nwkId == 0xFFFFFFFF)) {
nwkId = 0xFFFFFFFE;
}
// Main loop
while (1) {
// Wait 10 ms
EHIF_DELAY_MS(10);
// Perform action according to edge-triggered button events (debouncing with 100 ms delay)
switch (pollButtons()) {
// POWER TOGGLE
case BUTTON_S1:
if (currState == CC85XX_STATE_OFF) {
targetState = CC85XX_STATE_ACTIVE;
} else {
targetState = CC85XX_STATE_OFF;
}
break;
// PAIRING TRIGGER
case BUTTON_S2:
if (currState != CC85XX_STATE_OFF) {
targetState = CC85XX_STATE_PAIRING;
}
break;
}
// Run the state machine
if (currState != targetState) {
if (currState == CC85XX_STATE_OFF) {
// HANDLE POWER ON
// Ensure known state (power state 5)
ehifSysResetPin(true);
currState = CC85XX_STATE_ALONE;
} else if (targetState == CC85XX_STATE_OFF) {
// HANDLE POWER OFF
// Ensure known state (power state 5)
ehifSysResetPin(true);
currState = CC85XX_STATE_ALONE;
// Set power state 0
ehifCmdParam.pmSetState.state = 0;
ehifCmdExec(EHIF_CMD_PM_SET_STATE, sizeof(EHIF_CMD_PM_SET_STATE_PARAM_T), &ehifCmdParam);
currState = CC85XX_STATE_OFF;
} else if (targetState == CC85XX_STATE_PAIRING) {
// HANDLE PAIRING
// Let the last executed EHIF command complete, with 5 second timeout
ehifWaitReadyMs(5000);
// Disconnect if currently connected
if (ehifGetStatus() & BV_EHIF_STAT_CONNECTED) {
initParam();
// All parameters should be zero
ehifCmdExec(EHIF_CMD_NWM_DO_JOIN, sizeof(EHIF_CMD_NWM_DO_JOIN_PARAM_T), &ehifCmdParam);
}
// Search for one protocol master with pairing signal enabled for 10 seconds
initParam();
ehifCmdParam.nwmDoScan.scanTo = 1000;
ehifCmdParam.nwmDoScan.scanMax = 1;
ehifCmdParam.nwmDoScan.reqPairingSignal = 1;
ehifCmdParam.nwmDoScan.reqRssi = -128;
ehifCmdExecWithReadbc(EHIF_EXEC_CMD, EHIF_CMD_NWM_DO_SCAN,
sizeof(EHIF_CMD_NWM_DO_SCAN_PARAM_T), &ehifCmdParam,
NULL, NULL);
// Fetch network information once ready
ehifWaitReadyMs(12000);
readbcLength = sizeof(ehifNwmDoScanData);
ehifCmdExecWithReadbc(EHIF_EXEC_DATA, EHIF_CMD_NWM_DO_SCAN,
0, NULL,
&readbcLength, &ehifNwmDoScanData);
// If found ...
if (readbcLength == sizeof(EHIF_CMD_NWM_DO_SCAN_DATA_T)) {
// Update the network ID to be used next
nwkId = ehifNwmDoScanData.deviceId;
// Place the new network ID in CC85XX non-volatile storage
initParam();
ehifCmdParam.nvsSetData.index = 0;
ehifCmdParam.nvsSetData.data = ehifNwmDoScanData.deviceId;
ehifCmdExec(EHIF_CMD_NVS_SET_DATA, sizeof(EHIF_CMD_NVS_SET_DATA_PARAM_T), &ehifCmdParam);
}
// Done
currState = CC85XX_STATE_ALONE;
targetState = CC85XX_STATE_ACTIVE;
} else if (targetState == CC85XX_STATE_ACTIVE) {
// We're disconnected. Proceed only if EHIF is ready, so that power toggle and pairing
// buttons can still be operated
uint16_t status = ehifGetStatus();
if (status & BV_EHIF_STAT_CMD_REQ_RDY) {
// Perform join operation first and then activate audio channels. We're using remote
// volume control
if (!(status & BV_EHIF_STAT_CONNECTED)) {
// Enable disconnection notification to avoid unnecessary EHIF activity while active
initParam();
ehifCmdParam.ehcEvtClr.clearedEvents = BV_EHIF_EVT_NWK_CHG;
ehifCmdExec(EHIF_CMD_EHC_EVT_CLR, sizeof(EHIF_CMD_EHC_EVT_CLR_PARAM_T), &ehifCmdParam);
initParam();
ehifCmdParam.ehcEvtMask.irqGioLevel = 0;
ehifCmdParam.ehcEvtMask.eventFilter = BV_EHIF_EVT_NWK_CHG;
ehifCmdExec(EHIF_CMD_EHC_EVT_MASK, sizeof(EHIF_CMD_EHC_EVT_MASK_PARAM_T), &ehifCmdParam);
// Not connected: Start JOIN operation
initParam();
ehifCmdParam.nwmDoJoin.joinTo = 100;
ehifCmdParam.nwmDoJoin.deviceId = nwkId;
ehifCmdExec(EHIF_CMD_NWM_DO_JOIN, sizeof(EHIF_CMD_NWM_DO_JOIN_PARAM_T), &ehifCmdParam);
} else {
// Connected: Subscribe to audio channels (0xFF = unused)
memset(&ehifCmdParam, 0xFF, sizeof(EHIF_CMD_NWM_ACH_SET_USAGE_PARAM_T));
ehifCmdParam.nwmAchSetUsage.pAchUsage[0] = 0; // Front left -> I2S LEFT
ehifCmdParam.nwmAchSetUsage.pAchUsage[1] = 1; // Front right -> I2S RIGHT
ehifCmdExec(EHIF_CMD_NWM_ACH_SET_USAGE, sizeof(EHIF_CMD_NWM_ACH_SET_USAGE_PARAM_T), &ehifCmdParam);
currState = CC85XX_STATE_ACTIVE;
}
}
}
} else {
// Only OFF and ACTIVE are permanent target states. SCAN is only a temporary target state.
// In the OFF state we do nothing, so only need to handle the ACTIVE state.
if (currState == CC85XX_STATE_ACTIVE) {
// Detect network disconnection without generating noise on the SPI interface
if (EHIF_INTERRUPT_IS_ACTIVE()) {
currState = CC85XX_STATE_ALONE;
}
// Perform error checking at 10 ms * 100 = 1 second intervals:
// - No timeouts or SPI errors shall have occurred
// - We should be connected unless disconnection has been signalized
if (--errorCheckInterval == 0) {
errorCheckInterval = 100;
uint16_t status = ehifGetStatus();
if (ehifGetWaitReadyError() || (status & BV_EHIF_EVT_SPI_ERROR) ||
(!(status & BV_EHIF_STAT_CONNECTED) && !(status & BV_EHIF_EVT_NWK_CHG))) {
// The device is in an unknown state -> restart everything
ehifSysResetPin(true);
currState = CC85XX_STATE_ALONE;
}
}
// If the network connection is up and running...
if (currState == CC85XX_STATE_ACTIVE) {
// Send remote control commands (mouse or play control, depending on which uif file
// is included in the build)
if (uifPollFunc(&ehifRcSetDataParam)) {
ehifCmdExec(EHIF_CMD_RC_SET_DATA, sizeof(EHIF_CMD_RC_SET_DATA_PARAM_T), &ehifRcSetDataParam);
}
}
}
}
}
} // main