fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-30 16:50:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander : hotplug sensor support, better failure reporting

Browse Source
This commit is contained in:
Kabir Mohammed
2015-11-10 19:33:14 +05:30
parent 8ae78ab093
commit 5fcfdb759c
8 changed files with 153 additions and 83 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/systemlib/rc_check.c
+26 -11
View File
@@ -52,7 +52,7 @@
#define RC_INPUT_MAP_UNMAPPED 0
int rc_calibration_check(int mavlink_fd)
int rc_calibration_check(int mavlink_fd, bool report_fail)
{
char nbuf[20];
@@ -74,7 +74,8 @@ int rc_calibration_check(int mavlink_fd)
param_t map_parm = param_find(rc_map_mandatory[j]);
if (map_parm == PARAM_INVALID) {
mavlink_log_critical(mavlink_fd, "RC ERR: PARAM %s MISSING", rc_map_mandatory[j]);
if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: PARAM %s MISSING", rc_map_mandatory[j]); }
/* give system time to flush error message in case there are more */
usleep(100000);
map_fail_count++;
@@ -86,14 +87,16 @@ int rc_calibration_check(int mavlink_fd)
param_get(map_parm, &mapping);
if (mapping > RC_INPUT_MAX_CHANNELS) {
mavlink_log_critical(mavlink_fd, "RC ERR: %s >= # CHANS", rc_map_mandatory[j]);
if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: %s >= # CHANS", rc_map_mandatory[j]); }
/* give system time to flush error message in case there are more */
usleep(100000);
map_fail_count++;
}
if (mapping == 0) {
mavlink_log_critical(mavlink_fd, "RC ERR: Mandatory %s is unmapped", rc_map_mandatory[j]);
if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: Mandatory %s is unmapped", rc_map_mandatory[j]); }
/* give system time to flush error message in case there are more */
usleep(100000);
map_fail_count++;
@@ -144,35 +147,44 @@ int rc_calibration_check(int mavlink_fd)
/* assert min..center..max ordering */
if (param_min < RC_INPUT_LOWEST_MIN_US) {
count++;
mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_MIN < %u", i + 1, RC_INPUT_LOWEST_MIN_US);
if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_MIN < %u", i + 1, RC_INPUT_LOWEST_MIN_US); }
/* give system time to flush error message in case there are more */
usleep(100000);
}
if (param_max > RC_INPUT_HIGHEST_MAX_US) {
count++;
mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_MAX > %u", i + 1, RC_INPUT_HIGHEST_MAX_US);
if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_MAX > %u", i + 1, RC_INPUT_HIGHEST_MAX_US); }
/* give system time to flush error message in case there are more */
usleep(100000);
}
if (param_trim < param_min) {
count++;
mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM < MIN (%d/%d)", i + 1, (int)param_trim, (int)param_min);
if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM < MIN (%d/%d)", i + 1, (int)param_trim, (int)param_min); }
/* give system time to flush error message in case there are more */
usleep(100000);
}
if (param_trim > param_max) {
count++;
mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM > MAX (%d/%d)", i + 1, (int)param_trim, (int)param_max);
if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM > MAX (%d/%d)", i + 1, (int)param_trim, (int)param_max); }
/* give system time to flush error message in case there are more */
usleep(100000);
}
/* assert deadzone is sane */
if (param_dz > RC_INPUT_MAX_DEADZONE_US) {
mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_DZ > %u", i + 1, RC_INPUT_MAX_DEADZONE_US);
if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_DZ > %u", i + 1, RC_INPUT_MAX_DEADZONE_US); }
/* give system time to flush error message in case there are more */
usleep(100000);
count++;
@@ -187,8 +199,11 @@ int rc_calibration_check(int mavlink_fd)
if (channels_failed) {
sleep(2);
mavlink_and_console_log_critical(mavlink_fd, "%d config error%s for %d RC channel%s.", total_fail_count,
(total_fail_count > 1) ? "s" : "", channels_failed, (channels_failed > 1) ? "s" : "");
if (report_fail) mavlink_and_console_log_critical(mavlink_fd, "%d config error%s for %d RC channel%s.",
total_fail_count,
(total_fail_count > 1) ? "s" : "", channels_failed, (channels_failed > 1) ? "s" : "");
usleep(100000);
}
src/modules/systemlib/rc_check.h
+1 -1
View File
@@ -47,6 +47,6 @@ __BEGIN_DECLS
* @return 0 / OK if RC calibration is ok, index + 1 of the first
* channel that failed else (so 1 == first channel failed)
*/
__EXPORT int rc_calibration_check(int mavlink_fd);
__EXPORT int rc_calibration_check(int mavlink_fd, bool report_fail);
__END_DECLS