From 787fd018b504522751202bd161b14d7debbded70 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Mon, 28 Oct 2013 17:21:35 +1100
Subject: [PATCH] Rover: fixed performance monitoring

now the same as plane
---
 APMrover2/APMrover2.pde |  29 ++++++------
 APMrover2/Log.pde       |   4 +-
 APMrover2/test.pde      | 101 +++++++++++++++++-----------------------
 3 files changed, 60 insertions(+), 74 deletions(-)

diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde
index 77c08eeb9..5f1aea428 100644
--- a/APMrover2/APMrover2.pde
+++ b/APMrover2/APMrover2.pde
@@ -523,7 +523,7 @@ static float G_Dt						= 0.02;
 // Timer used to accrue data and trigger recording of the performanc monitoring log message
 static int32_t 	perf_mon_timer;
 // The maximum main loop execution time recorded in the current performance monitoring interval
-static int16_t 	G_Dt_max = 0;
+static uint32_t 	G_Dt_max;
 // The number of gps fixes recorded in the current performance monitoring interval
 static uint8_t 	gps_fix_count = 0;
 // A variable used by developers to track performanc metrics.
@@ -534,12 +534,10 @@ static int16_t pmTest1 = 0;
 ////////////////////////////////////////////////////////////////////////////////
 // System Timers
 ////////////////////////////////////////////////////////////////////////////////
-// Time in miliseconds of start of main control loop.  Milliseconds
-static uint32_t 	fast_loopTimer;
-// Time Stamp when fast loop was complete.  Milliseconds
-static uint32_t 	fast_loopTimeStamp;
+// Time in microseconds of start of main control loop. 
+static uint32_t 	fast_loopTimer_us;
 // Number of milliseconds used in last main loop cycle
-static uint8_t 		delta_ms_fast_loop;
+static uint32_t		delta_us_fast_loop;
 // Counter of main loop executions.  Used for performance monitoring and failsafe processing
 static uint16_t			mainLoop_count;
 
@@ -617,20 +615,19 @@ void loop()
     if (!ins.wait_for_sample(1000)) {
         return;
     }
-    uint32_t timer = millis();
+    uint32_t timer = hal.scheduler->micros();
 
-    delta_ms_fast_loop	= timer - fast_loopTimer;
-    G_Dt                = (float)delta_ms_fast_loop / 1000.f;
-    fast_loopTimer      = timer;
+    delta_us_fast_loop	= timer - fast_loopTimer_us;
+    G_Dt                = delta_us_fast_loop * 1.0e-6f;
+    fast_loopTimer_us   = timer;
 
-	if (delta_ms_fast_loop > G_Dt_max)
-		G_Dt_max = delta_ms_fast_loop;
+	if (delta_us_fast_loop > G_Dt_max)
+		G_Dt_max = delta_us_fast_loop;
 
     mainLoop_count++;
 
     // tell the scheduler one tick has passed
     scheduler.tick();
-    fast_loopTimeStamp = millis();
 
     scheduler.run(19500U);
 }
@@ -767,7 +764,11 @@ static void one_second_loop(void)
     counter++;
 
     // write perf data every 20s
-    if (counter == 20) {
+    if (counter % 10 == 0) {
+        if (scheduler.debug() != 0) {
+            hal.console->printf_P(PSTR("G_Dt_max=%lu\n"), (unsigned long)G_Dt_max);
+        }
+        G_Dt_max = 0;
         if (g.log_bitmask & MASK_LOG_PM)
             Log_Write_Performance();
         resetPerfData();
diff --git a/APMrover2/Log.pde b/APMrover2/Log.pde
index 269ededa1..5557d8656 100644
--- a/APMrover2/Log.pde
+++ b/APMrover2/Log.pde
@@ -165,7 +165,7 @@ struct PACKED log_Performance {
     LOG_PACKET_HEADER;
     uint32_t loop_time;
     uint16_t main_loop_count;
-    int16_t  g_dt_max;
+    int32_t  g_dt_max;
     uint8_t  renorm_count;
     uint8_t  renorm_blowup;
     uint8_t  gps_fix_count;
@@ -456,7 +456,7 @@ static const struct LogStructure log_structure[] PROGMEM = {
     { LOG_ATTITUDE_MSG, sizeof(log_Attitude),       
       "ATT", "ccC",        "Roll,Pitch,Yaw" },
     { LOG_PERFORMANCE_MSG, sizeof(log_Performance), 
-      "PM",  "IHhBBBhhhhB", "LTime,MLC,gDt,RNCnt,RNBl,GPScnt,GDx,GDy,GDz,PMT,I2CErr" },
+      "PM",  "IHIBBBhhhhB", "LTime,MLC,gDt,RNCnt,RNBl,GPScnt,GDx,GDy,GDz,PMT,I2CErr" },
     { LOG_CMD_MSG, sizeof(log_Cmd),                 
       "CMD", "BBBBBeLL",   "CTot,CNum,CId,COpt,Prm1,Alt,Lat,Lng" },
     { LOG_CAMERA_MSG, sizeof(log_Camera),                 
diff --git a/APMrover2/test.pde b/APMrover2/test.pde
index 5d13b1728..385c2cd1a 100644
--- a/APMrover2/test.pde
+++ b/APMrover2/test.pde
@@ -341,39 +341,32 @@ test_ins(uint8_t argc, const Menu::arg *argv)
     uint8_t medium_loopCounter = 0;
 
 	while(1){
-		delay(20);
-		if (millis() - fast_loopTimer > 19) {
-			delta_ms_fast_loop 	= millis() - fast_loopTimer;
-			G_Dt 				= (float)delta_ms_fast_loop / 1000.f;		// used by DCM integrator
-			fast_loopTimer		= millis();
-
-			// INS
-			// ---
-			ahrs.update();
-
-			if(g.compass_enabled) {
-				medium_loopCounter++;
-				if(medium_loopCounter >= 5){
-					compass.read();
-                    medium_loopCounter = 0;
-				}
-			}
-
-			// We are using the IMU
-			// ---------------------
-            Vector3f gyros 	= ins.get_gyro();
-            Vector3f accels = ins.get_accel();
-			cliSerial->printf_P(PSTR("r:%4d  p:%4d  y:%3d  g=(%5.1f %5.1f %5.1f)  a=(%5.1f %5.1f %5.1f)\n"),
+        ins.wait_for_sample(1000);
+
+        ahrs.update();
+
+        if(g.compass_enabled) {
+            medium_loopCounter++;
+            if(medium_loopCounter >= 5){
+                compass.read();
+                medium_loopCounter = 0;
+            }
+        }
+        
+        // We are using the IMU
+        // ---------------------
+        Vector3f gyros 	= ins.get_gyro();
+        Vector3f accels = ins.get_accel();
+        cliSerial->printf_P(PSTR("r:%4d  p:%4d  y:%3d  g=(%5.1f %5.1f %5.1f)  a=(%5.1f %5.1f %5.1f)\n"),
                             (int)ahrs.roll_sensor / 100,
                             (int)ahrs.pitch_sensor / 100,
                             (uint16_t)ahrs.yaw_sensor / 100,
                             gyros.x, gyros.y, gyros.z,
                             accels.x, accels.y, accels.z);
-		}
-		if(cliSerial->available() > 0){
-			return (0);
-		}
-	}
+    }
+    if(cliSerial->available() > 0){
+        return (0);
+    }
 }
 
 
@@ -408,32 +401,25 @@ test_mag(uint8_t argc, const Menu::arg *argv)
     uint8_t medium_loopCounter = 0;
 
     while(1) {
-		delay(20);
-		if (millis() - fast_loopTimer > 19) {
-			delta_ms_fast_loop 	= millis() - fast_loopTimer;
-			G_Dt 				= (float)delta_ms_fast_loop / 1000.f;		// used by DCM integrator
-			fast_loopTimer		= millis();
-
-			// IMU
-			// ---
-			ahrs.update();
-
-            medium_loopCounter++;
-            if(medium_loopCounter >= 5){
-                if (compass.read()) {
-                    // Calculate heading
-                    Matrix3f m = ahrs.get_dcm_matrix();
-                    heading = compass.calculate_heading(m);
-                    compass.null_offsets();
-                }
-                medium_loopCounter = 0;
+        ins.wait_for_sample(1000);
+        ahrs.update();
+
+        medium_loopCounter++;
+        if(medium_loopCounter >= 5){
+            if (compass.read()) {
+                // Calculate heading
+                Matrix3f m = ahrs.get_dcm_matrix();
+                heading = compass.calculate_heading(m);
+                compass.null_offsets();
             }
-
-			counter++;
-			if (counter>20) {
-                if (compass.healthy) {
-                    Vector3f maggy = compass.get_offsets();
-                    cliSerial->printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d,\tXYZoff: %6.2f, %6.2f, %6.2f\n"),
+            medium_loopCounter = 0;
+        }
+        
+        counter++;
+        if (counter>20) {
+            if (compass.healthy) {
+                Vector3f maggy = compass.get_offsets();
+                cliSerial->printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d,\tXYZoff: %6.2f, %6.2f, %6.2f\n"),
                                     (wrap_360_cd(ToDeg(heading) * 100)) /100,
                                     (int)compass.mag_x,
                                     (int)compass.mag_y,
@@ -441,12 +427,11 @@ test_mag(uint8_t argc, const Menu::arg *argv)
                                     maggy.x,
                                     maggy.y,
                                     maggy.z);
-                } else {
-                    cliSerial->println_P(PSTR("compass not healthy"));
-                }
-                counter=0;
+            } else {
+                cliSerial->println_P(PSTR("compass not healthy"));
             }
-		}
+            counter=0;
+        }
         if (cliSerial->available() > 0) {
             break;
         }
-- 
GitLab