Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-08 10:50:19 +08:00
Code Issues Actions 24 Packages Projects Releases Wiki Activity

commander: add macros for log messages and wait

Browse Source 
As a current workaround we need to wait some time after publishing a
mavlink log message in order for it to arrive in QGC.
This commit is contained in:
Julian Oes
2016-04-05 09:57:16 +02:00
committed by Lorenz Meier
parent dff78830ac
commit f583f51027
2 changed files with 37 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/calibration_routines.cpp
+13 -15
View File
@@ -307,7 +307,7 @@ enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub,
/* is still now */
if (t_still == 0) {
/* first time */
mavlink_and_console_log_info(mavlink_log_pub, "[cal] detected rest position, hold still...");
calibration_log_info(mavlink_log_pub, "[cal] detected rest position, hold still...");
t_still = t;
t_timeout = t + timeout;
@@ -324,7 +324,7 @@ enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub,
accel_disp[2] > still_thr2 * 4.0f) {
/* not still, reset still start time */
if (t_still != 0) {
mavlink_and_console_log_info(mavlink_log_pub, "[cal] detected motion, hold still...");
calibration_log_info(mavlink_log_pub, "[cal] detected motion, hold still...");
usleep(200000);
t_still = 0;
}
@@ -339,7 +339,7 @@ enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub,
}
if (poll_errcount > 1000) {
mavlink_and_console_log_critical(mavlink_log_pub, CAL_ERROR_SENSOR_MSG);
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SENSOR_MSG);
return DETECT_ORIENTATION_ERROR;
}
}
@@ -380,7 +380,7 @@ enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub,
return DETECT_ORIENTATION_RIGHTSIDE_UP; // [ 0, 0, -g ]
}
mavlink_and_console_log_critical(mavlink_log_pub, "[cal] ERROR: invalid orientation");
calibration_log_critical(mavlink_log_pub, "[cal] ERROR: invalid orientation");
return DETECT_ORIENTATION_ERROR; // Can't detect orientation
}
@@ -413,7 +413,7 @@ calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub,
int sub_accel = orb_subscribe(ORB_ID(sensor_combined));
if (sub_accel < 0) {
mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "No onboard accel");
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "No onboard accel");
return calibrate_return_error;
}
@@ -428,7 +428,7 @@ calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub,
if (orientation_failures > 4) {
result = calibrate_return_error;
mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "timeout: no motion");
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "timeout: no motion");
break;
}
@@ -456,28 +456,26 @@ calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub,
strcat(pendingStr, detect_orientation_str((enum detect_orientation_return)cur_orientation));
}
}
mavlink_and_console_log_info(mavlink_log_pub, "[cal] pending:%s", pendingStr);
calibration_log_info(mavlink_log_pub, "[cal] pending:%s", pendingStr);
mavlink_and_console_log_info(mavlink_log_pub, "[cal] hold vehicle still on a pending side");
calibration_log_info(mavlink_log_pub, "[cal] hold vehicle still on a pending side");
enum detect_orientation_return orient = detect_orientation(mavlink_log_pub, cancel_sub, sub_accel, lenient_still_position);
if (orient == DETECT_ORIENTATION_ERROR) {
orientation_failures++;
mavlink_and_console_log_info(mavlink_log_pub, "[cal] detected motion, hold still...");
calibration_log_info(mavlink_log_pub, "[cal] detected motion, hold still...");
continue;
}
/* inform user about already handled side */
if (side_data_collected[orient]) {
orientation_failures++;
mavlink_and_console_log_critical(mavlink_log_pub, "%s side already completed", detect_orientation_str(orient));
mavlink_and_console_log_critical(mavlink_log_pub, "rotate to a pending side");
calibration_log_critical(mavlink_log_pub, "%s side already completed", detect_orientation_str(orient));
calibration_log_critical(mavlink_log_pub, "rotate to a pending side");
continue;
}
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_ORIENTATION_DETECTED_MSG, detect_orientation_str(orient));
// TODO FIXME: sleep here, so that QGC receives this with higher chance.
usleep(10000);
calibration_log_info(mavlink_log_pub, CAL_QGC_ORIENTATION_DETECTED_MSG, detect_orientation_str(orient));
orientation_failures = 0;
// Call worker routine
@@ -486,7 +484,7 @@ calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub,
break;
}
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_SIDE_DONE_MSG, detect_orientation_str(orient));
calibration_log_info(mavlink_log_pub, CAL_QGC_SIDE_DONE_MSG, detect_orientation_str(orient));
// Note that this side is complete
side_data_collected[orient] = true;
src/modules/commander/calibration_routines.h
+24
View File
@@ -113,3 +113,27 @@ void calibrate_cancel_unsubscribe(int cancel_sub);
/// Used to periodically check for a cancel command
bool calibrate_cancel_check(orb_advert_t *mavlink_log_pub, ///< uORB handle to write output to
int cancel_sub); ///< Cancel subcription fromcalibration_cancel_subscribe
// TODO FIXME: below are workarounds for QGC. The issue is that sometimes
// a mavlink log message is overwritten by the following one. A workaround
// is to wait for some time after publishing each message and hope that it
// gets sent out in the meantime.
#define calibration_log_info(_pub, _text, ...) \
do { \
mavlink_and_console_log_info(_pub, _text, ##__VA_ARGS__); \
usleep(5000); \
} while(0);
#define calibration_log_critical(_pub, _text, ...) \
do { \
mavlink_and_console_log_critical(_pub, _text, ##__VA_ARGS__); \
usleep(5000); \
} while(0);
#define calibration_log_emergency(_pub, _text, ...) \
do { \
mavlink_and_console_log_emergency(_pub, _text, ##__VA_ARGS__); \
usleep(5000); \
} while(0);