int InputEventSensor::activate(int handle, int enabled) {
static int activated = 0;
int result;
if (handle != device.getHandle()) {
log_message(CRITICAL,"%s: line: %d: %s handle not match! handle: %d required handle: %d",
__FUNCTION__, __LINE__, data.name.c_str(), device.getHandle(), handle);
return -1;
}
enabled = enabled == 0 ? 0 : 1;
if (Calibration != NULL) {
if (enabled && !activated)
Calibration(&event, READ_DATA, data.calibrationFile.c_str());
else if (!enabled && activated)
Calibration(&event, STORE_DATA, data.calibrationFile.c_str());
}
result =writeToFile(data.activateInterface, handle, static_cast<int64_t>(enabled));
if (DriverCalibration != NULL && enabled != 0 && result == 0 && activated == 0)
DriverCalibration(&event, CALIBRATION_DATA, data.driverCalibrationFile.c_str(), data.driverCalibrationInterface.c_str());
activated = enabled;
return result;
}
//********************************************************************
// Main Functions
//********************************************************************
void main(void) { //Main function
Sys_Init(); // initialize board
putchar(' '); //the quotes in this line may not format correctly
Port_Init();//Init ports
XBR0_Init();//init xbro
PCA_Init();//init pca
SMB_Init();//init smb
printf("\r\n\n\n\nEmbedded Control Electric Compass and Ranger\n"); //print beginning message
Calibration();//Run calibration
comp_cal(); //Compass calibration
Choose_heading(); //Heading choice
printf("\r\nheading error");
while(1) { //inf loop, 40 ms it returns the heading
if (new_heading){ //enough overflows for a new heading COMPASS STUFF
new_heading = 0;//Clear new_heading
heading = ReadCompass(); //get compass heading
Steering_Servo(); //run steer func
}//end if new heading
if (new_range) { //if 80 ms has passed
new_range=0;//Clear new_range
range=ReadRanger();//read ranger
start_ping();//start ping
counts++;//set up text function
Drive_Motor(); //run drive func
}//end if new_range
if (counts == 3){ //prevoudly output prined every 200 ms, now every 180 ms
print_output();//Print data function. Delete this if faster output is desired
}//end if counts
}//end inf while
}//end main
int InputEventSensor::getData(std::queue<sensors_event_t> &eventQue) {
struct input_event inputEvent[32];
int count, ret;
ret = read(pollfd, inputEvent, 32 * sizeof(struct input_event));
if (ret < 0 || ret % sizeof(struct input_event)) {
log_message(CRITICAL,"Read input event error! ret: %d", ret);
return -1;
}
count = ret / sizeof(struct input_event);
for (int i = 0; i < count; i++) {
if ((inputEvent[i].type == EV_REL || inputEvent[i].type == EV_ABS) && !inputDataOverrun) {
log_message(CRITICAL,"in EV_REL\n");
float value = SensorHubHelper::ConvertToFloat(inputEvent[i].value, device.getType());
if ((inputEvent[i].type == EV_REL && inputEvent[i].code == REL_X) ||
(inputEvent[i].type == EV_ABS && inputEvent[i].code == ABS_X))
event.data[device.getMapper(AXIS_X)] = value * device.getScale(AXIS_X);
else if ((inputEvent[i].type == EV_REL && inputEvent[i].code == REL_Y) ||
(inputEvent[i].type == EV_ABS && inputEvent[i].code == ABS_Y))
event.data[device.getMapper(AXIS_Y)] = value * device.getScale(AXIS_Y);
else if ((inputEvent[i].type == EV_REL && inputEvent[i].code == REL_Z) ||
(inputEvent[i].type == EV_ABS && inputEvent[i].code == ABS_Z))
event.data[device.getMapper(AXIS_Z)] = value * device.getScale(AXIS_Z);
}
else if (inputEvent[i].type == EV_SYN) {
log_message(CRITICAL,"in EV_SYN\n");
if (inputEvent[i].code == SYN_DROPPED) {
log_message(CRITICAL,"input event overrun");
inputDataOverrun = true;
}
else if (inputDataOverrun) {
inputDataOverrun = false;
}
else {
event.timestamp = timevalToNano(inputEvent[i].time);
if (Calibration != NULL)
Calibration(&event, CALIBRATION_DATA, data.calibrationFile.c_str());
else if (device.getEventProperty() == VECTOR)
event.acceleration.status = SENSOR_STATUS_ACCURACY_MEDIUM;
eventQue.push(event);
}
}
}
return 0;
}
//*****************************************************************************
//
// Called by the NVIC as a result of GPIO port F interrupt event. For this
// application GPIO port F pin 0 corresponds to calibration button
//
//*****************************************************************************
void IntGPIOF(void) {
unsigned long ulStatus;
ulStatus = GPIOIntStatus(GPIO_PORTF_BASE, true);
//
// Clear all the pin interrupts that are set
//
GPIOIntClear(GPIO_PORTF_BASE, ulStatus);
if (ulStatus & GPIO_PIN_4) {
if (g_calibrationState == 0 || g_calibrationState == 1
|| g_calibrationState == 3 || g_calibrationState == 5)
Calibration();
}
}
void _ExecCalibration(void) {
int x,y;
uint16_t data[6];
uint16_t get_data[6];
int ax_Phys[2],ay_Phys[2];
/* calculate log. Positions */
int ax[2] = { 15, LCD_XSIZE -1-15};
// const int ay[2] = { 15, LCD_YSIZE-1-15};
int ay[2] = { LCD_YSIZE-1-15, 15};
GUI_TOUCH_SetDefaultCalibration();
/* _Calibrate upper left */
GUI_SetBkColor(GUI_RED);
GUI_Clear();
GUI_SetColor(GUI_WHITE); GUI_FillCircle(ax[0], ay[0], 10);
GUI_SetColor(GUI_RED); GUI_FillCircle(ax[0], ay[0], 5);
GUI_SetColor(GUI_WHITE);
GUI_DispStringAt("Press here", ax[0]+20, ay[0]);
do {
#if 0
GUI_PID_STATE State;
GUI_TOUCH_GetState(&State);
if (State.Pressed) {
#endif
#if 1
x = TPReadX();
y = TPReadY();
if ((x>=3700)&&(y>=3400)&&(x<3800)&&(y<3600)) {
#endif
ax_Phys[0] = GUI_TOUCH_GetxPhys();
ay_Phys[0] = GUI_TOUCH_GetyPhys();
break;
}
GUI_Delay (3000);
} while (1);
/* Tell user to release */
GUI_Clear();
GUI_DispStringAt("OK", ax[0]+20, ay[0]);
do {
GUI_PID_STATE State;
GUI_TOUCH_GetState(&State);
if (State.Pressed == 0) {
break;
}
GUI_Delay (100);
} while (1);
/* _Calibrate lower right */
GUI_SetBkColor(GUI_RED);
GUI_Clear();
GUI_SetColor(GUI_WHITE); GUI_FillCircle(ax[1], ay[1], 10);
GUI_SetColor(GUI_RED); GUI_FillCircle(ax[1], ay[1], 5);
GUI_SetColor(GUI_WHITE);
GUI_SetTextAlign(GUI_TA_RIGHT);
GUI_DispStringAt("Press here", ax[1]-20, ay[1]);
do {
#if 1
x = TPReadX();
y = TPReadY();
if ((y>450)&&(y<620)&&(x>350)&&(x<450)) {
#endif
#if 0
GUI_PID_STATE State;
GUI_TOUCH_GetState(&State);
if (State.Pressed) {
#endif
ax_Phys[1] = GUI_TOUCH_GetxPhys();
ay_Phys[1] = GUI_TOUCH_GetyPhys();
break;
}
GUI_Delay (3000);
} while (1);
GUI_TOUCH_Calibrate(GUI_COORD_X, ax[0], ax[1], ax_Phys[0], ax_Phys[1]);
GUI_TOUCH_Calibrate(GUI_COORD_Y, ay[0], ay[1], ay_Phys[0], ay_Phys[1]);
{ /* calculate and display values for configuration file */
int x0, x1;
int y0, y1;
GUI_Clear();
_Calibrate(GUI_COORD_X, ax[0], ax[1], ax_Phys[0], ax_Phys[1], &x0, &x1);
_Calibrate(GUI_COORD_Y, ay[0], ay[1], ay_Phys[0], ay_Phys[1], &y0, &y1);
GUI_DispStringAt("x0: ", 0, 0); GUI_DispDec(x0, 4); GUI_DispNextLine();
GUI_DispString ("x1: "); GUI_DispDec(x1, 4); GUI_DispNextLine();
GUI_DispString ("y0: "); GUI_DispDec(y0, 4); GUI_DispNextLine();
GUI_DispString ("y1: "); GUI_DispDec(y1, 4); GUI_DispNextLine();
GUI_DispString ("Please touch display to continue...");
GUI_Delay(1000);
data[0]= CAL_READY;
data[1]= ax_Phys[0];
data[2]= ay_Phys[0];
data[3]= ax_Phys[1];
data[4]= ay_Phys[1];
#if 1
save_calibrate_to_flash(data);
get_calibrate_data(get_data);
GUI_DispStringAt("x0: ", 100, 0); GUI_DispDec(get_data[1], 4); GUI_DispNextLine();
GUI_DispString ("x1: "); GUI_DispDec(get_data[2], 4); GUI_DispNextLine();
GUI_DispString ("y0: "); GUI_DispDec(get_data[3], 4); GUI_DispNextLine();
GUI_DispString ("y1: "); GUI_DispDec(get_data[4], 4); GUI_DispNextLine();
GUI_DispString ("state: "); GUI_DispDec(get_data[0], 4); GUI_DispNextLine();
#endif
do {
GUI_PID_STATE State;
GUI_TOUCH_GetState(&State);
if (State.Pressed)
break;
GUI_Delay (10);
} while (1);
}
}
int run_cal(void)
{
if(get_calibrate_state()== CAL_READY)
{
get_calibration();
}else{
#if 1
_ExecCalibration();
#endif
#if 0
Calibration();
#endif
}
}
int get_calibration(void)
{
uint16_t cal_data[5];
int ax_Phys[2],ay_Phys[2];
int ax[2] = { 15, LCD_XSIZE -1-15};
int ay[2] = { LCD_YSIZE-1-15, 15};
get_calibrate_data(cal_data);
ax_Phys[0] = cal_data[1];
ay_Phys[0] = cal_data[2];
ax_Phys[1] = cal_data[3];
ay_Phys[1] = cal_data[4];
GUI_TOUCH_Calibrate(GUI_COORD_X, ax[0], ax[1], ax_Phys[0], ax_Phys[1]);
GUI_TOUCH_Calibrate(GUI_COORD_Y, ay[0], ay[1], ay_Phys[0], ay_Phys[1]);
}
#if 0
void myAD2XY( int *adx , int *ady ){
float f_dat;
int dat1;
f_dat = (float)(*ady - admy)/(float)(*adx - admx);
if(f_dat>0){
if( *adx>admx && admk[2] >=f_dat) dat1 = 1;
else if( *adx>admx && admk[2] < f_dat ) dat1 =2;//2
else if( *adx<admx && admk[0] >=f_dat ) dat1 =0;
else if( *adx<admx && admk[0] < f_dat ) dat1 =3;//0
else{ dat1 =0; }//*adx =0;*ady =0; }
}else{
if( *adx>admx && admk[1] >=f_dat) dat1 = 0;
else if( *adx>admx && admk[1] < f_dat ) dat1 =1;//1
else if( *adx<admx && admk[3] >=f_dat ) dat1 =3;
else if( *adx<admx && admk[3] < f_dat ) dat1 =2;//3
else{ dat1 =0; }//*adx =0;*ady =0; }
}
*adx = (int)(Factors[dat1][0].a*(*adx)+Factors[dat1][0].b*(*ady)+Factors[dat1][0].c);
*ady = (int)(Factors[dat1][1].a*(*adx)+Factors[dat1][1].b*(*ady)+Factors[dat1][1].c);
}
/// Render and return calibration
Calibration Tuning::renderCalibration(
CalibrationMode _outputMode) const {
return Calibration(*this,_outputMode);
}
int main(void)
{
int16_t xSteps, ySteps;
uint16_t x, y, xOld, yOld;
uint8_t xNeutral8, yNeutral8;
uint8_t xWheel = 0b11001100;
uint8_t yWheel = 0b00110011;
uint16_t xNeutral16, yNeutral16;
uint8_t xFaktor = eeprom_read_byte(&cx);
uint8_t yFaktor = eeprom_read_byte(&cy);
uint8_t xDeadzone = eeprom_read_byte(&dx);
uint8_t yDeadzone = eeprom_read_byte(&dy);
uint8_t useDeadzone;
// Setup Ports
DDRA = (1<<DDA6)|(1<<DDA7);
DDRB = (1<<DDB0)|(1<<DDB1);
PORTA = (1<<PORTA2)|(1<<PORTA3)|(1<<PORTA5);
PORTB = (1<<PORTB2);
// nicht benötigte Module deaktivieren
PRR |= (1<<PRTIM0)|(1<<PRTIM1)|(1<<PRUSI);
// 0.25 s warten bis Spannung stabil
_delay_ms(250);
// bestimmen, ob Deadzones verwendet werden:
if ( (xDeadzone==0) && (yDeadzone==0) )
useDeadzone = 0;
else
useDeadzone = 1;
// Setup ADC
DIDR0 = (1<<ADC0D)|(1<<ADC1D); // Digital input disable für PA0+1
ADMUX = 0x01; // Kanal 1
ADCSRA = (1<<ADPS0)|(1<<ADPS1); // Taktteiler = 8 ==> f_ADC = 125 kHz
ADCSRA |= (1<<ADEN); // ADC aktivieren
// 1. ADC Wandlung
xNeutral16 = GetX();
// X Wert einlesen:
xNeutral16 = GetX();
// falls Deadzone vorhanden, Absolutwert aus EEPROM lesen
if (useDeadzone)
xNeutral16 = eeprom_read_word(&xAbsolute);
xNeutral8 = ScaleDown(xNeutral16, xFaktor);
xOld = xNeutral8;
// Y Wert einlesen:
yNeutral16 = GetY();
// falls Deadzone vorhanden, Absolutwert aus EEPROM lesen
if (useDeadzone)
yNeutral16 = eeprom_read_word(&yAbsolute);
yNeutral8 = ScaleDown(yNeutral16, yFaktor);
yOld = yNeutral8;
// Wenn PINB.2 = 0, dann beim nächsten Einschalten
// normalen Calibration Mode aktivieren:
if( ((1<<PINB2)&PINB)==0){
eeprom_write_byte(&calibrationStep, 5);
eeprom_write_byte(&dx, 0);
eeprom_write_byte(&dy, 0);
while(1);
}
// Wenn PINA.5 = 0, dann beim nächsten Einschalten
// Calibration Mode mit Deadzone aktivieren:
if( ((1<<PINA5)&PINA)==0){
eeprom_write_byte(&calibrationStep, 1);
while(1);
}
// Wenn calibration_step != 0, dann Calibration Mode ausführen
if ( (eeprom_read_byte(&calibrationStep)) > 0x00 ){
Calibration();
}
while(1)
{
// X Wert einlesen:
x = GetX();
// Deadzone berücksichtigen
if (useDeadzone){
if (x > xNeutral16){
if (x <= (xNeutral16+xDeadzone))
x = xNeutral16;
else
x = x - xDeadzone;
}
if (x < xNeutral16){
if (x >= (xNeutral16-xDeadzone))
x = xNeutral16;
else
x = x + xDeadzone;
}
}
// runterskalieren
x = ScaleDown( x, xFaktor);
// auf +/-84 begrenzen
if (x>xNeutral8)
if ( (x-xNeutral8) > MAX_RANGE)
x = xNeutral8 + MAX_RANGE;
if (x<xNeutral8)
if ( (xNeutral8-x) > MAX_RANGE)
x = xNeutral8 - MAX_RANGE;
// Y Wert einlesen:
y = GetY();
// Deadzone berücksichtigen
if (useDeadzone){
if (y > yNeutral16){
if (y <= (yNeutral16+yDeadzone))
y = yNeutral16;
else
y = y - yDeadzone;
}
if (y < yNeutral16){
if (y >= (yNeutral16-yDeadzone))
y = yNeutral16;
else
y = y + yDeadzone;
}
}
// runterskalieren
y = ScaleDown(y, yFaktor);
// auf +/-84 begrenzen
if (y>yNeutral8)
if ( (y-yNeutral8) > MAX_RANGE)
y = yNeutral8 + MAX_RANGE;
if (y<yNeutral8)
if ( (yNeutral8-y) > MAX_RANGE)
y = yNeutral8 - MAX_RANGE;
//Anzahl Steps berechnen
xSteps = (int16_t) x - xOld;
ySteps = (int16_t) y - yOld;
// wenn entspr. Jumper gebrückt, invertieren:
if (INVX)
xSteps = -xSteps;
if (INVY)
ySteps = -ySteps;
//alte Werte für nächsten Durchlauf speichern
xOld = x;
yOld = y;
//solange es noch Steps abzuarbeiten gibt:
while ( (xSteps!=0) || (ySteps!=0) ){
if (xSteps<0){
xWheel = RotateLeft(xWheel);
xSteps++;
}
if (xSteps>0){
xWheel = RotateRight(xWheel);
xSteps--;
}
if (ySteps>0){
yWheel = RotateRight(yWheel);
ySteps--;
}
if (ySteps<0){
yWheel = RotateLeft(yWheel);
ySteps++;
}
// neue XA/XB- und YA/YB-Werte ausgeben:
PORTB = (PORTB&0b11111100)|(xWheel & 0b00000011);
PORTA = (PORTA&0b00111111)|(yWheel & 0b11000000);
}
}
}
uint32_t CmdPwdAsserted(char *outputstr,T_MESSAGE *message)
{
char *p;
uint16_t bak_dr = 0;
p=outputstr;
switch(message->m_type)
{
case CMD_DO:
if( DoExecute((unsigned char)(message->m_intdata[0])) ) //执行成功
{
#if LINGUA == EN
sprintf(outputstr,"DO Executed!Please Check the Di Change\r\n");
p+=strlen("DO Executed!Please Check the Di Change\r\n");
#endif
#if LINGUA == CH
sprintf(outputstr,"遥控已操作，请查看遥信状态\r\n");
p+=strlen("遥控已操作，请查看遥信状态\r\n");
#endif
}
else
{
sprintf(outputstr,"总线状态异常，遥控执行失败！\r\n");
p+=strlen("总线状态异常，遥控执行失败！\r\n");
}
Shell_State=INIT;
message->m_type=CMD_NULL;
break;
case CMD_SET:
case CMD_COEF:
#if LINGUA == EN
sprintf(outputstr,"Please Enter Item Resetting Parameter\r\n");
p+=strlen("Please Enter Item Resetting Parameter\r\n");
#endif
#if LINGUA == CH
sprintf(outputstr,"输入“通道序号 系数”\r\n");
p+=strlen("输入“通道序号 系数”\r\n");
#endif
Shell_State=ENTER_DATA;
break;
case CMD_ADDR:
#if LINGUA == EN
sprintf(outputstr,"Please Enter IP address\r\n");
p+=strlen("Please Enter IP address\r\n");
#endif
#if LINGUA == CH
sprintf(outputstr,"输入IP地址\r\n");
p+=strlen("输入IP地址\r\n");
#endif
Shell_State=ENTER_ADDR;
break;
case CMD_CALB:
Calibration();
#if LINGUA == EN
sprintf(outputstr,"Please Check the result of calibration\r\n");
p+=strlen("Please Check the result of calibration\r\n");
#endif
#if LINGUA == CH
sprintf(outputstr,"校准完毕，核对遥测值\r\n");
p+=strlen("校准完毕，核对遥测值\r\n");
#endif
Shell_State=INIT;
message->m_type=CMD_NULL;
break;
case CMD_DCCAL:
Can_Ticalib(CAN1);
#if LINGUA == EN
sprintf(outputstr,"Please Check the result of calibration\r\n");
p+=strlen("Please Check the result of calibration\r\n");
#endif
#if LINGUA == CH
sprintf(outputstr,"标准源校准完毕，核对直流量\r\n");
p+=strlen("标准源校准完毕，核对直流量\r\n");
#endif
Shell_State=INIT;
message->m_type=CMD_NULL;
break;
case CMD_PARA:
#if LINGUA == EN
sprintf(outputstr,"Please Enter communiation settings\r\n");
p+=strlen("Please Enter communiation settings\r\n");
#endif
#if LINGUA == CH
sprintf(outputstr,"输入通讯参数\r\n");
p+=strlen("输入通讯参数\r\n");
#endif
Shell_State=ENTER_ADDR;
break;
//TYH:添加恢復默认IP
case CMD_RESTOREIP:
//infoNum = ShowInfo(outputstr, strRestoIP);
//p+=infoNum;
sprintf(outputstr,"是否确认恢复默认IP, 确认请按<Y>, 按其它键返回\r\n");
p+=strlen("是否确认恢复默认IP, 确认请按<Y>, 按其它键返回\r\n");
Shell_State=ENTER_RESTOREADDR;
break;
//TYH:20130508 恢复以太网
case CMD_RESET_ETH:
//复位以太网
Ethernet_HWRST();
sprintf(outputstr,"是开始重置设备以太网,请等待.......\r\n");
p+=strlen("开始重置设备以太网,请等待.......\r\n");
Shell_State=INIT;
message->m_type=CMD_NULL;
break;
//TYH:20130508 设置固件升级
case CMD_UPDATE_FIRMWARE:
//写备份寄存器 'KP_DR10', 用于启动升级固件
bak_dr = BKP_ReadBackupRegister(BKP_DR10);
if(bak_dr != 0x0707)
BKP_WriteBackupRegister(BKP_DR10, 0x0707);
sprintf(outputstr,"bak_dr=0x%4x 设置固件升级, 请重启设备启动升级!\r\n", bak_dr);
p+=strlen("bak_dr=0xffff 设置固件升级, 请重启设备启动升级!\r\n");
Shell_State=INIT;
message->m_type=CMD_NULL;
break;
//TYH:20130530 设置以太网状态判断时间
case CMD_ETH_LINK_TIME:
case CMD_ETH_RECV_TIME:
sprintf(outputstr,"输入以太网判断时间参数\r\n");
p+=strlen("输入以太网判断时间参数\r\n");
Shell_State=SET_ETH_TIME;
break;
default:
break;
}
return (p-outputstr);
}
EllipsoidFitter::Calibration EllipsoidFitter::calculateFit(void) const
{
/*********************************************************************
First step: Fit a quadric to the point set by least-squares
minimization based on algebraic distance.
*********************************************************************/
/* Create the least-squares system: */
Math::Matrix a(10,10,0.0);
/* Process all points: */
for(Misc::ChunkedArray<Point>::const_iterator pIt=points.begin();pIt!=points.end();++pIt)
{
/* Create the point's associated linear equation: */
double eq[10];
eq[0]=(*pIt)[0]*(*pIt)[0];
eq[1]=2.0*(*pIt)[0]*(*pIt)[1];
eq[2]=2.0*(*pIt)[0]*(*pIt)[2];
eq[3]=2.0*(*pIt)[0];
eq[4]=(*pIt)[1]*(*pIt)[1];
eq[5]=2.0*(*pIt)[1]*(*pIt)[2];
eq[6]=2.0*(*pIt)[1];
eq[7]=(*pIt)[2]*(*pIt)[2];
eq[8]=2.0*(*pIt)[2];
eq[9]=1.0;
/* Insert the equation into the least-squares system: */
for(unsigned int i=0;i<10;++i)
for(unsigned int j=0;j<10;++j)
a(i,j)+=eq[i]*eq[j];
}
/* Find the least-squares system's smallest eigenvalue: */
std::pair<Math::Matrix,Math::Matrix> qe=a.jacobiIteration();
unsigned int minEIndex=0;
double minE=Math::abs(qe.second(0,0));
for(unsigned int i=1;i<10;++i)
{
if(minE>Math::abs(qe.second(i,0)))
{
minEIndex=i;
minE=Math::abs(qe.second(i,0));
}
}
/* Create the quadric's defining matrices: */
Math::Matrix qa(3,3);
qa(0,0)=qe.first(0,minEIndex);
qa(0,1)=qe.first(1,minEIndex);
qa(0,2)=qe.first(2,minEIndex);
qa(1,0)=qe.first(1,minEIndex);
qa(1,1)=qe.first(4,minEIndex);
qa(1,2)=qe.first(5,minEIndex);
qa(2,0)=qe.first(2,minEIndex);
qa(2,1)=qe.first(5,minEIndex);
qa(2,2)=qe.first(7,minEIndex);
Math::Matrix qb(3,1);
qb(0)=qe.first(3,minEIndex);
qb(1)=qe.first(6,minEIndex);
qb(2)=qe.first(8,minEIndex);
double qc=qe.first(9,minEIndex);
/* Calculate the quadric's principal axes: */
qe=qa.jacobiIteration();
std::cout<<std::fixed<<std::setprecision(6);
std::cout<<std::setw(9)<<qe.first<<std::endl;
std::cout<<std::setw(9)<<qe.second<<std::endl<<std::endl;
std::cout.unsetf(std::ios_base::floatfield);
/* "Complete the square" to calculate the quadric's centroid and radii: */
Math::Matrix qbp=qb.divideFullPivot(qe.first);
Math::Matrix cp(3,1);
for(int i=0;i<3;++i)
cp(i)=-qbp(i)/qe.second(i);
Math::Matrix c=qe.first*cp;
std::cout<<"Centroid: "<<c(0)<<", "<<c(1)<<", "<<c(2)<<std::endl;
double rhs=-qc;
for(int i=0;i<3;++i)
rhs+=Math::sqr(qbp(i))/qe.second(i);
double radii[3];
for(int i=0;i<3;++i)
radii[i]=Math::sqrt(rhs/qe.second(i));
std::cout<<"Radii: "<<radii[0]<<", "<<radii[1]<<", "<<radii[2]<<std::endl;
Scalar averageRadius=Math::pow(radii[0]*radii[1]*radii[2],1.0/3.0);
std::cout<<"Average radius: "<<averageRadius<<std::endl;
/* Calculate the calibration matrix: */
Math::Matrix ellP(4,4,1.0);
for(int i=0;i<3;++i)
for(int j=0;j<3;++j)
ellP(i,j)=qe.first(i,j);
Math::Matrix ellScale(4,4,1.0);
for(int i=0;i<3;++i)
ellScale(i,i)=averageRadius/radii[i];
Math::Matrix ell=ellP;
ell.makePrivate();
for(int i=0;i<3;++i)
ell(i,3)=c(i);
Math::Matrix ellInv=ell.inverseFullPivot();
Math::Matrix calib=ellP*ellScale*ellInv;
std::cout<<std::fixed<<std::setprecision(6);
std::cout<<std::setw(9)<<calib<<std::endl;
std::cout.unsetf(std::ios_base::floatfield);
/* Calculate the calibration residual: */
double rms=0.0;
for(Misc::ChunkedArray<Point>::const_iterator pIt=points.begin();pIt!=points.end();++pIt)
{
/* Calibrate the point: */
Math::Matrix p(4,1);
for(int i=0;i<3;++i)
p(i)=(*pIt)[i];
p(3)=1.0;
Math::Matrix cp=calib*p;
rms+=Math::sqr(Math::sqrt(Math::sqr(cp(0))+Math::sqr(cp(1))+Math::sqr(cp(2)))-averageRadius);
}
std::cout<<"Calibration residual: "<<Math::sqrt(rms/double(points.size()))<<std::endl;
/* Create and return the calibration result: */
Matrix result;
for(int i=0;i<3;++i)
for(int j=0;j<4;++j)
result(i,j)=calib(i,j);
return Calibration(result,averageRadius);
}
/// Render and return calibration
Calibration Tuning::renderCalibration() const {
return Calibration(*this);
}
//*****************************************************************************
//
// Main application entry point.
//
//*****************************************************************************
int main(void) {
int_fast32_t i32IPart[17], i32FPart[17];
uint_fast32_t ui32Idx, ui32CompDCMStarted;
float pfData[17];
float *pfAccel, *pfGyro, *pfMag, *pfEulers, *pfQuaternion;
float *direction;
//
// Initialize convenience pointers that clean up and clarify the code
// meaning. We want all the data in a single contiguous array so that
// we can make our pretty printing easier later.
//
pfAccel = pfData;
pfGyro = pfData + 3;
pfMag = pfData + 6;
pfEulers = pfData + 9;
pfQuaternion = pfData + 12;
direction = pfData + 16;
//
// Setup the system clock to run at 40 Mhz from PLL with crystal reference
//
ROM_SysCtlClockSet(
SYSCTL_SYSDIV_5 | SYSCTL_USE_PLL | SYSCTL_XTAL_16MHZ
| SYSCTL_OSC_MAIN);
//
// Enable port E used for motion interrupt.
//
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOE);
//
// Enable port F used for calibration.
//
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF);
//
// Initialize the UART.
//
ConfigureUART();
/* EEPROM SETTINGS */
SysCtlPeripheralEnable(SYSCTL_PERIPH_EEPROM0); // EEPROM activate
EEPROMInit(); // EEPROM start
//
// Print the welcome message to the terminal.
//
UARTprintf("\033[2JMPU9150 Raw Example\n");
//
// Set the color to a purple approximation.
//
g_pui32Colors[RED] = 0x8000;
g_pui32Colors[BLUE] = 0x8000;
g_pui32Colors[GREEN] = 0x8000;
//
// Initialize RGB driver.
//
RGBInit(0);
RGBColorSet(g_pui32Colors);
RGBIntensitySet(0.5f);
RGBEnable();
// Initialize BGLib
bglib_output = output;
ConfigureBLE();
//
// The I2C3 peripheral must be enabled before use.
//
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_I2C3);
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOD);
//
// Configure the pin muxing for I2C3 functions on port D0 and D1.
//
ROM_GPIOPinConfigure(GPIO_PD0_I2C3SCL);
ROM_GPIOPinConfigure(GPIO_PD1_I2C3SDA);
//
// Select the I2C function for these pins. This function will also
// configure the GPIO pins pins for I2C operation, setting them to
// open-drain operation with weak pull-ups. Consult the data sheet
// to see which functions are allocated per pin.
//
GPIOPinTypeI2CSCL(GPIO_PORTD_BASE, GPIO_PIN_0);
ROM_GPIOPinTypeI2C(GPIO_PORTD_BASE, GPIO_PIN_1);
//
// Configure and Enable the GPIO interrupt. Used for INT signal from the
// MPU9150
//
ROM_GPIOPinTypeGPIOInput(GPIO_PORTE_BASE, GPIO_PIN_2);
GPIOIntEnable(GPIO_PORTE_BASE, GPIO_PIN_2);
ROM_GPIOIntTypeSet(GPIO_PORTE_BASE, GPIO_PIN_2, GPIO_FALLING_EDGE);
ROM_IntEnable(INT_GPIOE);
//
// Keep only some parts of the systems running while in sleep mode.
// GPIOE is for the MPU9150 interrupt pin.
// UART0 is the virtual serial port
// TIMER0, TIMER1 and WTIMER5 are used by the RGB driver
// I2C3 is the I2C interface to the ISL29023
//
ROM_SysCtlPeripheralClockGating(true);
ROM_SysCtlPeripheralSleepEnable(SYSCTL_PERIPH_GPIOE);
ROM_SysCtlPeripheralSleepEnable(SYSCTL_PERIPH_UART0);
ROM_SysCtlPeripheralSleepEnable(SYSCTL_PERIPH_TIMER0);
ROM_SysCtlPeripheralSleepEnable(SYSCTL_PERIPH_TIMER1);
ROM_SysCtlPeripheralSleepEnable(SYSCTL_PERIPH_I2C3);
ROM_SysCtlPeripheralSleepEnable(SYSCTL_PERIPH_WTIMER5);
//
// Enable interrupts to the processor.
//
ROM_IntMasterEnable();
//
// Initialize I2C3 peripheral.
//
I2CMInit(&g_sI2CInst, I2C3_BASE, INT_I2C3, 0xff, 0xff,
ROM_SysCtlClockGet());
//
// Initialize the MPU9150 Driver.
//
MPU9150Init(&g_sMPU9150Inst, &g_sI2CInst, MPU9150_I2C_ADDRESS,
MPU9150AppCallback, &g_sMPU9150Inst);
//
// Wait for transaction to complete
//
MPU9150AppI2CWait(__FILE__, __LINE__);
//
// Configure the sampling rate to 1000 Hz / (1+24).
//
g_sMPU9150Inst.pui8Data[0] = 24;
MPU9150Write(&g_sMPU9150Inst, MPU9150_O_SMPLRT_DIV, g_sMPU9150Inst.pui8Data,
1, MPU9150AppCallback, &g_sMPU9150Inst);
//
// Wait for transaction to complete
//
MPU9150AppI2CWait(__FILE__, __LINE__);
//
// Write application specifice sensor configuration such as filter settings
// and sensor range settings.
//
g_sMPU9150Inst.pui8Data[0] = MPU9150_CONFIG_DLPF_CFG_94_98;
g_sMPU9150Inst.pui8Data[1] = MPU9150_GYRO_CONFIG_FS_SEL_250;
g_sMPU9150Inst.pui8Data[2] = (MPU9150_ACCEL_CONFIG_ACCEL_HPF_5HZ
| MPU9150_ACCEL_CONFIG_AFS_SEL_2G);
// g_sMPU9150Inst.pui8Data[2] = MPU9150_ACCEL_CONFIG_AFS_SEL_2G;
MPU9150Write(&g_sMPU9150Inst, MPU9150_O_CONFIG, g_sMPU9150Inst.pui8Data, 3,
MPU9150AppCallback, &g_sMPU9150Inst);
//
// Wait for transaction to complete
//
MPU9150AppI2CWait(__FILE__, __LINE__);
//
// Configure the data ready interrupt pin output of the MPU9150.
//
g_sMPU9150Inst.pui8Data[0] = MPU9150_INT_PIN_CFG_INT_LEVEL
| MPU9150_INT_PIN_CFG_INT_RD_CLEAR
| MPU9150_INT_PIN_CFG_LATCH_INT_EN;
g_sMPU9150Inst.pui8Data[1] = MPU9150_INT_ENABLE_DATA_RDY_EN;
MPU9150Write(&g_sMPU9150Inst, MPU9150_O_INT_PIN_CFG,
g_sMPU9150Inst.pui8Data, 2, MPU9150AppCallback, &g_sMPU9150Inst);
//
// Wait for transaction to complete
//
MPU9150AppI2CWait(__FILE__, __LINE__);
//
// Initialize the DCM system. 40 hz sample rate.
// accel weight = .2, gyro weight = .8, mag weight = .2
//
CompDCMInit(&g_sCompDCMInst, 1.0f / 40.0f, 0.2f, 0.6f, 0.2f);
//
// Enable blinking indicates config finished successfully
//
RGBBlinkRateSet(1.0f);
//
// Configure and Enable the GPIO interrupt. Used for calibration
//
HWREG(GPIO_PORTF_BASE + GPIO_O_LOCK) = GPIO_LOCK_KEY;
HWREG(GPIO_PORTF_BASE + GPIO_O_CR) |= 0x01;
ROM_GPIOPinTypeGPIOInput(GPIO_PORTF_BASE, GPIO_PIN_4);
GPIOPadConfigSet(GPIO_PORTF_BASE, GPIO_PIN_4, GPIO_STRENGTH_2MA,
GPIO_PIN_TYPE_STD_WPU);
ROM_IntEnable(INT_GPIOF);
ROM_GPIOIntTypeSet(GPIO_PORTF_BASE, GPIO_PIN_4, GPIO_FALLING_EDGE);
GPIOIntEnable(GPIO_PORTF_BASE, GPIO_PIN_4);
g_calibrationState = 0;
ui32CompDCMStarted = 0;
// Configure the white noise, read the error from EEPROM
EEPROMRead((uint32_t *) zeroErrorAccel,
EEPROM_ZERO_ERROR_ACCELERATION_ADDRESS, 12);
EEPROMRead((uint32_t *) linearErrorAccel,
EEPROM_LINEAR_ERROR_ACCELERATION_ADDRESS, 12);
EEPROMRead((uint32_t *) zeroErrorGyro, EEPROM_ZERO_ERROR_GYROSCOPE_ADDRESS,
12);
while (1) {
//
// Go to sleep mode while waiting for data ready.
//
while (!g_vui8I2CDoneFlag) {
//ROM_SysCtlSleep();
}
//
// Clear the flag
//
g_vui8I2CDoneFlag = 0;
//
// Get floating point version of the Accel Data in m/s^2.
//
MPU9150DataAccelGetFloat(&g_sMPU9150Inst, pfAccel, pfAccel + 1,
pfAccel + 2);
//
// Get floating point version of angular velocities in rad/sec
//
MPU9150DataGyroGetFloat(&g_sMPU9150Inst, pfGyro, pfGyro + 1,
pfGyro + 2);
//
// Get floating point version of magnetic fields strength in tesla
//
MPU9150DataMagnetoGetFloat(&g_sMPU9150Inst, pfMag, pfMag + 1,
pfMag + 2);
if (g_calibrationState == 2) {
zeroErrorAccel[0] = (pfAccel[0]
+ zeroErrorAccel[0] * g_calibrationCount)
/ (g_calibrationCount + 1);
zeroErrorAccel[1] = (pfAccel[1]
+ zeroErrorAccel[1] * g_calibrationCount)
/ (g_calibrationCount + 1);
accelAtGravity[2] = (pfAccel[2]
+ accelAtGravity[2] * g_calibrationCount)
/ (g_calibrationCount + 1);
zeroErrorGyro[0] = (pfGyro[0]
+ zeroErrorGyro[0] * g_calibrationCount)
/ (g_calibrationCount + 1);
zeroErrorGyro[1] = (pfGyro[1]
+ zeroErrorGyro[1] * g_calibrationCount)
/ (g_calibrationCount + 1);
zeroErrorGyro[2] = (pfGyro[2]
+ zeroErrorGyro[2] * g_calibrationCount)
/ (g_calibrationCount + 1);
g_calibrationCount++;
if (g_calibrationCount > 500) {
Calibration();
}
continue;
} else if (g_calibrationState == 4) {
zeroErrorAccel[2] = (pfAccel[2]
+ zeroErrorAccel[2] * g_calibrationCount)
/ (g_calibrationCount + 1);
accelAtGravity[1] = (pfAccel[1]
+ accelAtGravity[1] * g_calibrationCount)
/ (g_calibrationCount + 1);
g_calibrationCount++;
if (g_calibrationCount > 500) {
Calibration();
}
continue;
} else if (g_calibrationState == 6) {
accelAtGravity[0] = (pfAccel[0]
+ accelAtGravity[0] * g_calibrationCount)
/ (g_calibrationCount + 1);
g_calibrationCount++;
if (g_calibrationCount > 500) {
Calibration();
}
continue;
}
// Cancel out white noise
// pfAccel[0] = pfAccel[0] - zeroErrorAccel[0];
// pfAccel[1] = pfAccel[1] - zeroErrorAccel[1];
// pfAccel[2] = pfAccel[2] - zeroErrorAccel[2];
// pfGyro[0] = pfGyro[0] - zeroErrorGyro[0];
// pfGyro[1] = pfGyro[1] - zeroErrorGyro[1];
// pfGyro[2] = pfGyro[2] - zeroErrorGyro[2];
// // Straighten out linear noise
// pfAccel[0] = pfAccel[0] * (1 + linearErrorAccel[0]);
// pfAccel[1] = pfAccel[1] * (1 + linearErrorAccel[1]);
// pfAccel[2] = pfAccel[2] * (1 + linearErrorAccel[2]);
//
// Check if this is our first data ever.
//
if (ui32CompDCMStarted == 0) {
//
// Set flag indicating that DCM is started.
// Perform the seeding of the DCM with the first data set.
//
ui32CompDCMStarted = 1;
CompDCMMagnetoUpdate(&g_sCompDCMInst, pfMag[0], pfMag[1], pfMag[2]);
CompDCMAccelUpdate(&g_sCompDCMInst, pfAccel[0], pfAccel[1],
pfAccel[2]);
CompDCMGyroUpdate(&g_sCompDCMInst, pfGyro[0], pfGyro[1], pfGyro[2]);
CompDCMStart(&g_sCompDCMInst);
} else {
//
// DCM Is already started. Perform the incremental update.
//
CompDCMMagnetoUpdate(&g_sCompDCMInst, pfMag[0], pfMag[1], pfMag[2]);
CompDCMAccelUpdate(&g_sCompDCMInst, pfAccel[0], pfAccel[1],
pfAccel[2]);
CompDCMGyroUpdate(&g_sCompDCMInst, -pfGyro[0], -pfGyro[1],
-pfGyro[2]);
CompDCMUpdate(&g_sCompDCMInst);
}
//
// Increment the skip counter. Skip counter is used so we do not
// overflow the UART with data.
//
g_ui32PrintSkipCounter++;
if (g_ui32PrintSkipCounter >= PRINT_SKIP_COUNT) {
//
// Reset skip counter.
//
g_ui32PrintSkipCounter = 0;
//
// Get Euler data. (Roll Pitch Yaw)
//
CompDCMComputeEulers(&g_sCompDCMInst, pfEulers, pfEulers + 1,
pfEulers + 2);
//
// Get Quaternions.
//
CompDCMComputeQuaternion(&g_sCompDCMInst, pfQuaternion);
//
// convert mag data to micro-tesla for better human interpretation.
//
pfMag[0] *= 1e6;
pfMag[1] *= 1e6;
pfMag[2] *= 1e6;
//
// Convert Eulers to degrees. 180/PI = 57.29...
// Convert Yaw to 0 to 360 to approximate compass headings.
//
pfEulers[0] *= 57.295779513082320876798154814105f;
pfEulers[1] *= 57.295779513082320876798154814105f;
pfEulers[2] *= 57.295779513082320876798154814105f;
if (pfEulers[2] < 0) {
pfEulers[2] += 360.0f;
}
// Use pfMag to display degrees of the Magnetomer's x-axis
// (y-axis of accelerometer and gyroscope) to the east of
// magnetic north pole
// direction[0] = 0;
// if (pfMag[1] == 0) {
// if (pfMag[0] > 0) {
// direction[0] = 0;
// } else {
// direction[0] = 180;
// }
// } else if (pfMag[1] > 0) {
// direction[0] = 90 - atan2f(pfMag[0], pfMag[1]) * 180 / 3.14159265359;
// } else if (pfMag[1] < 0) {
// direction[0] = 270 - atan2f(pfMag[0], pfMag[1]) * 180 / 3.14159265359;
// }
//
// Now drop back to using the data as a single array for the
// purpose of decomposing the float into a integer part and a
// fraction (decimal) part.
//
for (ui32Idx = 0; ui32Idx < 17; ui32Idx++) {
//
// Conver float value to a integer truncating the decimal part.
//
i32IPart[ui32Idx] = (int32_t) pfData[ui32Idx];
//
// Multiply by 1000 to preserve first three decimal values.
// Truncates at the 3rd decimal place.
//
i32FPart[ui32Idx] = (int32_t) (pfData[ui32Idx] * 1000.0f);
//
// Subtract off the integer part from this newly formed decimal
// part.
//
i32FPart[ui32Idx] = i32FPart[ui32Idx]
- (i32IPart[ui32Idx] * 1000);
//
// make the decimal part a positive number for display.
//
if (i32FPart[ui32Idx] < 0) {
i32FPart[ui32Idx] *= -1;
}
}
if (g_bleUserFlag == 1) {
g_bleFlag = 0;
ble_cmd_attributes_write(58, 0, 12, (uint8_t*)pfEuler);
while (g_bleFlag == 0) {
}
} else if (g_bleDisconnectFlag == 1) {
ConfigureBLE();
}
//
// Print the acceleration numbers in the table.
//
// UARTprintf("%3d.%03d, ", i32IPart[0], i32FPart[0]);
// UARTprintf("%3d.%03d, ", i32IPart[1], i32FPart[1]);
// UARTprintf("%3d.%03d\n", i32IPart[2], i32FPart[2]);
//
// //
// // Print the angular velocities in the table.
// //
// UARTprintf("%3d.%03d, ", i32IPart[3], i32FPart[3]);
// UARTprintf("%3d.%03d, ", i32IPart[4], i32FPart[4]);
// UARTprintf("%3d.%03d\n", i32IPart[5], i32FPart[5]);
//
// //
// // Print the magnetic data in the table.
// //
// UARTprintf("%3d.%03d, ", i32IPart[6], i32FPart[6]);
// UARTprintf("%3d.%03d, ", i32IPart[7], i32FPart[7]);
// UARTprintf("%3d.%03d\n", i32IPart[8], i32FPart[8]);
//
// //
// // Print the direction in the table.
// //
// UARTprintf("%3d.%03d\n", i32IPart[16], i32FPart[16]);
// //
// // Print the Eulers in a table.
// //
// UARTprintf("%3d.%03d, ", i32IPart[9], i32FPart[9]);
// UARTprintf("%3d.%03d, ", i32IPart[10], i32FPart[10]);
// UARTprintf("%3d.%03d\n", i32IPart[11], i32FPart[11]);
//
// //
// // Print the quaternions in a table format.
// //
// UARTprintf("\033[19;14H%3d.%03d", i32IPart[12], i32FPart[12]);
// UARTprintf("\033[19;32H%3d.%03d", i32IPart[13], i32FPart[13]);
// UARTprintf("\033[19;50H%3d.%03d", i32IPart[14], i32FPart[14]);
// UARTprintf("\033[19;68H%3d.%03d", i32IPart[15], i32FPart[15]);
}
}
}
bool Communication::DataListening()
{
u8 ch=0;
u8 data[30]={0};
u8 check=0;
u8 num = usart.ReceiveBufferSize();
if(num>0)
{
usart.GetReceivedData(&ch,1);
if(ch == 0xaa)
{
usart.GetReceivedData(&ch,1);
if(ch == 0xaf)
{
//功能字判断
data[0] = 0xaa;
data[1] = 0xaf;
usart.GetReceivedData(&ch,1);
if(ch == 0x01)//命令集1
{
data[2]= 0x01;
while(usart.ReceiveBufferSize()<2);//等待数据
usart.GetReceivedData(data+3,3);
check=data[5];
if( Calibration(data,data[3]+4,check )) //如果校验正确
{
if(data[4] == 0x01) //ACC校准
{
mAcc_Calibrate = true;
return true;
}
else if(data[4] == 0x02)//GYRO校准
{
mGyro_Calibrate = true;
return true;
}
else if(data[4] == 0x04)//MAG校准
{
mMag_Calibrate = true;
return true;
}
else if(data[4] == 0xa0)//飞机锁定
{
mClockState = true;
reply(data[2],check);
return true;
}
else if(data[4] == 0xa1)//飞机解锁
{
mClockState = false;
reply(data[2],check);
return true;
}
else
{
//未知命令
return false;
}
}
else
return false; //校准错误
}
else if(ch == 0x02)//命令集2
{
data[2]= ch;
while(usart.ReceiveBufferSize()<3);//等待数据
usart.GetReceivedData(data+3,3);
check=data[5];
if( Calibration(data,data[3]+4,check )) //如果校验正确
{
if(data[4] == 0x01) //PID请求
{
mGetPid=true;
return true;
}
else if(data[4]==0XA0)//读取下位机版本信息
{
return true;
}
else
{
//未知命令
return false;
}
}
else //校验错误
return false;
}
else if(ch == 0x03)//控制信息
{
data[2]= ch;
while(usart.ReceiveBufferSize()<22);//等待数据
usart.GetReceivedData(data+3,22);
check=data[24];
if( Calibration(data,data[3]+4,check )) //如果校验正确
{
mRcvTargetThr =(u16)data[4]*256+ data[5];
mRcvTargetYaw =(u16)data[6]*256+ data[7];
mRcvTargetRoll =(u16)data[8]*256+ data[9];
mRcvTargetPitch =(u16)data[10]*256+ data[11];
return true;
}
return false;
}
else if(ch ==0x10 ||ch ==0x11 ||ch ==0x12) //PID更新
{
static u8 PIDnumber;
PIDnumber = ch -0x10;
if(PIDnumber>2) //防止PID数值越界
{
return false;
}
data[2]= ch;
while(usart.ReceiveBufferSize()<20);//等待数据
usart.GetReceivedData(data+3,20);
check=data[22];
if( Calibration(data,data[3]+4,check )) //如果校验正确
{
PID[PIDnumber][0]=((u16)data[4]*256+data[5])/1000.0;
PID[PIDnumber][1]=((u16)data[6]*256+data[7])/1000.0;
PID[PIDnumber][2]=((u16)data[8]*256+data[9])/1000.0;
PID[PIDnumber][3]=((u16)data[10]*256+data[11])/1000.0;
PID[PIDnumber][4]=((u16)data[12]*256+data[13])/1000.0;
PID[PIDnumber][5]=((u16)data[14]*256+data[15])/1000.0;
PID[PIDnumber][6]=((u16)data[16]*256+data[17])/1000.0;
PID[PIDnumber][7]=((u16)data[18]*256+data[19])/1000.0;
PID[PIDnumber][8]=((u16)data[20]*256+data[21])/1000.0;
reply(ch,check);
mPidUpdata = true;
return true;
}
else
return false;
}
else
{
//未知功能字
return false;
}
}
else
return false; //不是帧头
}
else
return false; //不是帧头
}
else
return false;//没接收到数据
}
