yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 09:22:18 +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

67
src/modules/commander/commander_helper.cpp
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() {