ultra update

src/decition/decition_brain_sf_jsrunlegs/decition/brain.cpp

@@ -153,7 +153,7 @@ iv::decition::BrainDecition::BrainDecition()
     mpaObsTraceLeft = iv::modulecomm::RegisterSend("obstraceleft",100000,1);
     mpaObsTraceRight = iv::modulecomm::RegisterSend("obstraceright",100000,1);
 
-    mpultraArea=iv::modulecomm::RegisterRecv("ultra-area",iv::decition::ListenUltraArea);//监听超声波数据
+    mpultraArea=iv::modulecomm::RegisterRecv("ultra_area",iv::decition::ListenUltraArea);//监听超声波数据
 
 
     mTime.start();

src/decition/decition_brain_sf_jsrunlegs/decition/decide_gps_00.cpp

@@ -2381,6 +2381,47 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
        ServiceCarStatus.mbNoPath=false;
        ServiceCarStatus.mbNoPathCnt=0;
     }
+
+    //超声波逻辑,begin
+   if(ServiceCarStatus.mbUltraDis[0]>=3.0&&ServiceCarStatus.mbUltraDis[0]<=5.0)
+   {
+      dSpeed=min(dSpeed,3.0);
+   }
+   else if(ServiceCarStatus.mbUltraDis[0]>=0.0&&ServiceCarStatus.mbUltraDis[0]<3.0)
+   {
+       minDecelerate=min(-1.0f,minDecelerate);
+   }
+
+/*   if((ServiceCarStatus.mbUltraDis[1]>=1.0&&ServiceCarStatus.mbUltraDis[1]<5.0)||
+      (ServiceCarStatus.mbUltraDis[3]>=1.0&&ServiceCarStatus.mbUltraDis[3]<5.0))
+   {
+      dSpeed=min(dSpeed,3.0);
+   }
+   else */
+   if((ServiceCarStatus.mbUltraDis[1]>=0.0&&ServiceCarStatus.mbUltraDis[1]<1.0)||
+           (ServiceCarStatus.mbUltraDis[3]>=0.0&&ServiceCarStatus.mbUltraDis[3]<1.0))
+   {
+      // minDecelerate=min(-0.5f,minDecelerate);
+       dSpeed=min(dSpeed,5.0);
+   }
+
+   if(ServiceCarStatus.daocheMode)
+   {
+       if(ServiceCarStatus.mbUltraDis[2]>=3.0&&ServiceCarStatus.mbUltraDis[2]<=5.0)
+       {
+          dSpeed=min(dSpeed,3.0);
+       }
+       else if(ServiceCarStatus.mbUltraDis[2]>=0.0&&ServiceCarStatus.mbUltraDis[2]<3.0)
+       {
+           minDecelerate=min(-1.0f,minDecelerate);
+       }
+   }
+//超声波逻辑,end
+
+   if((dSpeed-realspeed>2.0)&&(final_brake_lock==false))
+   {
+       dSpeed = min(realspeed+0.5,dSpeed) ;
+   }
 //junsheng ditu zhongdian
 
     if((realspeed>3.5)&&(dSpeed>3.5)&&(gfinal_brake==true)&&(final_brake_lock==false))
@@ -2388,11 +2429,6 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         dSpeed=realspeed-0.5;
     }
 
-    if((dSpeed-realspeed>1.0)&&(final_brake_lock==false))
-    {
-        dSpeed = min(realspeed+0.5,dSpeed) ;
-    }
-
     if(brake_mode==true){
            // dSpeed=min(dSpeed, realspeed);
        dSpeed=min(dSpeed, 3.0);
@@ -2417,39 +2453,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         }
     }
 
-    //超声波逻辑,begin
-   if(ServiceCarStatus.mbUltraDis[0]>=3.0&&ServiceCarStatus.mbUltraDis[0]<=5.0)
-   {
-      dSpeed=min(dSpeed,3.0);
-   }
-   else if(ServiceCarStatus.mbUltraDis[0]>=0.0&&ServiceCarStatus.mbUltraDis[0]<3.0)
-   {
-       minDecelerate=min(-1.0f,minDecelerate);
-   }
 
-   if((ServiceCarStatus.mbUltraDis[1]>=1.0&&ServiceCarStatus.mbUltraDis[1]<5.0)||
-      (ServiceCarStatus.mbUltraDis[3]>=1.0&&ServiceCarStatus.mbUltraDis[3]<5.0))
-   {
-      dSpeed=min(dSpeed,3.0);
-   }
-   else if((ServiceCarStatus.mbUltraDis[1]>=0.0&&ServiceCarStatus.mbUltraDis[1]<1.0)||
-           (ServiceCarStatus.mbUltraDis[3]>=0.0&&ServiceCarStatus.mbUltraDis[3]<1.0))
-   {
-       minDecelerate=min(-0.5f,minDecelerate);
-   }
-
-   if(ServiceCarStatus.daocheMode)
-   {
-       if(ServiceCarStatus.mbUltraDis[2]>=3.0&&ServiceCarStatus.mbUltraDis[2]<=5.0)
-       {
-          dSpeed=min(dSpeed,3.0);
-       }
-       else if(ServiceCarStatus.mbUltraDis[2]>=0.0&&ServiceCarStatus.mbUltraDis[2]<3.0)
-       {
-           minDecelerate=min(-1.0f,minDecelerate);
-       }
-   }
-//超声波逻辑,end
 
 
     std::cout<<"final desire speed="<<dSpeed<<std::endl;
@@ -2626,24 +2630,26 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                            <<"Decide_ACC"  << ","  <<setprecision(2)<<gps_decition->torque<< ","
                            <<"Decide_Brake"<< ","  <<gps_decition->brake<< ","
                            <<"Vehicle_RealSpd"<< ","  <<setprecision(2)<<now_gps_ins.speed<< ","
-                           <<"OBS_Distance"<< ","<<obsDistance_log<< ","
-                           <<"OBS_Speed"<< ","<<obs_speed_for_avoid<< ","
-                           <<"Vehicle_state"<< ","<<vehState<< ","
-                           <<"avoid_flag"<<","<<obstacle_avoid_flag<<","
-                           <<"avoidXNew"<<","<<avoidXNew<<","
-                           <<"avoidXNewPre"<<","<<avoidXNewPre<<","
-                           <<"avoidXPre"<<","<<avoidXPre<<","
-                           <<"ObsTimeEnd"<<","<<ObsTimeEnd<<","
-                           <<"avoidXY_size"<<","<<gpsTraceAvoidXY.size()<<","
+                           <<"zuo_ultra"<< ","<<ServiceCarStatus.mbUltraDis[3]<< ","
+                           <<"you_ultra"<< ","<<ServiceCarStatus.mbUltraDis[1]<< ","
+//                          <<"OBS_Distance"<< ","<<obsDistance_log<< ","
+//                           <<"OBS_Speed"<< ","<<obs_speed_for_avoid<< ","
+//                           <<"Vehicle_state"<< ","<<vehState<< ","
+//                           <<"avoid_flag"<<","<<obstacle_avoid_flag<<","
+//                           <<"avoidXNew"<<","<<avoidXNew<<","
+//                           <<"avoidXNewPre"<<","<<avoidXNewPre<<","
+//                           <<"avoidXPre"<<","<<avoidXPre<<","
+//                           <<"ObsTimeEnd"<<","<<ObsTimeEnd<<","
+//                           <<"avoidXY_size"<<","<<gpsTraceAvoidXY.size()<<","
                            <<"Min_Decelation"","<<minDecelerate<< ","
-                           <<"Decide_Angle"<< ","  << setprecision(2)<<gps_decition->wheel_angle<< ","
-                           <<"Vehicle_GPS_heading"<< ","<< setprecision(10)<<now_gps_ins.ins_heading_angle<< ","
-                           <<"Vehicle_GPS_X"<< ","<< setprecision(10)<<now_gps_ins.gps_lat<< ","
-                           <<"Vehicle_GPS_Y"<< ","<< setprecision(10)<<now_gps_ins.gps_lng<< ","
-                           <<"Trace_Point"<< ","<<PathPoint<< ","
-                           <<"OBS_Speed"<< ","<<obsSpeed<< ","
-                           <<"OBS_Distance"<< ","<<obsDistance<< ","
-                           <<"TTC"<< ","<<ttc<< ","
+//                           <<"Decide_Angle"<< ","  << setprecision(2)<<gps_decition->wheel_angle<< ","
+//                           <<"Vehicle_GPS_heading"<< ","<< setprecision(10)<<now_gps_ins.ins_heading_angle<< ","
+//                           <<"Vehicle_GPS_X"<< ","<< setprecision(10)<<now_gps_ins.gps_lat<< ","
+//                           <<"Vehicle_GPS_Y"<< ","<< setprecision(10)<<now_gps_ins.gps_lng<< ","
+//                           <<"Trace_Point"<< ","<<PathPoint<< ","
+//                           <<"OBS_Speed"<< ","<<obsSpeed<< ","
+//                           <<"OBS_Distance"<< ","<<obsDistance<< ","
+//                           <<"TTC"<< ","<<ttc<< ","
                            <<endl;
                    outfile.close();
                }

src/detection/detection_ultrasonic/main.cpp

@@ -37,18 +37,19 @@ int min(int ids[],int len)
 
 void ProcessData(iv::ultrasonic::ultrasonic xmsg)
 {
-        std::cout<<xmsg.sigobjdist_flside()<<" ";//4
-        std::cout<<xmsg.sigobjdist_flcorner()<<" ";//4
+
         std::cout<<xmsg.sigobjdist_flmiddle()<<" ";//1
-        std::cout<<xmsg.sigobjdist_frmiddle()<<" ";//1
+        std::cout<<xmsg.sigobjdist_frmiddle()<<"      ";//1
         std::cout<<xmsg.sigobjdist_frcorner()<<" ";//2
         std::cout<<xmsg.sigobjdist_frside()<<" ";//2
         std::cout<<xmsg.sigobjdist_rrside()<<" ";//2
-        std::cout<<xmsg.sigobjdist_rrcorner()<<" ";//2
+        std::cout<<xmsg.sigobjdist_rrcorner()<<"      ";//2
         std::cout<<xmsg.sigobjdist_rrmiddle()<<" ";//3
-        std::cout<<xmsg.sigobjdist_rlmiddle()<<" ";//3
+        std::cout<<xmsg.sigobjdist_rlmiddle()<<"      ";//3
         std::cout<<xmsg.sigobjdist_rlcorner()<<" ";//4
-        std::cout<<xmsg.sigobjdist_rlside()<<std::endl;//4
+        std::cout<<xmsg.sigobjdist_rlside()<<" ";//4
+        std::cout<<xmsg.sigobjdist_flside()<<" ";//4
+        std::cout<<xmsg.sigobjdist_flcorner()<<std::endl;//4
     iv::ultrasonic::ultraarea areas;
     areas.set_timestamp(xmsg.timestamp());
 
@@ -65,7 +66,7 @@ void ProcessData(iv::ultrasonic::ultrasonic xmsg)
         int min_dist = min(tmp,2);
         iv::ultrasonic::Area area;
         area.set_id(1);
-        area.set_dist(min_dist);
+        area.set_dist(min_dist*1.e-3);
         iv::ultrasonic::Area * p = areas.add_area();
         p->CopyFrom(area);
     }
@@ -84,7 +85,7 @@ void ProcessData(iv::ultrasonic::ultrasonic xmsg)
         int min_dist = min(tmp,4);
         iv::ultrasonic::Area area;
         area.set_id(2);
-        area.set_dist(min_dist);
+        area.set_dist(min_dist*1.e-3);
         iv::ultrasonic::Area * p = areas.add_area();
         p->CopyFrom(area);
     }
@@ -98,10 +99,11 @@ void ProcessData(iv::ultrasonic::ultrasonic xmsg)
         int tmp[2];
         tmp[0] = xmsg.sigobjdist_rrmiddle();
         tmp[1] = xmsg.sigobjdist_rlmiddle();
+
         int min_dist = min(tmp,2);
         iv::ultrasonic::Area area;
         area.set_id(3);
-        area.set_dist(min_dist);
+        area.set_dist(min_dist*1.e-3);
         iv::ultrasonic::Area * p = areas.add_area();
         p->CopyFrom(area);
     }
@@ -120,7 +122,7 @@ void ProcessData(iv::ultrasonic::ultrasonic xmsg)
         int min_dist = min(tmp,4);
         iv::ultrasonic::Area area;
         area.set_id(4);
-        area.set_dist(min_dist);
+        area.set_dist(min_dist*1.e-3);
         iv::ultrasonic::Area * p = areas.add_area();
         p->CopyFrom(area);
     }
@@ -181,7 +183,7 @@ int main(int argc, char *argv[])
     QCoreApplication a(argc, argv);
 
     std::string strmemultra = "sonar";
-    std::string strmemsend = "ultra_output";
+    std::string strmemsend = "ultra_area";
 
     givlog = new iv::Ivlog(strmemultra.data());
     givfault = new iv::Ivfault(strmemultra.data());

src/driver/driver_ultrasonic_dauxi_KS136A/canrecv_consumer.cpp

@@ -5,7 +5,7 @@
 extern setupConfig_t setupConfig;
 extern iv::msgunit shmSonar;
 
-//#define RESULT_OUTPUT_ENABLE
+#define RESULT_OUTPUT_ENABLE
 
 CANRecv_Consumer::CANRecv_Consumer(CAN_Producer_Consumer *pBuf)
 {
@@ -151,14 +151,18 @@ void CANRecv_Consumer::run()
 //                        std::cout<<"finish decode sensor "<<(int)decodeSensorID<<" result:"<<QDateTime::currentMSecsSinceEpoch()<<std::endl;
                         decodeTimer.restart();
                     }
+#ifdef RESULT_OUTPUT_ENABLE
+                    else if(tempDist == 0xEEEE)
+                    {
+                        std::cout<<"sensor: "<<(int)(decodeSensorID + 1)<<" may be disturbed."<<std::endl;
+                    }
+#endif
                 }
             }
         }
     }
 }
 
-#define RESULT_OUTPUT_ENABLE
-
 void CANRecv_Consumer::Enable_DecodeResult_Slot(bool enableFlag,uint8_t sensorID)
 {
     decodeEnableFlag = enableFlag;

src/driver/driver_ultrasonic_dauxi_KS136A/cansend_producer.cpp

@@ -78,5 +78,5 @@ void CANSend_Producer::Enable_Ask_Slot(bool enableFlag , uint8_t sensorID, uint8
 {
     askEnableFlag = enableFlag;
     askSensorID = sensorID;
-    askDistanceType = distanceType;
+    askDistanceType = 0; //distanceType;
 }

src/include/proto/ultraarea.proto

@@ -19,7 +19,7 @@ package iv.ultrasonic;
 
 message Area {
     optional int32 id = 1;   // id of area.
-    optional float dist = 2 [default = 50000]; // min dist in the area. (mm)
+    optional float dist = 2 [default = 50.0]; // min dist in the area. (m)
     optional bool valid = 3 [default = true]; // true -> working   |   false -> all ultras in the area don't work.
 };