|
@@ -63,7 +63,8 @@ void Sub::userhook_50Hz()
|
|
#ifdef USERHOOK_MEDIUMLOOP
|
|
#ifdef USERHOOK_MEDIUMLOOP
|
|
void Sub::userhook_MediumLoop()
|
|
void Sub::userhook_MediumLoop()
|
|
{
|
|
{
|
|
- // put your 10Hz code here
|
|
|
|
|
|
+ //read_external_DVL(hal.uartD);//uart4
|
|
|
|
+ //write_external_DVL(hal.uartD);
|
|
}
|
|
}
|
|
#endif
|
|
#endif
|
|
|
|
|
|
@@ -167,6 +168,34 @@ float Sub::getgainf(float data){
|
|
}
|
|
}
|
|
return temp;
|
|
return temp;
|
|
}
|
|
}
|
|
|
|
+float Sub::getgainfstatble(float data){
|
|
|
|
+ float temp =0;
|
|
|
|
+
|
|
|
|
+ if (data>=-0.5 && data <-0.08)
|
|
|
|
+ {
|
|
|
|
+ temp = -0.2;
|
|
|
|
+ }else if(data>=-0.8 && data<-0.5 )
|
|
|
|
+ {
|
|
|
|
+ temp = -0.5;
|
|
|
|
+ }else if(data>=-1.0 && data<-0.8 )
|
|
|
|
+ {
|
|
|
|
+ temp = -1.0;
|
|
|
|
+ }
|
|
|
|
+ else if (data<=0.5 && data >0.08)
|
|
|
|
+ {
|
|
|
|
+ temp = 0.2;
|
|
|
|
+ }else if(data<=0.8 && data>0.5 )
|
|
|
|
+ {
|
|
|
|
+ temp = 0.5;
|
|
|
|
+ }else if(data<=1.0 && data>0.8 )
|
|
|
|
+ {
|
|
|
|
+ temp = 1.0;
|
|
|
|
+ }else{
|
|
|
|
+ temp = 0.0;
|
|
|
|
+ }
|
|
|
|
+ return temp;
|
|
|
|
+}
|
|
|
|
+
|
|
float Sub::sign_in(float f){
|
|
float Sub::sign_in(float f){
|
|
return (f<0)? -1 :1;
|
|
return (f<0)? -1 :1;
|
|
}
|
|
}
|
|
@@ -212,7 +241,7 @@ float Sub::gaindelay(float gain1){
|
|
if (count>400)
|
|
if (count>400)
|
|
{
|
|
{
|
|
count = 0;
|
|
count = 0;
|
|
- gcs().send_text(MAV_SEVERITY_INFO, "conti %d %d %d \n",(int)continuous_count,(int)last_continuous,(int)continuous_countdec);
|
|
|
|
|
|
+ gcs().send_text(MAV_SEVERITY_INFO, "conti %d %d %d %f\n",(int)continuous_count,(int)last_continuous,(int)continuous_countdec,result);
|
|
|
|
|
|
}
|
|
}
|
|
|
|
|
|
@@ -428,6 +457,285 @@ float Sub::get_pitch_to_ned(Matrix3f mat_body){
|
|
}
|
|
}
|
|
return pitch_ang;
|
|
return pitch_ang;
|
|
}
|
|
}
|
|
|
|
+void Sub::charArrtobyteArr(unsigned char *charArr ,uint8_t * dec,int16_t len){
|
|
|
|
+ int16_t i = 0;
|
|
|
|
+ for(i = 0 ;i<len;i++){
|
|
|
|
+ dec[i] = (uint8_t)charArr[i] ;
|
|
|
|
+ }
|
|
|
|
+}
|
|
|
|
+
|
|
|
|
+void Sub::charArrtodecArr(unsigned char *charArr ,uint8_t * dec,int16_t len){
|
|
|
|
+
|
|
|
|
+ int16_t i = 0;
|
|
|
|
+ uint8_t j=0;
|
|
|
|
+ uint8_t k=0;
|
|
|
|
+ char charArrtemp[20]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
|
|
|
|
+
|
|
|
|
|
|
|
|
+ static uint8_t cnt = 0;
|
|
|
|
+ cnt = 0;
|
|
|
|
+ int typeflag = 0;
|
|
|
|
+ for(i = 4 ;i<len;i++){
|
|
|
|
+
|
|
|
|
+ if (charArr[i]!=',' && charArr[i]!=';'&& charArr[i]!='*')//&&charArr[i]!='y'&&charArr[i]!='n')//分割数据
|
|
|
|
+ {
|
|
|
|
+ charArrtemp[cnt++] = charArr[i];
|
|
|
|
+ if (charArr[i] == '.')//不是整形
|
|
|
|
+ {
|
|
|
|
+ typeflag = 1;
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+ }else{
|
|
|
|
+ charArrtemp[cnt] = '\0';
|
|
|
|
+ if (charArr[i-1]=='y' ||charArr[i-1]=='n')
|
|
|
|
+ {
|
|
|
|
+ arryf[j++] = charArr[i-1];
|
|
|
|
+ }
|
|
|
|
+ else if(typeflag == 0 && cnt>11)//长长整型 只有时间戳才会有这样的长整形
|
|
|
|
+ {
|
|
|
|
+ int64_t l = atoll (charArrtemp);
|
|
|
|
+ timearry[k++] = l;
|
|
|
|
+ }else if(cnt>11){//双精度 转换的话会丢掉精度 因此变成整形不会丢失精度
|
|
|
|
+ int il, jl,kl=0;
|
|
|
|
+ for (il = 0, jl = 0; charArrtemp[il] != '\0'; il++) {
|
|
|
|
+ if (charArrtemp[il] != '.') {
|
|
|
|
+ charArrtemp[jl++] = charArrtemp[il];
|
|
|
|
+ }else{
|
|
|
|
+ kl = il;//找出.的位置
|
|
|
|
+ }
|
|
|
|
+ }
|
|
|
|
+ charArrtemp[jl] = '\0';
|
|
|
|
+ DVLdata_wrp.time_Mulfactor = jl-kl;
|
|
|
|
+ int64_t l = atoll (charArrtemp);
|
|
|
|
+ timearry[2] = l;
|
|
|
|
+ //gcs().send_text(MAV_SEVERITY_INFO, "kl %02x %lld",DVLdata_wrp.time_Mulfactor,timearry[2]);
|
|
|
|
+ }
|
|
|
|
+ else{
|
|
|
|
+ float f = atof(charArrtemp);
|
|
|
|
+ arryf[j++] = f;
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+ memset(charArrtemp, 0, sizeof(unsigned char)*20);
|
|
|
|
+ cnt = 0;
|
|
|
|
+ typeflag = 0;
|
|
|
|
+
|
|
|
|
+ }
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+
|
|
|
|
+}
|
|
|
|
+void Sub:: write_external_DVL(AP_HAL::UARTDriver *uart){
|
|
|
|
+ if (uart == nullptr) {
|
|
|
|
+ return;
|
|
|
|
+ }
|
|
|
|
+ //unsigned char send_sucess = 0;
|
|
|
|
+ /*if (DVLSet.type == 'D')
|
|
|
|
+ {
|
|
|
|
+ //send_sucess = uart->write(DVLSet.data,DVLSet.len);
|
|
|
|
+ uart->write(DVLSet.data,DVLSet.len);
|
|
|
|
+ DVLSet.type = '0';
|
|
|
|
+
|
|
|
|
+ }*/
|
|
|
|
+ static int jsn = 0;
|
|
|
|
+ jsn++;
|
|
|
|
+ if(jsn>100)
|
|
|
|
+ {
|
|
|
|
+ // gcs().send_text(MAV_SEVERITY_INFO, " send %c%c%c%c%c%c%c%c%c%c%c\n",DVLSet.data[0],DVLSet.data[1],DVLSet.data[2],DVLSet.data[3],DVLSet.data[4],DVLSet.data[5],DVLSet.data[6],DVLSet.data[7],DVLSet.data[8],DVLSet.data[9],DVLSet.data[10]);
|
|
|
|
+ // gcs().send_text(MAV_SEVERITY_INFO, " send %d\n",(int)send_sucess);
|
|
|
|
+ jsn=0;
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+}
|
|
|
|
+void Sub::read_external_DVL(AP_HAL::UARTDriver *uart){
|
|
|
|
+ if (uart == nullptr) {
|
|
|
|
+ return;
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+ uint32_t readtime=AP_HAL::micros();//微秒
|
|
|
|
+ static uint32_t DVLvalidtime=AP_HAL::millis();//毫秒
|
|
|
|
+ unsigned char received_char;
|
|
|
|
+ static bool exitLoop = FALSE;
|
|
|
|
+ exitLoop = FALSE;
|
|
|
|
+ while(uart->available()&&!exitLoop)//recive num
|
|
|
|
+ {
|
|
|
|
+ received_char = uart->read();
|
|
|
|
+
|
|
|
|
+ if (received_char == 'w')// 开始
|
|
|
|
+ {
|
|
|
|
+ RxDVL[0] = received_char;
|
|
|
|
+ }
|
|
|
|
+ if (RxDVL[0] == 'w' && received_char != '\r' && received_char != '\n')//得到开头 并且不是 尾 后开始存数据 '\r' 回车 '\n' 换行
|
|
|
|
+ {
|
|
|
|
+ RxDVL[DVL_RxCnt++]=received_char;//保存数据
|
|
|
|
+ }
|
|
|
|
+ if ((received_char == '\r') || (received_char == '\n'))//到达帧尾
|
|
|
|
+ {
|
|
|
|
+ if(RxDVL[0] == 'w' && RxDVL[1] == 'r'&&(RxDVL[2] == 'z'||RxDVL[2] == 'p'||RxDVL[2] == 'a'||RxDVL[2] == 'n'||RxDVL[2] == '?')){
|
|
|
|
+ //开始校验
|
|
|
|
+ uint8_t stringobj[DVL_RxCnt];
|
|
|
|
+ charArrtobyteArr(RxDVL,stringobj,DVL_RxCnt);//转换成byte数组
|
|
|
|
+ uint8_t crcsum = crc_crc8(stringobj, DVL_RxCnt-3);//使用模拟测试字符数组
|
|
|
|
+ uint8_t dh = chartodec(RxDVL[DVL_RxCnt-2]);
|
|
|
|
+ uint8_t dl = chartodec(RxDVL[DVL_RxCnt-1]);
|
|
|
|
+ for (int gcsi2 = 0; gcsi2 < DVL_RxCnt/32 + 1; gcsi2++)
|
|
|
|
+ {
|
|
|
|
+ gcs().send_text(MAV_SEVERITY_INFO, "org %c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c",RxDVL[gcsi2*32+0],RxDVL[gcsi2*32+1],RxDVL[gcsi2*32+2],RxDVL[gcsi2*32+3],RxDVL[gcsi2*32+4],RxDVL[gcsi2*32+5],RxDVL[gcsi2*32+6],RxDVL[gcsi2*32+7],
|
|
|
|
+ RxDVL[gcsi2*32+8],RxDVL[gcsi2*32+9],RxDVL[gcsi2*32+10],RxDVL[gcsi2*32+11],RxDVL[gcsi2*32+12],RxDVL[gcsi2*32+13],RxDVL[gcsi2*32+14],RxDVL[gcsi2*32+15],
|
|
|
|
+ RxDVL[gcsi2*32+16],RxDVL[gcsi2*32+17],RxDVL[gcsi2*32+18],RxDVL[gcsi2*32+19],RxDVL[gcsi2*32+20],RxDVL[gcsi2*32+21],RxDVL[gcsi2*32+22],RxDVL[gcsi2*32+23],
|
|
|
|
+ RxDVL[gcsi2*32+24],RxDVL[gcsi2*32+25],RxDVL[gcsi2*32+26],RxDVL[gcsi2*32+27],RxDVL[gcsi2*32+28],RxDVL[gcsi2*32+29],RxDVL[gcsi2*32+30],RxDVL[gcsi2*32+31]);
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+
|
|
|
|
+ if (crcsum == dh*16+dl)
|
|
|
|
+ {//校验通过
|
|
|
|
+ charArrtodecArr(RxDVL,stringobj,DVL_RxCnt-2);//string 类型转为数据类型 计算数据
|
|
|
|
+ switch (RxDVL[2]) {
|
|
|
|
+ case 'z'://Velocity report
|
|
|
|
+ //Velocity report wrz, [vx],[vy],[vz],[valid],[altitude],[fom],[covariance],
|
|
|
|
+ //[time_of_validity],[time_of_transmission],[time],[status]
|
|
|
|
+ DVLdata_wrz.Vx = arryf[0];
|
|
|
|
+ DVLdata_wrz.Vy = arryf[1];
|
|
|
|
+ DVLdata_wrz.Vz = arryf[2];
|
|
|
|
+ DVLdata_wrz.valid = arryf[3];// 'y'/'n'
|
|
|
|
+ DVLdata_wrz.altitude = arryf[4];
|
|
|
|
+ DVLdata_wrz.fom = arryf[5];
|
|
|
|
+ DVLdata_wrz.covariancearr[0] = arryf[6];
|
|
|
|
+ DVLdata_wrz.covariancearr[1] = arryf[7];
|
|
|
|
+ DVLdata_wrz.covariancearr[2] = arryf[8];
|
|
|
|
+ DVLdata_wrz.covariancearr[3] = arryf[9];
|
|
|
|
+ DVLdata_wrz.covariancearr[4] = arryf[10];
|
|
|
|
+ DVLdata_wrz.covariancearr[5] = arryf[11];
|
|
|
|
+ DVLdata_wrz.covariancearr[6] = arryf[12];
|
|
|
|
+ DVLdata_wrz.covariancearr[7] = arryf[13];
|
|
|
|
+ DVLdata_wrz.covariancearr[8] = arryf[14];
|
|
|
|
+ DVLdata_wrz.timeofvalidity = timearry[0];
|
|
|
|
+ DVLdata_wrz.timeoftransmission = timearry[1];
|
|
|
|
+ DVLdata_wrz.time = arryf[15];
|
|
|
|
+ DVLdata_wrz.status = (int)arryf[16];
|
|
|
|
+ //树莓派源码中使用的是这一帧数据而不是wrp
|
|
|
|
+ gcs().send_text(MAV_SEVERITY_INFO, "z %d %c %.3f %.3f %.3f %.3f",DVLdata_wrz.status,DVLdata_wrz.valid,DVLdata_wrz.time,DVLdata_wrz.Vx,DVLdata_wrz.Vy,DVLdata_wrz.Vz) ;
|
|
|
|
+ gcs().send_text(MAV_SEVERITY_INFO, "z %lld %lld %.3f",DVLdata_wrz.timeofvalidity,DVLdata_wrz.timeoftransmission,DVLdata_wrz.time) ;
|
|
|
|
+
|
|
|
|
+ if(DVLdata_wrz.status == 0){//0正常 1 故障
|
|
|
|
+ //说明数据有效
|
|
|
|
+ if (DVLdata_wrz.valid == 'y')//说明高度和速度有效
|
|
|
|
+ {
|
|
|
|
+ //transfer_position_delta_msg();
|
|
|
|
+
|
|
|
|
+ DVLvalidtime=AP_HAL::millis();
|
|
|
|
+ gcs().send_text(MAV_SEVERITY_INFO, "wrz %ld",DVLvalidtime) ;
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+ }
|
|
|
|
+ exitLoop = TRUE;//退出循环
|
|
|
|
+ break;
|
|
|
|
+ case 'u'://Transducer report
|
|
|
|
+ // wru, [id],[velocity],[distance],[rssi],[nsd]
|
|
|
|
+ DVLdata_wru.id = (int)arryf[0];
|
|
|
|
+ DVLdata_wru.velocity = arryf[1];
|
|
|
|
+ DVLdata_wru.distance= arryf[2];
|
|
|
|
+ DVLdata_wru.rssi = (int)arryf[3];
|
|
|
|
+ DVLdata_wru.nsd = (int)arryf[4];
|
|
|
|
+ gcs().send_text(MAV_SEVERITY_INFO, "wru %d %f %f %d",DVLdata_wru.id,DVLdata_wru.velocity,DVLdata_wru.distance,DVLdata_wru.rssi) ;
|
|
|
|
+ exitLoop = TRUE;//退出循环
|
|
|
|
+ break;
|
|
|
|
+ case 'p'://Dead reckoning report
|
|
|
|
+ // wrp, [time_stamp],[x],[y],[z],[pos_std],[roll],[pitch],[yaw],[status]
|
|
|
|
+ DVLdata_wrp.time_stamp=timearry[2];//s 不能准确得到或者说不能准确打印出来
|
|
|
|
+ DVLdata_wrp.Dx = arryf[0];
|
|
|
|
+ DVLdata_wrp.Dy = arryf[1];
|
|
|
|
+ DVLdata_wrp.Dz = arryf[2];
|
|
|
|
+ DVLdata_wrp.pos_std = arryf[3];
|
|
|
|
+ DVLdata_wrp.roll = arryf[4];
|
|
|
|
+ DVLdata_wrp.pitch = arryf[5];
|
|
|
|
+ DVLdata_wrp.yaw = arryf[6];
|
|
|
|
+ DVLdata_wrp.status = (int)arryf[7];
|
|
|
|
+ gcs().send_text(MAV_SEVERITY_INFO, "wrp %f %f %f %f",DVLdata_wrp.Dx,DVLdata_wrp.Dy,DVLdata_wrp.Dz,DVLdata_wrp.pos_std) ;
|
|
|
|
+
|
|
|
|
+ exitLoop = TRUE;//退出循环
|
|
|
|
+ break;
|
|
|
|
+ //Dead reckoning can be reset by issuing the wcr command. The reply will be an ack ( wra ) if
|
|
|
|
+ //the reset is successful, and a nak ( wrn ) if not.
|
|
|
|
+ case 'a'://下发命令反馈 Successfully
|
|
|
|
+ /*memset(DVLreturn.data, 0, 32);
|
|
|
|
+ DVLreturn.type = 'V';
|
|
|
|
+ DVLreturn.len = 3;
|
|
|
|
+ DVLreturn.data[0] = 'w';
|
|
|
|
+ DVLreturn.data[1] = 'r';
|
|
|
|
+ DVLreturn.data[2] = 'a';*/
|
|
|
|
+ gcs().send_text(MAV_SEVERITY_INFO, "wra");
|
|
|
|
+ exitLoop = TRUE;//退出循环
|
|
|
|
+ break;
|
|
|
|
+ case ('n'||'?')://下发反馈 失败
|
|
|
|
+ /*memset(DVLreturn.data, 0, 32);
|
|
|
|
+ DVLreturn.type = 'V';
|
|
|
|
+ DVLreturn.len = 3;
|
|
|
|
+ DVLreturn.data[0] = 'w';
|
|
|
|
+ DVLreturn.data[1] = 'r';
|
|
|
|
+ DVLreturn.data[2] = 'n';*/
|
|
|
|
+ gcs().send_text(MAV_SEVERITY_INFO, "wrn");
|
|
|
|
+ exitLoop = TRUE;//退出循环
|
|
|
|
+ break;
|
|
|
|
+ case 'c'://读配置成功
|
|
|
|
+ /*memset(DVLreturn.data, 0, 32);
|
|
|
|
+ DVLreturn.type = 'V';
|
|
|
|
+ DVLreturn.len = DVL_RxCnt;
|
|
|
|
+ for (int copyi = 0; copyi < DVL_RxCnt; copyi++)
|
|
|
|
+ {
|
|
|
|
+ DVLreturn.data[copyi] = RxDVL[copyi];
|
|
|
|
+ }*/
|
|
|
|
+ gcs().send_text(MAV_SEVERITY_INFO, "wrc");
|
|
|
|
+ exitLoop = TRUE;//退出循环
|
|
|
|
+ break;
|
|
|
|
+
|
|
|
|
+ }
|
|
|
|
+ DVL_RxCnt =0;//数据处理完了
|
|
|
|
+ exitLoop = TRUE;//退出while
|
|
|
|
+ memset(RxDVL, 0, sizeof(unsigned char)*240);
|
|
|
|
+ }
|
|
|
|
+ else{
|
|
|
|
+ DVL_RxCnt =0;//校验错误
|
|
|
|
+ //exitLoop = TRUE;//退出while
|
|
|
|
+ memset(RxDVL, 0, sizeof(unsigned char)*240);
|
|
|
|
+ }
|
|
|
|
+ }
|
|
|
|
+ else{
|
|
|
|
+ DVL_RxCnt =0;
|
|
|
|
+ //exitLoop = TRUE;//退出while
|
|
|
|
+ //不是要接收数据的数据帧
|
|
|
|
+ memset(RxDVL, 0, sizeof(unsigned char)*240);
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+ }
|
|
|
|
+ if(AP_HAL::micros() - readtime >800)//时间溢出0.8ms micros是微秒
|
|
|
|
+ {//接收数据超时
|
|
|
|
+ DVL_RxCnt=0;
|
|
|
|
+ memset(RxDVL, 0, sizeof(unsigned char)*240);
|
|
|
|
+ gcs().send_text(MAV_SEVERITY_INFO, " E read timeout");
|
|
|
|
+ break;//退出while
|
|
|
|
+
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+ }
|
|
|
|
+ if (AP_HAL::millis() - DVLvalidtime>3000)//超过3秒没有可靠数据 millis是毫秒
|
|
|
|
+ {
|
|
|
|
+ DVLvalidtime = AP_HAL::micros();
|
|
|
|
+ //gcs().send_text(MAV_SEVERITY_WARNING, " DVL lost valid data");
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+
|
|
|
|
+}
|
|
|
|
+int Sub::chartodec(unsigned char _ch){
|
|
|
|
+ int dl = 0;
|
|
|
|
+ if(_ch>='a' && _ch<='f'){
|
|
|
|
+ dl = _ch-87;
|
|
|
|
+ }else if(_ch>='0' && _ch<='9'){
|
|
|
|
+ dl = _ch-48;
|
|
|
|
+ }else{
|
|
|
|
+ //不是数字
|
|
|
|
+ }
|
|
|
|
+ return dl;
|
|
|
|
+
|
|
|
|
+}
|
|
|
|
|
|
|
|
|