JD class for AGRA SHELL. Derived from Monitor base class. Controls JD monitor portion.

Files at this revision

API Documentation at this revision

Comitter:
mHelgeland
Date:
Wed May 09 21:07:57 2018 +0000
Parent:
31:b8d68d5e3cc1
Child:
33:88d3c42b4784
Commit message:
Yawrate upper limit

Changed in this revision

JD_Base/JD_Base.cpp Show annotated file Show diff for this revision Revisions of this file
--- a/JD_Base/JD_Base.cpp	Thu May 03 05:24:12 2018 +0000
+++ b/JD_Base/JD_Base.cpp	Wed May 09 21:07:57 2018 +0000
@@ -80,6 +80,7 @@
 
 void JDMonitor::CANrcv_Monitor_ISO(const DCANMsg &msgA)
 {
+    static double lastYaw = 0;
     //allow to go into test mode by receiving this test can message!
     if (msgA.id == 0x12345678) {
         if (msgA.len == 2) {
@@ -184,22 +185,29 @@
         //08FFFF1C  E1E0 0064 0065 00 FA roll=0 yaw=2.000
         //          E1E0 0067 0066 00 00 roll=6.000 yaw=4.000
         //yaw, roll 0x64=100 = NULL
-        if (msgA.data[0] == 0xE1) {
+        if (msgA.data[0] == 0xE1){// && msgA.data[0] == 0xDF) {
             got_jdGPSRollYaw = true;
             jdGPSRollYaw_lastGet = base_millis();
+            
             double r1 = msgA.data[2];
             r1 = r1 / 128;
             double r2 = msgA.data[3];
             r2 = (r2 - 100) * 2;
             double STRoll = r1 + r2;
-            monitorData.roll = STRoll;
-
+            
             r1 = msgA.data[4];
             r1 = r1 / 128;
             r2 = msgA.data[5];
             r2 = (r2 - 100) * 2;
             double STYaw = r1 + r2;
-            monitorData.yawRate = STYaw;
+
+            if(abs(STYaw < 90))
+            {
+                monitorData.roll = STRoll;
+                monitorData.yawRate = STYaw;
+            }
+            
+            base_writeSer(0, "$YAW,%X,%X,%0.2f\r\n", jdGPSRollYawID, msgA.data[1],STYaw);
         }
     } else if (msgA.id == jdGPSHeadingID) { //0CFEE81C
         double _gSpeed = msgA.data[3] * 255 + msgA.data[2];
@@ -208,7 +216,7 @@
 
         // STSpeed=_gSpeed;
         monitorData.speed = _gSpeed; //STSpeed;
-
+        
         unsigned int h1 = msgA.data[0]; //heading
         unsigned int h2 = msgA.data[1];
         double h1a = h1;
@@ -579,7 +587,7 @@
     msg.data[0] = 0x00;
     msg.data[1] = 0xEE;
     msg.data[2] = 0x00;
-    CANsend(msg);
+    //CANsend(msg);
     //now we have to collect answers on already used addresses for 250 ms
 }