dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge branch 'master' into mpc_yaw_fix

This commit is contained in:
Anton Babushkin 2013-09-20 16:14:21 +02:00
parent 528666c912 52573f5c7e
commit ab26ecf188
8 changed files with 469 additions and 267 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

0
ROMFS/px4fmu_common/init.d/1000_rc.hil → ROMFS/px4fmu_common/init.d/1000_rc_fw.hil
View File

84
ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil Normal file
View File

@ -0,0 +1,84 @@
#!nsh
#
# USB HIL start
#
echo "[HIL] HILStar starting in state-HIL mode.."
#
# Load default params for this platform
#
if param compare SYS_AUTOCONFIG 1
then
# Set all params here, then disable autoconfig
param set FW_P_D 0
param set FW_P_I 0
param set FW_P_IMAX 15
param set FW_P_LIM_MAX 50
param set FW_P_LIM_MIN -50
param set FW_P_P 60
param set FW_P_RMAX_NEG 0
param set FW_P_RMAX_POS 0
param set FW_P_ROLLFF 1.1
param set FW_R_D 0
param set FW_R_I 5
param set FW_R_IMAX 20
param set FW_R_P 100
param set FW_R_RMAX 100
param set FW_THR_CRUISE 0.65
param set FW_THR_MAX 1
param set FW_THR_MIN 0
param set FW_T_SINK_MAX 5.0
param set FW_T_SINK_MIN 4.0
param set FW_Y_ROLLFF 1.1
param set SYS_AUTOCONFIG 0
param save
fi
# Allow USB some time to come up
sleep 1
# Tell MAVLink that this link is "fast"
mavlink start -b 230400 -d /dev/ttyACM0
# Create a fake HIL /dev/pwm_output interface
hil mode_pwm
#
# Force some key parameters to sane values
# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor
# see https://pixhawk.ethz.ch/mavlink/
#
param set MAV_TYPE 1
#
# Start the commander (depends on orb, mavlink)
#
commander start
#
# Check if we got an IO
#
if px4io start
then
echo "IO started"
else
fmu mode_serial
echo "FMU started"
fi
#
# Start the sensors (depends on orb, px4io)
#
sh /etc/init.d/rc.sensors
#
# Load mixer and start controllers (depends on px4io)
#
mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix
fw_pos_control_l1 start
fw_att_control start
echo "[HIL] setup done, running"

13
ROMFS/px4fmu_common/init.d/rcS
View File

@ -108,15 +108,22 @@ then
if param compare SYS_AUTOSTART 1000 if param compare SYS_AUTOSTART 1000
then then
sh /etc/init.d/1000_rc.hil sh /etc/init.d/1000_rc_fw.hil
set MODE custom set MODE custom
else else
if param compare SYS_AUTOSTART 1001 if param compare SYS_AUTOSTART 1001
then then
sh /etc/init.d/1001_rc_quad.hil sh /etc/init.d/1001_rc_quad.hil
set MODE custom
else else
# Try to get an USB console if param compare SYS_AUTOSTART 1002
nshterm /dev/ttyACM0 & then
sh /etc/init.d/1002_rc_fw_state.hil
set MODE custom
else
# Try to get an USB console
nshterm /dev/ttyACM0 &
fi
fi fi
fi fi

466
src/drivers/rgbled/rgbled.cpp
View File

@ -1,6 +1,8 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved. * Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved.
* Author: Julian Oes <joes@student.ethz.ch>
* Anton Babushkin <anton.babushkin@me.com>
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@ -36,7 +38,6 @@
* *
* Driver for the onboard RGB LED controller (TCA62724FMG) connected via I2C. * Driver for the onboard RGB LED controller (TCA62724FMG) connected via I2C.
* *
*
*/ */
#include <nuttx/config.h> #include <nuttx/config.h>
@ -92,16 +93,14 @@ public:
private: private:
work_s _work; work_s _work;
rgbled_color_t _color;
rgbled_mode_t _mode; rgbled_mode_t _mode;
rgbled_pattern_t _pattern; rgbled_pattern_t _pattern;
float _brightness;
uint8_t _r; uint8_t _r;
uint8_t _g; uint8_t _g;
uint8_t _b; uint8_t _b;
float _brightness;
bool _should_run;
bool _running; bool _running;
int _led_interval; int _led_interval;
int _counter; int _counter;
@ -109,35 +108,33 @@ private:
void set_color(rgbled_color_t ledcolor); void set_color(rgbled_color_t ledcolor);
void set_mode(rgbled_mode_t mode); void set_mode(rgbled_mode_t mode);
void set_pattern(rgbled_pattern_t *pattern); void set_pattern(rgbled_pattern_t *pattern);
void set_brightness(float brightness);
static void led_trampoline(void *arg); static void led_trampoline(void *arg);
void led(); void led();
int set(bool on, uint8_t r, uint8_t g, uint8_t b); int send_led_enable(bool enable);
int set_on(bool on); int send_led_rgb();
int set_rgb(uint8_t r, uint8_t g, uint8_t b); int get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b);
int get(bool &on, bool &not_powersave, uint8_t &r, uint8_t &g, uint8_t &b);
}; };
/* for now, we only support one RGBLED */ /* for now, we only support one RGBLED */
namespace namespace
{ {
RGBLED *g_rgbled; RGBLED *g_rgbled;
} }
void rgbled_usage();
extern "C" __EXPORT int rgbled_main(int argc, char *argv[]); extern "C" __EXPORT int rgbled_main(int argc, char *argv[]);
RGBLED::RGBLED(int bus, int rgbled) : RGBLED::RGBLED(int bus, int rgbled) :
I2C("rgbled", RGBLED_DEVICE_PATH, bus, rgbled, 100000), I2C("rgbled", RGBLED_DEVICE_PATH, bus, rgbled, 100000),
_color(RGBLED_COLOR_OFF),
_mode(RGBLED_MODE_OFF), _mode(RGBLED_MODE_OFF),
_running(false),
_brightness(1.0f),
_r(0), _r(0),
_g(0), _g(0),
_b(0), _b(0),
_brightness(1.0f),
_running(false),
_led_interval(0), _led_interval(0),
_counter(0) _counter(0)
{ {
@ -159,8 +156,9 @@ RGBLED::init()
return ret; return ret;
} }
/* start off */ /* switch off LED on start */
set(false, 0, 0, 0); send_led_enable(false);
send_led_rgb();
return OK; return OK;
} }
@ -169,10 +167,10 @@ int
RGBLED::probe() RGBLED::probe()
{ {
int ret; int ret;
bool on, not_powersave; bool on, powersave;
uint8_t r, g, b; uint8_t r, g, b;
ret = get(on, not_powersave, r, g, b); ret = get(on, powersave, r, g, b);
return ret; return ret;
} }
@ -181,15 +179,16 @@ int
RGBLED::info() RGBLED::info()
{ {
int ret; int ret;
bool on, not_powersave; bool on, powersave;
uint8_t r, g, b; uint8_t r, g, b;
ret = get(on, not_powersave, r, g, b); ret = get(on, powersave, r, g, b);
if (ret == OK) { if (ret == OK) {
/* we don't care about power-save mode */ /* we don't care about power-save mode */
log("state: %s", on ? "ON" : "OFF"); log("state: %s", on ? "ON" : "OFF");
log("red: %u, green: %u, blue: %u", (unsigned)r, (unsigned)g, (unsigned)b); log("red: %u, green: %u, blue: %u", (unsigned)r, (unsigned)g, (unsigned)b);
} else { } else {
warnx("failed to read led"); warnx("failed to read led");
} }
@ -201,28 +200,30 @@ int
RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg) RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg)
{ {
int ret = ENOTTY; int ret = ENOTTY;
switch (cmd) { switch (cmd) {
case RGBLED_SET_RGB: case RGBLED_SET_RGB:
/* set the specified RGB values */ /* set the specified color */
rgbled_rgbset_t rgbset; _r = ((rgbled_rgbset_t *) arg)->red;
memcpy(&rgbset, (rgbled_rgbset_t*)arg, sizeof(rgbset)); _g = ((rgbled_rgbset_t *) arg)->green;
set_rgb(rgbset.red, rgbset.green, rgbset.blue); _b = ((rgbled_rgbset_t *) arg)->blue;
set_mode(RGBLED_MODE_ON); send_led_rgb();
return OK; return OK;
case RGBLED_SET_COLOR: case RGBLED_SET_COLOR:
/* set the specified color name */ /* set the specified color name */
set_color((rgbled_color_t)arg); set_color((rgbled_color_t)arg);
send_led_rgb();
return OK; return OK;
case RGBLED_SET_MODE: case RGBLED_SET_MODE:
/* set the specified blink speed */ /* set the specified mode */
set_mode((rgbled_mode_t)arg); set_mode((rgbled_mode_t)arg);
return OK; return OK;
case RGBLED_SET_PATTERN: case RGBLED_SET_PATTERN:
/* set a special pattern */ /* set a special pattern */
set_pattern((rgbled_pattern_t*)arg); set_pattern((rgbled_pattern_t *)arg);
return OK; return OK;
default: default:
@ -241,39 +242,54 @@ RGBLED::led_trampoline(void *arg)
rgbl->led(); rgbl->led();
} }
/**
* Main loop function
*/
void void
RGBLED::led() RGBLED::led()
{ {
switch (_mode) { switch (_mode) {
case RGBLED_MODE_BLINK_SLOW: case RGBLED_MODE_BLINK_SLOW:
case RGBLED_MODE_BLINK_NORMAL: case RGBLED_MODE_BLINK_NORMAL:
case RGBLED_MODE_BLINK_FAST: case RGBLED_MODE_BLINK_FAST:
if(_counter % 2 == 0) if (_counter >= 2)
set_on(true); _counter = 0;
else
set_on(false);
break;
case RGBLED_MODE_BREATHE:
if (_counter >= 30)
_counter = 0;
if (_counter <= 15) {
set_brightness(((float)_counter)*((float)_counter)/(15.0f*15.0f));
} else {
set_brightness(((float)(30-_counter))*((float)(30-_counter))/(15.0f*15.0f));
}
break;
case RGBLED_MODE_PATTERN:
/* don't run out of the pattern array and stop if the next frame is 0 */
if (_counter >= RGBLED_PATTERN_LENGTH || _pattern.duration[_counter] <= 0)
_counter = 0;
set_color(_pattern.color[_counter]); send_led_enable(_counter == 0);
_led_interval = _pattern.duration[_counter];
break; break;
default:
break; case RGBLED_MODE_BREATHE:
if (_counter >= 62)
_counter = 0;
int n;
if (_counter < 32) {
n = _counter;
} else {
n = 62 - _counter;
}
_brightness = n * n / (31.0f * 31.0f);
send_led_rgb();
break;
case RGBLED_MODE_PATTERN:
/* don't run out of the pattern array and stop if the next frame is 0 */
if (_counter >= RGBLED_PATTERN_LENGTH || _pattern.duration[_counter] <= 0)
_counter = 0;
set_color(_pattern.color[_counter]);
send_led_rgb();
_led_interval = _pattern.duration[_counter];
break;
default:
break;
} }
_counter++; _counter++;
@ -282,181 +298,231 @@ RGBLED::led()
work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, _led_interval); work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, _led_interval);
} }
/**
* Parse color constant and set _r _g _b values
*/
void void
RGBLED::set_color(rgbled_color_t color) { RGBLED::set_color(rgbled_color_t color)
{
_color = color;
switch (color) { switch (color) {
case RGBLED_COLOR_OFF: // off case RGBLED_COLOR_OFF:
set_rgb(0,0,0); _r = 0;
break; _g = 0;
case RGBLED_COLOR_RED: // red _b = 0;
set_rgb(255,0,0); break;
break;
case RGBLED_COLOR_YELLOW: // yellow case RGBLED_COLOR_RED:
set_rgb(255,70,0); _r = 255;
break; _g = 0;
case RGBLED_COLOR_PURPLE: // purple _b = 0;
set_rgb(255,0,255); break;
break;
case RGBLED_COLOR_GREEN: // green case RGBLED_COLOR_YELLOW:
set_rgb(0,255,0); _r = 255;
break; _g = 200;
case RGBLED_COLOR_BLUE: // blue _b = 0;
set_rgb(0,0,255); break;
break;
case RGBLED_COLOR_WHITE: // white case RGBLED_COLOR_PURPLE:
set_rgb(255,255,255); _r = 255;
break; _g = 0;
case RGBLED_COLOR_AMBER: // amber _b = 255;
set_rgb(255,20,0); break;
break;
case RGBLED_COLOR_DIM_RED: // red case RGBLED_COLOR_GREEN:
set_rgb(90,0,0); _r = 0;
break; _g = 255;
case RGBLED_COLOR_DIM_YELLOW: // yellow _b = 0;
set_rgb(80,30,0); break;
break;
case RGBLED_COLOR_DIM_PURPLE: // purple case RGBLED_COLOR_BLUE:
set_rgb(45,0,45); _r = 0;
break; _g = 0;
case RGBLED_COLOR_DIM_GREEN: // green _b = 255;
set_rgb(0,90,0); break;
break;
case RGBLED_COLOR_DIM_BLUE: // blue case RGBLED_COLOR_WHITE:
set_rgb(0,0,90); _r = 255;
break; _g = 255;
case RGBLED_COLOR_DIM_WHITE: // white _b = 255;
set_rgb(30,30,30); break;
break;
case RGBLED_COLOR_DIM_AMBER: // amber case RGBLED_COLOR_AMBER:
set_rgb(80,20,0); _r = 255;
break; _g = 80;
default: _b = 0;
warnx("color unknown"); break;
break;
case RGBLED_COLOR_DIM_RED:
_r = 90;
_g = 0;
_b = 0;
break;
case RGBLED_COLOR_DIM_YELLOW:
_r = 80;
_g = 30;
_b = 0;
break;
case RGBLED_COLOR_DIM_PURPLE:
_r = 45;
_g = 0;
_b = 45;
break;
case RGBLED_COLOR_DIM_GREEN:
_r = 0;
_g = 90;
_b = 0;
break;
case RGBLED_COLOR_DIM_BLUE:
_r = 0;
_g = 0;
_b = 90;
break;
case RGBLED_COLOR_DIM_WHITE:
_r = 30;
_g = 30;
_b = 30;
break;
case RGBLED_COLOR_DIM_AMBER:
_r = 80;
_g = 20;
_b = 0;
break;
default:
warnx("color unknown");
break;
} }
} }
/**
* Set mode, if mode not changed has no any effect (doesn't reset blinks phase)
*/
void void
RGBLED::set_mode(rgbled_mode_t mode) RGBLED::set_mode(rgbled_mode_t mode)
{ {
_mode = mode; if (mode != _mode) {
_mode = mode;
bool should_run = false;
switch (mode) { switch (mode) {
case RGBLED_MODE_OFF: case RGBLED_MODE_OFF:
_should_run = false; send_led_enable(false);
set_on(false);
break; break;
case RGBLED_MODE_ON: case RGBLED_MODE_ON:
_should_run = false; _brightness = 1.0f;
set_on(true); send_led_rgb();
send_led_enable(true);
break; break;
case RGBLED_MODE_BLINK_SLOW: case RGBLED_MODE_BLINK_SLOW:
_should_run = true; should_run = true;
_counter = 0;
_led_interval = 2000; _led_interval = 2000;
_brightness = 1.0f;
send_led_rgb();
break; break;
case RGBLED_MODE_BLINK_NORMAL: case RGBLED_MODE_BLINK_NORMAL:
_should_run = true; should_run = true;
_counter = 0;
_led_interval = 500; _led_interval = 500;
_brightness = 1.0f;
send_led_rgb();
break; break;
case RGBLED_MODE_BLINK_FAST: case RGBLED_MODE_BLINK_FAST:
_should_run = true; should_run = true;
_counter = 0;
_led_interval = 100; _led_interval = 100;
_brightness = 1.0f;
send_led_rgb();
break; break;
case RGBLED_MODE_BREATHE: case RGBLED_MODE_BREATHE:
_should_run = true; should_run = true;
set_on(true);
_counter = 0; _counter = 0;
_led_interval = 1000/15; _led_interval = 25;
send_led_enable(true);
break; break;
case RGBLED_MODE_PATTERN: case RGBLED_MODE_PATTERN:
_should_run = true; should_run = true;
set_on(true);
_counter = 0; _counter = 0;
_brightness = 1.0f;
send_led_enable(true);
break; break;
default: default:
warnx("mode unknown"); warnx("mode unknown");
break; break;
} }
/* if it should run now, start the workq */ /* if it should run now, start the workq */
if (_should_run && !_running) { if (should_run && !_running) {
_running = true; _running = true;
work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 1); work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 1);
} }
/* if it should stop, then cancel the workq */
if (!_should_run && _running) { /* if it should stop, then cancel the workq */
_running = false; if (!should_run && _running) {
work_cancel(LPWORK, &_work); _running = false;
work_cancel(LPWORK, &_work);
}
} }
} }
/**
* Set pattern for PATTERN mode, but don't change current mode
*/
void void
RGBLED::set_pattern(rgbled_pattern_t *pattern) RGBLED::set_pattern(rgbled_pattern_t *pattern)
{ {
memcpy(&_pattern, pattern, sizeof(rgbled_pattern_t)); memcpy(&_pattern, pattern, sizeof(rgbled_pattern_t));
set_mode(RGBLED_MODE_PATTERN);
}
void
RGBLED::set_brightness(float brightness) {
_brightness = brightness;
set_rgb(_r, _g, _b);
} }
/**
* Sent ENABLE flag to LED driver
*/
int int
RGBLED::set(bool on, uint8_t r, uint8_t g, uint8_t b) RGBLED::send_led_enable(bool enable)
{ {
uint8_t settings_byte = 0; uint8_t settings_byte = 0;
if (on) if (enable)
settings_byte |= SETTING_ENABLE;
/* powersave not used */
// if (not_powersave)
settings_byte |= SETTING_NOT_POWERSAVE;
const uint8_t msg[5] = { SUB_ADDR_START, (uint8_t)(b*15/255), (uint8_t)(g*15/255), (uint8_t)(r*15/255), settings_byte};
return transfer(msg, sizeof(msg), nullptr, 0);
}
int
RGBLED::set_on(bool on)
{
uint8_t settings_byte = 0;
if (on)
settings_byte |= SETTING_ENABLE; settings_byte |= SETTING_ENABLE;
/* powersave not used */ settings_byte |= SETTING_NOT_POWERSAVE;
// if (not_powersave)
settings_byte |= SETTING_NOT_POWERSAVE;
const uint8_t msg[2] = { SUB_ADDR_SETTINGS, settings_byte}; const uint8_t msg[2] = { SUB_ADDR_SETTINGS, settings_byte};
return transfer(msg, sizeof(msg), nullptr, 0); return transfer(msg, sizeof(msg), nullptr, 0);
} }
/**
* Send RGB PWM settings to LED driver according to current color and brightness
*/
int int
RGBLED::set_rgb(uint8_t r, uint8_t g, uint8_t b) RGBLED::send_led_rgb()
{ {
/* save the RGB values in case we want to change the brightness later */ /* To scale from 0..255 -> 0..15 shift right by 4 bits */
_r = r; const uint8_t msg[6] = {
_g = g; SUB_ADDR_PWM0, (uint8_t)((int)(_b * _brightness) >> 4),
_b = b; SUB_ADDR_PWM1, (uint8_t)((int)(_g * _brightness) >> 4),
SUB_ADDR_PWM2, (uint8_t)((int)(_r * _brightness) >> 4)
const uint8_t msg[6] = { SUB_ADDR_PWM0, (uint8_t)((float)b/255.0f*15.0f*_brightness), SUB_ADDR_PWM1, (uint8_t)((float)g/255.0f*15.0f*_brightness), SUB_ADDR_PWM2, (uint8_t)((float)r/255.0f*15.0f*_brightness)}; };
return transfer(msg, sizeof(msg), nullptr, 0); return transfer(msg, sizeof(msg), nullptr, 0);
} }
int int
RGBLED::get(bool &on, bool &not_powersave, uint8_t &r, uint8_t &g, uint8_t &b) RGBLED::get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b)
{ {
uint8_t result[2]; uint8_t result[2];
int ret; int ret;
@ -465,24 +531,23 @@ RGBLED::get(bool &on, bool &not_powersave, uint8_t &r, uint8_t &g, uint8_t &b)
if (ret == OK) { if (ret == OK) {
on = result[0] & SETTING_ENABLE; on = result[0] & SETTING_ENABLE;
not_powersave = result[0] & SETTING_NOT_POWERSAVE; powersave = !(result[0] & SETTING_NOT_POWERSAVE);
/* XXX check, looks wrong */ /* XXX check, looks wrong */
r = (result[0] & 0x0f)*255/15; r = (result[0] & 0x0f) << 4;
g = (result[1] & 0xf0)*255/15; g = (result[1] & 0xf0);
b = (result[1] & 0x0f)*255/15; b = (result[1] & 0x0f) << 4;
} }
return ret; return ret;
} }
void rgbled_usage(); void
rgbled_usage()
{
void rgbled_usage() { warnx("missing command: try 'start', 'test', 'info', 'off', 'rgb 30 40 50'");
warnx("missing command: try 'start', 'test', 'info', 'stop'/'off', 'rgb 30 40 50'");
warnx("options:"); warnx("options:");
warnx(" -b i2cbus (%d)", PX4_I2C_BUS_LED); warnx(" -b i2cbus (%d)", PX4_I2C_BUS_LED);
errx(0, " -a addr (0x%x)", ADDR); warnx(" -a addr (0x%x)", ADDR);
} }
int int
@ -492,17 +557,21 @@ rgbled_main(int argc, char *argv[])
int rgbledadr = ADDR; /* 7bit */ int rgbledadr = ADDR; /* 7bit */
int ch; int ch;
/* jump over start/off/etc and look at options first */ /* jump over start/off/etc and look at options first */
while ((ch = getopt(argc-1, &argv[1], "a:b:")) != EOF) { while ((ch = getopt(argc - 1, &argv[1], "a:b:")) != EOF) {
switch (ch) { switch (ch) {
case 'a': case 'a':
rgbledadr = strtol(optarg, NULL, 0); rgbledadr = strtol(optarg, NULL, 0);
break; break;
case 'b': case 'b':
i2cdevice = strtol(optarg, NULL, 0); i2cdevice = strtol(optarg, NULL, 0);
break; break;
default: default:
rgbled_usage(); rgbled_usage();
exit(0);
} }
} }
@ -519,17 +588,21 @@ rgbled_main(int argc, char *argv[])
// try the external bus first // try the external bus first
i2cdevice = PX4_I2C_BUS_EXPANSION; i2cdevice = PX4_I2C_BUS_EXPANSION;
g_rgbled = new RGBLED(PX4_I2C_BUS_EXPANSION, rgbledadr); g_rgbled = new RGBLED(PX4_I2C_BUS_EXPANSION, rgbledadr);
if (g_rgbled != nullptr && OK != g_rgbled->init()) { if (g_rgbled != nullptr && OK != g_rgbled->init()) {
delete g_rgbled; delete g_rgbled;
g_rgbled = nullptr; g_rgbled = nullptr;
} }
if (g_rgbled == nullptr) { if (g_rgbled == nullptr) {
// fall back to default bus // fall back to default bus
i2cdevice = PX4_I2C_BUS_LED; i2cdevice = PX4_I2C_BUS_LED;
} }
} }
if (g_rgbled == nullptr) { if (g_rgbled == nullptr) {
g_rgbled = new RGBLED(i2cdevice, rgbledadr); g_rgbled = new RGBLED(i2cdevice, rgbledadr);
if (g_rgbled == nullptr) if (g_rgbled == nullptr)
errx(1, "new failed"); errx(1, "new failed");
@ -545,21 +618,24 @@ rgbled_main(int argc, char *argv[])
/* need the driver past this point */ /* need the driver past this point */
if (g_rgbled == nullptr) { if (g_rgbled == nullptr) {
warnx("not started"); warnx("not started");
rgbled_usage(); rgbled_usage();
exit(0); exit(0);
} }
if (!strcmp(verb, "test")) { if (!strcmp(verb, "test")) {
fd = open(RGBLED_DEVICE_PATH, 0); fd = open(RGBLED_DEVICE_PATH, 0);
if (fd == -1) { if (fd == -1) {
errx(1, "Unable to open " RGBLED_DEVICE_PATH); errx(1, "Unable to open " RGBLED_DEVICE_PATH);
} }
rgbled_pattern_t pattern = { {RGBLED_COLOR_RED, RGBLED_COLOR_GREEN, RGBLED_COLOR_BLUE, RGBLED_COLOR_OFF}, rgbled_pattern_t pattern = { {RGBLED_COLOR_RED, RGBLED_COLOR_GREEN, RGBLED_COLOR_BLUE, RGBLED_COLOR_WHITE, RGBLED_COLOR_OFF, RGBLED_COLOR_OFF},
{200, 200, 200, 400 } }; {500, 500, 500, 500, 1000, 0 } // "0" indicates end of pattern
};
ret = ioctl(fd, RGBLED_SET_PATTERN, (unsigned long)&pattern); ret = ioctl(fd, RGBLED_SET_PATTERN, (unsigned long)&pattern);
ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_PATTERN);
close(fd); close(fd);
exit(ret); exit(ret);
@ -570,33 +646,39 @@ rgbled_main(int argc, char *argv[])
exit(0); exit(0);
} }
if (!strcmp(verb, "stop") || !strcmp(verb, "off")) { if (!strcmp(verb, "off")) {
/* although technically it doesn't stop, this is the excepted syntax */
fd = open(RGBLED_DEVICE_PATH, 0); fd = open(RGBLED_DEVICE_PATH, 0);
if (fd == -1) { if (fd == -1) {
errx(1, "Unable to open " RGBLED_DEVICE_PATH); errx(1, "Unable to open " RGBLED_DEVICE_PATH);
} }
ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_OFF); ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_OFF);
close(fd); close(fd);
exit(ret); exit(ret);
} }
if (!strcmp(verb, "rgb")) { if (!strcmp(verb, "rgb")) {
fd = open(RGBLED_DEVICE_PATH, 0);
if (fd == -1) {
errx(1, "Unable to open " RGBLED_DEVICE_PATH);
}
if (argc < 5) { if (argc < 5) {
errx(1, "Usage: rgbled rgb <red> <green> <blue>"); errx(1, "Usage: rgbled rgb <red> <green> <blue>");
} }
fd = open(RGBLED_DEVICE_PATH, 0);
if (fd == -1) {
errx(1, "Unable to open " RGBLED_DEVICE_PATH);
}
rgbled_rgbset_t v; rgbled_rgbset_t v;
v.red = strtol(argv[2], NULL, 0); v.red = strtol(argv[2], NULL, 0);
v.green = strtol(argv[3], NULL, 0); v.green = strtol(argv[3], NULL, 0);
v.blue = strtol(argv[4], NULL, 0); v.blue = strtol(argv[4], NULL, 0);
ret = ioctl(fd, RGBLED_SET_RGB, (unsigned long)&v); ret = ioctl(fd, RGBLED_SET_RGB, (unsigned long)&v);
ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_ON);
close(fd); close(fd);
exit(ret); exit(ret);
} }
rgbled_usage(); rgbled_usage();
exit(0);
} }

119
src/modules/commander/commander.cpp
View File

@ -5,6 +5,7 @@
* Lorenz Meier <lm@inf.ethz.ch> * Lorenz Meier <lm@inf.ethz.ch>
* Thomas Gubler <thomasgubler@student.ethz.ch> * Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch> * Julian Oes <joes@student.ethz.ch>
* Anton Babushkin <anton.babushkin@me.com>
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@ -198,7 +199,7 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode
*/ */
int commander_thread_main(int argc, char *argv[]); int commander_thread_main(int argc, char *argv[]);
void toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed); void control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed);
void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed); void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed);
@ -650,7 +651,6 @@ int commander_thread_main(int argc, char *argv[])
bool critical_battery_voltage_actions_done = false; bool critical_battery_voltage_actions_done = false;
uint64_t last_idle_time = 0; uint64_t last_idle_time = 0;
uint64_t start_time = 0; uint64_t start_time = 0;
bool status_changed = true; bool status_changed = true;
@ -728,7 +728,7 @@ int commander_thread_main(int argc, char *argv[])
struct subsystem_info_s info; struct subsystem_info_s info;
memset(&info, 0, sizeof(info)); memset(&info, 0, sizeof(info));
toggle_status_leds(&status, &armed, true); control_status_leds(&status, &armed, true);
/* now initialized */ /* now initialized */
commander_initialized = true; commander_initialized = true;
@ -950,11 +950,9 @@ int commander_thread_main(int argc, char *argv[])
battery_tune_played = false; battery_tune_played = false;
if (armed.armed) { if (armed.armed) {
// XXX not sure what should happen when voltage is low in flight arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
//arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
} else { } else {
// XXX should we still allow to arm with critical battery? arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed);
//arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed);
} }
status_changed = true; status_changed = true;
@ -1166,9 +1164,6 @@ int commander_thread_main(int argc, char *argv[])
if (arming_state_changed || main_state_changed || navigation_state_changed) { if (arming_state_changed || main_state_changed || navigation_state_changed) {
mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state); mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state);
status_changed = true; status_changed = true;
} else {
status_changed = false;
} }
hrt_abstime t1 = hrt_absolute_time(); hrt_abstime t1 = hrt_absolute_time();
@ -1228,7 +1223,19 @@ int commander_thread_main(int argc, char *argv[])
fflush(stdout); fflush(stdout);
counter++; counter++;
toggle_status_leds(&status, &armed, arming_state_changed || status_changed); int blink_state = blink_msg_state();
if (blink_state > 0) {
/* blinking LED message, don't touch LEDs */
if (blink_state == 2) {
/* blinking LED message completed, restore normal state */
control_status_leds(&status, &armed, true);
}
} else {
/* normal state */
control_status_leds(&status, &armed, status_changed);
}
status_changed = false;
usleep(COMMANDER_MONITORING_INTERVAL); usleep(COMMANDER_MONITORING_INTERVAL);
} }
@ -1276,8 +1283,48 @@ check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *val
} }
void void
toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed) control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed)
{ {
/* driving rgbled */
if (changed) {
bool set_normal_color = false;
/* set mode */
if (status->arming_state == ARMING_STATE_ARMED) {
rgbled_set_mode(RGBLED_MODE_ON);
set_normal_color = true;
} else if (status->arming_state == ARMING_STATE_ARMED_ERROR) {
rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
rgbled_set_color(RGBLED_COLOR_RED);
} else if (status->arming_state == ARMING_STATE_STANDBY) {
rgbled_set_mode(RGBLED_MODE_BREATHE);
set_normal_color = true;
} else { // STANDBY_ERROR and other states
rgbled_set_mode(RGBLED_MODE_BLINK_NORMAL);
rgbled_set_color(RGBLED_COLOR_RED);
}
if (set_normal_color) {
/* set color */
if (status->battery_warning != VEHICLE_BATTERY_WARNING_NONE) {
if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW) {
rgbled_set_color(RGBLED_COLOR_AMBER);
}
/* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */
} else {
if (status->condition_local_position_valid) {
rgbled_set_color(RGBLED_COLOR_GREEN);
} else {
rgbled_set_color(RGBLED_COLOR_BLUE);
}
}
}
}
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
/* this runs at around 20Hz, full cycle is 16 ticks = 10/16Hz */ /* this runs at around 20Hz, full cycle is 16 ticks = 10/16Hz */
@ -1298,54 +1345,6 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang
#endif #endif
if (changed) {
/* XXX TODO blink fast when armed and serious error occurs */
if (armed->armed) {
rgbled_set_mode(RGBLED_MODE_ON);
} else if (armed->ready_to_arm) {
rgbled_set_mode(RGBLED_MODE_BREATHE);
} else {
rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
}
}
if (status->battery_warning != VEHICLE_BATTERY_WARNING_NONE) {
switch (status->battery_warning) {
case VEHICLE_BATTERY_WARNING_LOW:
rgbled_set_color(RGBLED_COLOR_YELLOW);
break;
case VEHICLE_BATTERY_WARNING_CRITICAL:
rgbled_set_color(RGBLED_COLOR_AMBER);
break;
default:
break;
}
} else {
switch (status->main_state) {
case MAIN_STATE_MANUAL:
rgbled_set_color(RGBLED_COLOR_WHITE);
break;
case MAIN_STATE_SEATBELT:
case MAIN_STATE_EASY:
rgbled_set_color(RGBLED_COLOR_GREEN);
break;
case MAIN_STATE_AUTO:
rgbled_set_color(RGBLED_COLOR_BLUE);
break;
default:
break;
}
}
/* give system warnings on error LED, XXX maybe add memory usage warning too */ /* give system warnings on error LED, XXX maybe add memory usage warning too */
if (status->load > 0.95f) { if (status->load > 0.95f) {
if (leds_counter % 2 == 0) if (leds_counter % 2 == 0)

50
src/modules/commander/commander_helper.cpp
View File

@ -3,6 +3,7 @@
* Copyright (C) 2013 PX4 Development Team. All rights reserved. * Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch> * Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch> * Julian Oes <joes@student.ethz.ch>
* Anton Babushkin <anton.babushkin@me.com>
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@ -55,7 +56,6 @@
#include <drivers/drv_led.h> #include <drivers/drv_led.h>
#include <drivers/drv_rgbled.h> #include <drivers/drv_rgbled.h>
#include "commander_helper.h" #include "commander_helper.h"
/* oddly, ERROR is not defined for c++ */ /* oddly, ERROR is not defined for c++ */
@ -64,21 +64,24 @@
#endif #endif
static const int ERROR = -1; static const int ERROR = -1;
#define BLINK_MSG_TIME 700000 // 3 fast blinks
bool is_multirotor(const struct vehicle_status_s *current_status) bool is_multirotor(const struct vehicle_status_s *current_status)
{ {
return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) ||
(current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) ||
(current_status->system_type == VEHICLE_TYPE_OCTOROTOR) || (current_status->system_type == VEHICLE_TYPE_OCTOROTOR) ||
(current_status->system_type == VEHICLE_TYPE_TRICOPTER)); (current_status->system_type == VEHICLE_TYPE_TRICOPTER));
} }
bool is_rotary_wing(const struct vehicle_status_s *current_status) bool is_rotary_wing(const struct vehicle_status_s *current_status)
{ {
return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER) return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER)
|| (current_status->system_type == VEHICLE_TYPE_COAXIAL); || (current_status->system_type == VEHICLE_TYPE_COAXIAL);
} }
static int buzzer; static int buzzer;
static hrt_abstime blink_msg_end;
int buzzer_init() int buzzer_init()
{ {
@ -104,16 +107,25 @@ void tune_error()
void tune_positive() void tune_positive()
{ {
blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
rgbled_set_color(RGBLED_COLOR_GREEN);
rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_POSITIVE_TUNE); ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_POSITIVE_TUNE);
} }
void tune_neutral() void tune_neutral()
{ {
blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
rgbled_set_color(RGBLED_COLOR_WHITE);
rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEUTRAL_TUNE); ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEUTRAL_TUNE);
} }
void tune_negative() void tune_negative()
{ {
blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
rgbled_set_color(RGBLED_COLOR_RED);
rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEGATIVE_TUNE); ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEGATIVE_TUNE);
} }
@ -132,18 +144,31 @@ int tune_critical_bat()
return ioctl(buzzer, TONE_SET_ALARM, TONE_BATTERY_WARNING_FAST_TUNE); return ioctl(buzzer, TONE_SET_ALARM, TONE_BATTERY_WARNING_FAST_TUNE);
} }
void tune_stop() void tune_stop()
{ {
ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE); ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE);
} }
int blink_msg_state()
{
if (blink_msg_end == 0) {
return 0;
} else if (hrt_absolute_time() > blink_msg_end) {
return 2;
} else {
return 1;
}
}
static int leds; static int leds;
static int rgbleds; static int rgbleds;
int led_init() int led_init()
{ {
blink_msg_end = 0;
/* first open normal LEDs */ /* first open normal LEDs */
leds = open(LED_DEVICE_PATH, 0); leds = open(LED_DEVICE_PATH, 0);
@ -159,6 +184,7 @@ int led_init()
warnx("Blue LED: ioctl fail\n"); warnx("Blue LED: ioctl fail\n");
return ERROR; return ERROR;
} }
#endif #endif
if (ioctl(leds, LED_ON, LED_AMBER)) { if (ioctl(leds, LED_ON, LED_AMBER)) {
@ -168,6 +194,7 @@ int led_init()
/* then try RGB LEDs, this can fail on FMUv1*/ /* then try RGB LEDs, this can fail on FMUv1*/
rgbleds = open(RGBLED_DEVICE_PATH, 0); rgbleds = open(RGBLED_DEVICE_PATH, 0);
if (rgbleds == -1) { if (rgbleds == -1) {
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
errx(1, "Unable to open " RGBLED_DEVICE_PATH); errx(1, "Unable to open " RGBLED_DEVICE_PATH);
@ -203,19 +230,22 @@ int led_off(int led)
return ioctl(leds, LED_OFF, led); return ioctl(leds, LED_OFF, led);
} }
void rgbled_set_color(rgbled_color_t color) { void rgbled_set_color(rgbled_color_t color)
{
if (rgbleds != -1) if (rgbleds != -1)
ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color); ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color);
} }
void rgbled_set_mode(rgbled_mode_t mode) { void rgbled_set_mode(rgbled_mode_t mode)
{
if (rgbleds != -1) if (rgbleds != -1)
ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode); ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode);
} }
void rgbled_set_pattern(rgbled_pattern_t *pattern) { void rgbled_set_pattern(rgbled_pattern_t *pattern)
{
if (rgbleds != -1) if (rgbleds != -1)
ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern); ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern);

3
src/modules/commander/commander_helper.h
View File

@ -62,6 +62,7 @@ int tune_arm(void);
int tune_low_bat(void); int tune_low_bat(void);
int tune_critical_bat(void); int tune_critical_bat(void);
void tune_stop(void); void tune_stop(void);
int blink_msg_state();
int led_init(void); int led_init(void);
void led_deinit(void); void led_deinit(void);
@ -70,9 +71,7 @@ int led_on(int led);
int led_off(int led); int led_off(int led);
void rgbled_set_color(rgbled_color_t color); void rgbled_set_color(rgbled_color_t color);
void rgbled_set_mode(rgbled_mode_t mode); void rgbled_set_mode(rgbled_mode_t mode);
void rgbled_set_pattern(rgbled_pattern_t *pattern); void rgbled_set_pattern(rgbled_pattern_t *pattern);
/** /**

1
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -574,6 +574,7 @@ handle_message(mavlink_message_t *msg)
orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed); orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed);
} }
hil_global_pos.valid = true;
hil_global_pos.lat = hil_state.lat; hil_global_pos.lat = hil_state.lat;
hil_global_pos.lon = hil_state.lon; hil_global_pos.lon = hil_state.lon;
hil_global_pos.alt = hil_state.alt / 1000.0f; hil_global_pos.alt = hil_state.alt / 1000.0f;