diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde
index 77c08eeb9208e61b77f56845bb76c5d389a8b5ef..5f1aea428740dbec8d55d1c58d344865182e2e77 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 269ededa1f438f00c3aef77c1d7d0fd480e71f58..5557d8656e299a7a37512690ad6104866423fe67 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 5d13b1728d50c26b50161b47423896bbc7431ee3..385c2cd1a36a24e26436cec2ef88ec4d9258af3a 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;
}