Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Rover: fixed performance monitoring 

now the same as plane
This commit is contained in:
Andrew Tridgell 2013-10-28 17:21:35 +11:00
parent 3ff407cb75
commit 787fd018b5
3 changed files with 57 additions and 71 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
APMrover2/APMrover2.pde
View File

@ -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();

4
APMrover2/Log.pde
View File

@ -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),

95
APMrover2/test.pde
View File

@ -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.wait_for_sample(1000);
// INS
// ---
ahrs.update();
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"),
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();
ins.wait_for_sample(1000);
ahrs.update();
// 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;
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;
}