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

Enabled DriverFramework drivers for SITL build

Browse Source 
The code here works only for SITL at the present time.

Signed-off-by: Mark Charlebois <charlebm@gmail.com>
This commit is contained in:
Mark Charlebois
2015-11-18 11:58:21 -08:00
parent d2cacb9bc6
commit b8c40ecb6b
14 changed files with 1099 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -69,21 +69,23 @@
#include "PreflightCheck.h"
#include "DevMgr.hpp"
using namespace DriverFramework;
namespace Commander
{
static int check_calibration(int fd, const char* param_template, int &devid);
int check_calibration(int fd, const char* param_template, int &devid)
static int check_calibration(DevHandle &h, const char* param_template, int &devid)
{
bool calibration_found;
/* new style: ask device for calibration state */
int ret = px4_ioctl(fd, SENSORIOCCALTEST, 0);
int ret = h.ioctl(SENSORIOCCALTEST, 0);
calibration_found = (ret == OK);
devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
devid = h.ioctl(DEVIOCGDEVICEID, 0);
char s[20];
int instance = 0;
@@ -120,9 +122,10 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in
char s[30];
sprintf(s, "%s%u", MAG_BASE_DEVICE_PATH, instance);
int fd = px4_open(s, 0);
DevHandle h;
DevMgr::getHandle(s, h);
if (fd < 0) {
if (!h.isValid()) {
if (!optional) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: NO MAG SENSOR #%u", instance);
@@ -131,7 +134,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in
return false;
}
int ret = check_calibration(fd, "CAL_MAG%u_ID", device_id);
int ret = check_calibration(h, "CAL_MAG%u_ID", device_id);
if (ret) {
mavlink_and_console_log_critical(mavlink_fd,
@@ -140,7 +143,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in
goto out;
}
ret = px4_ioctl(fd, MAGIOCSELFTEST, 0);
ret = h.ioctl(MAGIOCSELFTEST, 0);
if (ret != OK) {
mavlink_and_console_log_critical(mavlink_fd,
@@ -150,7 +153,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in
}
out:
px4_close(fd);
DevMgr::releaseHandle(h);
return success;
}
@@ -160,9 +163,10 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
char s[30];
sprintf(s, "%s%u", ACCEL_BASE_DEVICE_PATH, instance);
int fd = px4_open(s, O_RDONLY);
DevHandle h;
DevMgr::getHandle(s, h);
if (fd < 0) {
if (!h.isValid()) {
if (!optional) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: NO ACCEL SENSOR #%u", instance);
@@ -171,7 +175,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
return false;
}
int ret = check_calibration(fd, "CAL_ACC%u_ID", device_id);
int ret = check_calibration(h, "CAL_ACC%u_ID", device_id);
if (ret) {
mavlink_and_console_log_critical(mavlink_fd,
@@ -180,7 +184,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
goto out;
}
ret = px4_ioctl(fd, ACCELIOCSELFTEST, 0);
ret = h.ioctl(ACCELIOCSELFTEST, 0);
if (ret != OK) {
mavlink_and_console_log_critical(mavlink_fd,
@@ -215,7 +219,9 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
#endif
out:
#ifdef __PX4_NUTTX
px4_close(fd);
#endif
return success;
}
@@ -225,9 +231,10 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
char s[30];
sprintf(s, "%s%u", GYRO_BASE_DEVICE_PATH, instance);
int fd = px4_open(s, 0);
DevHandle h;
DevMgr::getHandle(s, h);
if (fd < 0) {
if (!h.isValid()) {
if (!optional) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: NO GYRO SENSOR #%u", instance);
@@ -236,7 +243,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
return false;
}
int ret = check_calibration(fd, "CAL_GYRO%u_ID", device_id);
int ret = check_calibration(h, "CAL_GYRO%u_ID", device_id);
if (ret) {
mavlink_and_console_log_critical(mavlink_fd,
@@ -245,7 +252,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
goto out;
}
ret = px4_ioctl(fd, GYROIOCSELFTEST, 0);
ret = h.ioctl(GYROIOCSELFTEST, 0);
if (ret != OK) {
mavlink_and_console_log_critical(mavlink_fd,
@@ -255,7 +262,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
}
out:
px4_close(fd);
DevMgr::releaseHandle(h);
return success;
}
@@ -265,9 +272,10 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
char s[30];
sprintf(s, "%s%u", BARO_BASE_DEVICE_PATH, instance);
int fd = px4_open(s, 0);
DevHandle h;
DevMgr::getHandle(s, h);
if (fd < 0) {
if (!h.isValid()) {
if (!optional) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: NO BARO SENSOR #%u", instance);
@@ -290,7 +298,7 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
//out:
px4_close(fd);
DevMgr::releaseHandle(h);
return success;
}