yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 09:28:58 +00:00
Code Issues Packages Projects Releases Wiki Activity

drivers/modules: changes after mavlink_log change

Browse Source 
The mavlink_log API changes lead to changes in all drivers/modules using
it.
This commit is contained in:
Julian Oes
2016-03-15 18:25:02 +00:00
parent b49b012d35
commit bba0d0384d
62 changed files with 731 additions and 831 deletions
Download Patch File Download Diff File Expand all files Collapse all files

89
src/modules/commander/PreflightCheck.cpp
View File

@@ -54,6 +54,7 @@
#include <systemlib/err.h>
#include <systemlib/param/param.h>
#include <systemlib/rc_check.h>
#include <systemlib/mavlink_log.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_mag.h>
@@ -65,7 +66,6 @@
#include <uORB/topics/airspeed.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <mavlink/mavlink_log.h>
#include "PreflightCheck.h"
@@ -84,7 +84,7 @@ static int check_calibration(DevHandle &h, const char* param_template, int &devi
int ret = h.ioctl(SENSORIOCCALTEST, 0);
calibration_found = (ret == OK);
devid = h.ioctl(DEVIOCGDEVICEID, 0);
char s[20];
@@ -116,7 +116,7 @@ static int check_calibration(DevHandle &h, const char* param_template, int &devi
return !calibration_found;
}
static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id, bool report_fail)
static bool magnometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool optional, int &device_id, bool report_fail)
{
bool success = true;
@@ -128,8 +128,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in
if (!h.isValid()) {
if (!optional) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: NO MAG SENSOR #%u", instance);
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: NO MAG SENSOR #%u", instance);
}
}
@@ -140,8 +139,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in
if (ret) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: MAG #%u UNCALIBRATED", instance);
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: MAG #%u UNCALIBRATED", instance);
}
success = false;
goto out;
@@ -151,8 +149,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in
if (ret != OK) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: MAG #%u SELFTEST FAILED", instance);
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: MAG #%u SELFTEST FAILED", instance);
}
success = false;
goto out;
@@ -163,7 +160,7 @@ out:
return success;
}
static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic, int &device_id, bool report_fail)
static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool optional, bool dynamic, int &device_id, bool report_fail)
{
bool success = true;
@@ -175,8 +172,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
if (!h.isValid()) {
if (!optional) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: NO ACCEL SENSOR #%u", instance);
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: NO ACCEL SENSOR #%u", instance);
}
}
@@ -187,8 +183,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
if (ret) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED", instance);
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED", instance);
}
success = false;
goto out;
@@ -198,8 +193,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
if (ret != OK) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: ACCEL #%u TEST FAILED: %d", instance, ret);
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCEL #%u TEST FAILED: %d", instance, ret);
}
success = false;
goto out;
@@ -217,7 +211,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL RANGE, hold still on arming");
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCEL RANGE, hold still on arming");
}
/* this is frickin' fatal */
success = false;
@@ -225,7 +219,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
}
} else {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL READ");
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCEL READ");
}
/* this is frickin' fatal */
success = false;
@@ -239,7 +233,7 @@ out:
return success;
}
static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id, bool report_fail)
static bool gyroCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool optional, int &device_id, bool report_fail)
{
bool success = true;
@@ -251,8 +245,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
if (!h.isValid()) {
if (!optional) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: NO GYRO SENSOR #%u", instance);
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: NO GYRO SENSOR #%u", instance);
}
}
@@ -263,8 +256,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
if (ret) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: GYRO #%u UNCALIBRATED", instance);
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GYRO #%u UNCALIBRATED", instance);
}
success = false;
goto out;
@@ -274,8 +266,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
if (ret != OK) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: GYRO #%u SELFTEST FAILED", instance);
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GYRO #%u SELFTEST FAILED", instance);
}
success = false;
goto out;
@@ -286,7 +277,7 @@ out:
return success;
}
static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id, bool report_fail)
static bool baroCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool optional, int &device_id, bool report_fail)
{
bool success = true;
@@ -298,8 +289,7 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
if (!h.isValid()) {
if (!optional) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: NO BARO SENSOR #%u", instance);
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: NO BARO SENSOR #%u", instance);
}
}
@@ -312,8 +302,7 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
// int ret = check_calibration(fd, "CAL_BARO%u_ID");
// if (ret) {
// mavlink_and_console_log_critical(mavlink_fd,
// "PREFLIGHT FAIL: BARO #%u UNCALIBRATED", instance);
// mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: BARO #%u UNCALIBRATED", instance);
// success = false;
// goto out;
// }
@@ -324,7 +313,7 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
return success;
}
static bool airspeedCheck(int mavlink_fd, bool optional, bool report_fail)
static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool report_fail)
{
bool success = true;
int ret;
@@ -335,7 +324,7 @@ static bool airspeedCheck(int mavlink_fd, bool optional, bool report_fail)
if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) ||
(hrt_elapsed_time(&airspeed.timestamp) > (500 * 1000))) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING");
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING");
}
success = false;
goto out;
@@ -343,7 +332,7 @@ static bool airspeedCheck(int mavlink_fd, bool optional, bool report_fail)
if (fabsf(airspeed.confidence) < 0.99f) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: AIRSPEED SENSOR COMM ERROR");
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED SENSOR COMM ERROR");
}
success = false;
goto out;
@@ -351,7 +340,7 @@ static bool airspeedCheck(int mavlink_fd, bool optional, bool report_fail)
if (fabsf(airspeed.indicated_airspeed_m_s) > 6.0f) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE");
mavlink_and_console_log_critical(mavlink_log_pub, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE");
}
// XXX do not make this fatal yet
}
@@ -361,7 +350,7 @@ out:
return success;
}
static bool gnssCheck(int mavlink_fd, bool report_fail)
static bool gnssCheck(orb_advert_t *mavlink_log_pub, bool report_fail)
{
bool success = true;
@@ -385,7 +374,7 @@ static bool gnssCheck(int mavlink_fd, bool report_fail)
//Report failure to detect module
if (!success) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GPS RECEIVER MISSING");
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GPS RECEIVER MISSING");
}
}
@@ -393,7 +382,7 @@ static bool gnssCheck(int mavlink_fd, bool report_fail)
return success;
}
bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro,
bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkMag, bool checkAcc, bool checkGyro,
bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic, bool reportFailures)
{
bool failed = false;
@@ -409,7 +398,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
bool required = (i < max_mandatory_mag_count);
int device_id = -1;
if (!magnometerCheck(mavlink_fd, i, !required, device_id, reportFailures) && required) {
if (!magnometerCheck(mavlink_log_pub, i, !required, device_id, reportFailures) && required) {
failed = true;
}
@@ -421,7 +410,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
/* check if the primary device is present */
if (!prime_found && prime_id != 0) {
if (reportFailures) {
mavlink_and_console_log_critical(mavlink_fd, "Warning: Primary compass not found");
mavlink_and_console_log_critical(mavlink_log_pub, "Warning: Primary compass not found");
}
failed = true;
}
@@ -438,7 +427,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
bool required = (i < max_mandatory_accel_count);
int device_id = -1;
if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic, device_id, reportFailures) && required) {
if (!accelerometerCheck(mavlink_log_pub, i, !required, checkDynamic, device_id, reportFailures) && required) {
failed = true;
}
@@ -450,7 +439,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
/* check if the primary device is present */
if (!prime_found && prime_id != 0) {
if (reportFailures) {
mavlink_and_console_log_critical(mavlink_fd, "Warning: Primary accelerometer not found");
mavlink_and_console_log_critical(mavlink_log_pub, "Warning: Primary accelerometer not found");
}
failed = true;
}
@@ -467,7 +456,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
bool required = (i < max_mandatory_gyro_count);
int device_id = -1;
if (!gyroCheck(mavlink_fd, i, !required, device_id, reportFailures) && required) {
if (!gyroCheck(mavlink_log_pub, i, !required, device_id, reportFailures) && required) {
failed = true;
}
@@ -479,7 +468,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
/* check if the primary device is present */
if (!prime_found && prime_id != 0) {
if (reportFailures) {
mavlink_and_console_log_critical(mavlink_fd, "Warning: Primary gyro not found");
mavlink_and_console_log_critical(mavlink_log_pub, "Warning: Primary gyro not found");
}
failed = true;
}
@@ -496,7 +485,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
bool required = (i < max_mandatory_baro_count);
int device_id = -1;
if (!baroCheck(mavlink_fd, i, !required, device_id, reportFailures) && required) {
if (!baroCheck(mavlink_log_pub, i, !required, device_id, reportFailures) && required) {
failed = true;
}
@@ -506,10 +495,10 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
}
// TODO there is no logic in place to calibrate the primary baro yet
// // check if the primary device is present
// // check if the primary device is present
if (!prime_found && prime_id != 0) {
if (reportFailures) {
mavlink_and_console_log_critical(mavlink_fd, "warning: primary barometer not operational");
mavlink_and_console_log_critical(mavlink_log_pub, "warning: primary barometer not operational");
}
failed = true;
}
@@ -517,16 +506,16 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
/* ---- AIRSPEED ---- */
if (checkAirspeed) {
if (!airspeedCheck(mavlink_fd, true, reportFailures)) {
if (!airspeedCheck(mavlink_log_pub, true, reportFailures)) {
failed = true;
}
}
/* ---- RC CALIBRATION ---- */
if (checkRC) {
if (rc_calibration_check(mavlink_fd, reportFailures) != OK) {
if (rc_calibration_check(mavlink_log_pub, reportFailures) != OK) {
if (reportFailures) {
mavlink_and_console_log_critical(mavlink_fd, "RC calibration check failed");
mavlink_and_console_log_critical(mavlink_log_pub, "RC calibration check failed");
}
failed = true;
}
@@ -534,7 +523,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
/* ---- Global Navigation Satellite System receiver ---- */
if (checkGNSS) {
if (!gnssCheck(mavlink_fd, reportFailures)) {
if (!gnssCheck(mavlink_log_pub, reportFailures)) {
failed = true;
}
}