fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 04:23:34 +08: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
src/modules/commander/PreflightCheck.cpp
+30 -22
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;
}
src/modules/commander/commander_helper.cpp
+24 -43
View File
@@ -63,6 +63,9 @@
#include <drivers/drv_rgbled.h>
#include "commander_helper.h"
#include "DevMgr.hpp"
using namespace DriverFramework;
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
@@ -136,7 +139,7 @@ int buzzer_init()
buzzer = px4_open(TONEALARM0_DEVICE_PATH, O_WRONLY);
if (buzzer < 0) {
warnx("Buzzer: px4_open fail\n");
PX4_WARN("Buzzer: px4_open fail\n");
return ERROR;
}
@@ -264,30 +267,30 @@ int blink_msg_state()
}
}
static int leds = -1;
static int rgbleds = -1;
static DevHandle h_leds;
static DevHandle h_rgbleds;
int led_init()
{
blink_msg_end = 0;
/* first open normal LEDs */
leds = px4_open(LED0_DEVICE_PATH, 0);
DevMgr::getHandle(LED0_DEVICE_PATH, h_leds);
if (leds < 0) {
warnx("LED: px4_open fail\n");
if (!h_leds.isValid()) {
PX4_WARN("LED: getHandle fail\n");
return ERROR;
}
/* the blue LED is only available on FMUv1 & AeroCore but not FMUv2 */
(void)px4_ioctl(leds, LED_ON, LED_BLUE);
(void)h_leds.ioctl(LED_ON, LED_BLUE);
/* switch blue off */
led_off(LED_BLUE);
/* we consider the amber led mandatory */
if (px4_ioctl(leds, LED_ON, LED_AMBER)) {
warnx("Amber LED: px4_ioctl fail\n");
if (h_leds.ioctl(LED_ON, LED_AMBER)) {
PX4_WARN("Amber LED: ioctl fail\n");
return ERROR;
}
@@ -295,10 +298,11 @@ int led_init()
led_off(LED_AMBER);
/* then try RGB LEDs, this can fail on FMUv1*/
rgbleds = px4_open(RGBLED0_DEVICE_PATH, 0);
DevHandle h;
DevMgr::getHandle(RGBLED0_DEVICE_PATH, h_rgbleds);
if (rgbleds < 0) {
warnx("No RGB LED found at " RGBLED0_DEVICE_PATH);
if (!h_rgbleds.isValid()) {
PX4_WARN("No RGB LED found at " RGBLED0_DEVICE_PATH);
}
return 0;
@@ -306,64 +310,41 @@ int led_init()
void led_deinit()
{
if (leds >= 0) {
px4_close(leds);
}
if (rgbleds >= 0) {
px4_close(rgbleds);
}
DevMgr::releaseHandle(h_leds);
DevMgr::releaseHandle(h_rgbleds);
}
int led_toggle(int led)
{
if (leds < 0) {
return leds;
}
return px4_ioctl(leds, LED_TOGGLE, led);
return h_leds.ioctl(LED_TOGGLE, led);
}
int led_on(int led)
{
if (leds < 0) {
return leds;
}
return px4_ioctl(leds, LED_ON, led);
return h_leds.ioctl(LED_ON, led);
}
int led_off(int led)
{
if (leds < 0) {
return leds;
}
return px4_ioctl(leds, LED_OFF, led);
return h_leds.ioctl(LED_OFF, led);
}
void rgbled_set_color(rgbled_color_t color)
{
if (rgbleds < 0) {
return;
}
px4_ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color);
h_rgbleds.ioctl(RGBLED_SET_COLOR, (unsigned long)color);
}
void rgbled_set_mode(rgbled_mode_t mode)
{
if (rgbleds < 0) {
return;
}
px4_ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode);
h_rgbleds.ioctl(RGBLED_SET_MODE, (unsigned long)mode);
}
void rgbled_set_pattern(rgbled_pattern_t *pattern)
{
if (rgbleds < 0) {
return;
}
px4_ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern);
h_rgbleds.ioctl(RGBLED_SET_PATTERN, (unsigned long)pattern);
}
unsigned battery_get_n_cells() {
src/modules/sensors/sensors.cpp
+2 -2
View File
@@ -926,7 +926,7 @@ Sensors::accel_init()
{
DevHandle h_accel;
DevMgr::getHandle(ACCEL0_DEVICE_PATH, h_accel);
if (!h_accel.isValid()) {
warnx("FATAL: no accelerometer found: %s (%d)", ACCEL0_DEVICE_PATH, h_accel.getError());
return ERROR;
@@ -971,7 +971,7 @@ Sensors::mag_init()
DevHandle h_mag;
DevMgr::getHandle(MAG0_DEVICE_PATH, h_mag);
if (!h_mag.isValid()) {
warnx("FATAL: no magnetometer found: %s (%d)", MAG0_DEVICE_PATH, h_mag.getError());
return ERROR;