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
then
sh /etc/init.d/1000_rc.hil
sh /etc/init.d/1000_rc_fw.hil
set MODE custom
else
if param compare SYS_AUTOSTART 1001
then
sh /etc/init.d/1001_rc_quad.hil
set MODE custom
else
# Try to get an USB console
nshterm /dev/ttyACM0 &
if param compare SYS_AUTOSTART 1002
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

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

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

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

@ -5,6 +5,7 @@
* Lorenz Meier <lm@inf.ethz.ch>
* Thomas Gubler <thomasgubler@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
* 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[]);
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);
@ -650,7 +651,6 @@ int commander_thread_main(int argc, char *argv[])
bool critical_battery_voltage_actions_done = false;
uint64_t last_idle_time = 0;
uint64_t start_time = 0;
bool status_changed = true;
@ -728,7 +728,7 @@ int commander_thread_main(int argc, char *argv[])
struct subsystem_info_s info;
memset(&info, 0, sizeof(info));
toggle_status_leds(&status, &armed, true);
control_status_leds(&status, &armed, true);
/* now initialized */
commander_initialized = true;
@ -950,11 +950,9 @@ int commander_thread_main(int argc, char *argv[])
battery_tune_played = false;
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 {
// 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;
@ -1166,9 +1164,6 @@ int commander_thread_main(int argc, char *argv[])
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);
status_changed = true;
} else {
status_changed = false;
}
hrt_abstime t1 = hrt_absolute_time();
@ -1228,7 +1223,19 @@ int commander_thread_main(int argc, char *argv[])
fflush(stdout);
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);
}
@ -1276,8 +1283,48 @@ check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *val
}
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
/* 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
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 */
if (status->load > 0.95f) {
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.
* Author: Thomas Gubler <thomasgubler@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
* modification, are permitted provided that the following conditions
@ -55,7 +56,6 @@
#include <drivers/drv_led.h>
#include <drivers/drv_rgbled.h>
#include "commander_helper.h"
/* oddly, ERROR is not defined for c++ */
@ -64,21 +64,24 @@
#endif
static const int ERROR = -1;
#define BLINK_MSG_TIME 700000 // 3 fast blinks
bool is_multirotor(const struct vehicle_status_s *current_status)
{
return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) ||
(current_status->system_type == VEHICLE_TYPE_HEXAROTOR) ||
(current_status->system_type == VEHICLE_TYPE_OCTOROTOR) ||
(current_status->system_type == VEHICLE_TYPE_TRICOPTER));
(current_status->system_type == VEHICLE_TYPE_HEXAROTOR) ||
(current_status->system_type == VEHICLE_TYPE_OCTOROTOR) ||
(current_status->system_type == VEHICLE_TYPE_TRICOPTER));
}
bool is_rotary_wing(const struct vehicle_status_s *current_status)
{
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 hrt_abstime blink_msg_end;
int buzzer_init()
{
@ -104,16 +107,25 @@ void tune_error()
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);
}
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);
}
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);
}
@ -132,18 +144,31 @@ int tune_critical_bat()
return ioctl(buzzer, TONE_SET_ALARM, TONE_BATTERY_WARNING_FAST_TUNE);
}
void tune_stop()
{
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 rgbleds;
int led_init()
{
blink_msg_end = 0;
/* first open normal LEDs */
leds = open(LED_DEVICE_PATH, 0);
@ -159,6 +184,7 @@ int led_init()
warnx("Blue LED: ioctl fail\n");
return ERROR;
}
#endif
if (ioctl(leds, LED_ON, LED_AMBER)) {
@ -168,6 +194,7 @@ int led_init()
/* then try RGB LEDs, this can fail on FMUv1*/
rgbleds = open(RGBLED_DEVICE_PATH, 0);
if (rgbleds == -1) {
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
errx(1, "Unable to open " RGBLED_DEVICE_PATH);
@ -203,19 +230,22 @@ int led_off(int 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)
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)
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)
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_critical_bat(void);
void tune_stop(void);
int blink_msg_state();
int led_init(void);
void led_deinit(void);
@ -70,9 +71,7 @@ int led_on(int led);
int led_off(int led);
void rgbled_set_color(rgbled_color_t color);
void rgbled_set_mode(rgbled_mode_t mode);
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);
}
hil_global_pos.valid = true;
hil_global_pos.lat = hil_state.lat;
hil_global_pos.lon = hil_state.lon;
hil_global_pos.alt = hil_state.alt / 1000.0f;