mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-18 17:19:06 +08:00
Merge branch 'master' into inav_fix
This commit is contained in:
Anton Babushkin
commit
4679a13f17
84
ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil
Normal file
84
ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil
Normal 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"
|
||||
|
||||
@ -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
|
||||
|
||||
|
||||
@ -262,7 +262,7 @@ CONFIG_STM32_USART=y
|
||||
# U[S]ART Configuration
|
||||
#
|
||||
# CONFIG_USART1_RS485 is not set
|
||||
# CONFIG_USART1_RXDMA is not set
|
||||
CONFIG_USART1_RXDMA=y
|
||||
# CONFIG_USART2_RS485 is not set
|
||||
CONFIG_USART2_RXDMA=y
|
||||
# CONFIG_USART3_RXDMA is not set
|
||||
|
||||
@ -85,7 +85,7 @@ static const int ERROR = -1;
|
||||
class GPS : public device::CDev
|
||||
{
|
||||
public:
|
||||
GPS(const char* uart_path);
|
||||
GPS(const char *uart_path);
|
||||
virtual ~GPS();
|
||||
|
||||
virtual int init();
|
||||
@ -156,7 +156,7 @@ GPS *g_dev;
|
||||
}
|
||||
|
||||
|
||||
GPS::GPS(const char* uart_path) :
|
||||
GPS::GPS(const char *uart_path) :
|
||||
CDev("gps", GPS_DEVICE_PATH),
|
||||
_task_should_exit(false),
|
||||
_healthy(false),
|
||||
@ -192,6 +192,7 @@ GPS::~GPS()
|
||||
/* well, kill it anyway, though this will probably crash */
|
||||
if (_task != -1)
|
||||
task_delete(_task);
|
||||
|
||||
g_dev = nullptr;
|
||||
|
||||
}
|
||||
@ -270,19 +271,24 @@ GPS::task_main()
|
||||
}
|
||||
|
||||
switch (_mode) {
|
||||
case GPS_DRIVER_MODE_UBX:
|
||||
_Helper = new UBX(_serial_fd, &_report);
|
||||
break;
|
||||
case GPS_DRIVER_MODE_MTK:
|
||||
_Helper = new MTK(_serial_fd, &_report);
|
||||
break;
|
||||
case GPS_DRIVER_MODE_NMEA:
|
||||
//_Helper = new NMEA(); //TODO: add NMEA
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
case GPS_DRIVER_MODE_UBX:
|
||||
_Helper = new UBX(_serial_fd, &_report);
|
||||
break;
|
||||
|
||||
case GPS_DRIVER_MODE_MTK:
|
||||
_Helper = new MTK(_serial_fd, &_report);
|
||||
break;
|
||||
|
||||
case GPS_DRIVER_MODE_NMEA:
|
||||
//_Helper = new NMEA(); //TODO: add NMEA
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
unlock();
|
||||
|
||||
if (_Helper->configure(_baudrate) == 0) {
|
||||
unlock();
|
||||
|
||||
@ -294,6 +300,7 @@ GPS::task_main()
|
||||
/* opportunistic publishing - else invalid data would end up on the bus */
|
||||
if (_report_pub > 0) {
|
||||
orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
|
||||
|
||||
} else {
|
||||
_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
|
||||
}
|
||||
@ -310,10 +317,30 @@ GPS::task_main()
|
||||
}
|
||||
|
||||
if (!_healthy) {
|
||||
warnx("module found");
|
||||
char *mode_str = "unknown";
|
||||
|
||||
switch (_mode) {
|
||||
case GPS_DRIVER_MODE_UBX:
|
||||
mode_str = "UBX";
|
||||
break;
|
||||
|
||||
case GPS_DRIVER_MODE_MTK:
|
||||
mode_str = "MTK";
|
||||
break;
|
||||
|
||||
case GPS_DRIVER_MODE_NMEA:
|
||||
mode_str = "NMEA";
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
warnx("module found: %s", mode_str);
|
||||
_healthy = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (_healthy) {
|
||||
warnx("module lost");
|
||||
_healthy = false;
|
||||
@ -322,25 +349,29 @@ GPS::task_main()
|
||||
|
||||
lock();
|
||||
}
|
||||
|
||||
lock();
|
||||
|
||||
/* select next mode */
|
||||
switch (_mode) {
|
||||
case GPS_DRIVER_MODE_UBX:
|
||||
_mode = GPS_DRIVER_MODE_MTK;
|
||||
break;
|
||||
case GPS_DRIVER_MODE_MTK:
|
||||
_mode = GPS_DRIVER_MODE_UBX;
|
||||
break;
|
||||
// case GPS_DRIVER_MODE_NMEA:
|
||||
// _mode = GPS_DRIVER_MODE_UBX;
|
||||
// break;
|
||||
default:
|
||||
break;
|
||||
case GPS_DRIVER_MODE_UBX:
|
||||
_mode = GPS_DRIVER_MODE_MTK;
|
||||
break;
|
||||
|
||||
case GPS_DRIVER_MODE_MTK:
|
||||
_mode = GPS_DRIVER_MODE_UBX;
|
||||
break;
|
||||
|
||||
// case GPS_DRIVER_MODE_NMEA:
|
||||
// _mode = GPS_DRIVER_MODE_UBX;
|
||||
// break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
debug("exiting");
|
||||
|
||||
warnx("exiting");
|
||||
|
||||
::close(_serial_fd);
|
||||
|
||||
@ -361,23 +392,29 @@ void
|
||||
GPS::print_info()
|
||||
{
|
||||
switch (_mode) {
|
||||
case GPS_DRIVER_MODE_UBX:
|
||||
warnx("protocol: UBX");
|
||||
break;
|
||||
case GPS_DRIVER_MODE_MTK:
|
||||
warnx("protocol: MTK");
|
||||
break;
|
||||
case GPS_DRIVER_MODE_NMEA:
|
||||
warnx("protocol: NMEA");
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
case GPS_DRIVER_MODE_UBX:
|
||||
warnx("protocol: UBX");
|
||||
break;
|
||||
|
||||
case GPS_DRIVER_MODE_MTK:
|
||||
warnx("protocol: MTK");
|
||||
break;
|
||||
|
||||
case GPS_DRIVER_MODE_NMEA:
|
||||
warnx("protocol: NMEA");
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK");
|
||||
|
||||
if (_report.timestamp_position != 0) {
|
||||
warnx("position lock: %dD, last update %4.2f seconds ago", (int)_report.fix_type,
|
||||
(double)((float)(hrt_absolute_time() - _report.timestamp_position) / 1000000.0f));
|
||||
warnx("position lock: %dD, satellites: %d, last update: %fms ago", (int)_report.fix_type,
|
||||
_report.satellites_visible, (hrt_absolute_time() - _report.timestamp_position) / 1000.0f);
|
||||
warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt);
|
||||
warnx("eph: %.2fm, epv: %.2fm", _report.eph_m, _report.epv_m);
|
||||
warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate());
|
||||
warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate());
|
||||
warnx("rate publication:\t%6.2f Hz", (double)_rate);
|
||||
@ -428,6 +465,7 @@ start(const char *uart_path)
|
||||
errx(1, "Could not open device path: %s\n", GPS_DEVICE_PATH);
|
||||
goto fail;
|
||||
}
|
||||
|
||||
exit(0);
|
||||
|
||||
fail:
|
||||
@ -503,7 +541,7 @@ gps_main(int argc, char *argv[])
|
||||
{
|
||||
|
||||
/* set to default */
|
||||
char* device_name = GPS_DEFAULT_UART_PORT;
|
||||
char *device_name = GPS_DEFAULT_UART_PORT;
|
||||
|
||||
/*
|
||||
* Start/load the driver.
|
||||
@ -513,15 +551,18 @@ gps_main(int argc, char *argv[])
|
||||
if (argc > 3) {
|
||||
if (!strcmp(argv[2], "-d")) {
|
||||
device_name = argv[3];
|
||||
|
||||
} else {
|
||||
goto out;
|
||||
}
|
||||
}
|
||||
|
||||
gps::start(device_name);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "stop"))
|
||||
gps::stop();
|
||||
|
||||
/*
|
||||
* Test the driver/device.
|
||||
*/
|
||||
|
||||
@ -87,13 +87,15 @@ GPS_Helper::set_baudrate(const int &fd, unsigned baud)
|
||||
|
||||
case 115200: speed = B115200; break;
|
||||
|
||||
warnx("try baudrate: %d\n", speed);
|
||||
warnx("try baudrate: %d\n", speed);
|
||||
|
||||
default:
|
||||
warnx("ERROR: Unsupported baudrate: %d\n", baud);
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
struct termios uart_config;
|
||||
|
||||
int termios_state;
|
||||
|
||||
/* fill the struct for the new configuration */
|
||||
@ -109,14 +111,17 @@ GPS_Helper::set_baudrate(const int &fd, unsigned baud)
|
||||
warnx("ERROR setting config: %d (cfsetispeed)\n", termios_state);
|
||||
return -1;
|
||||
}
|
||||
|
||||
if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
|
||||
warnx("ERROR setting config: %d (cfsetospeed)\n", termios_state);
|
||||
return -1;
|
||||
}
|
||||
|
||||
if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) {
|
||||
warnx("ERROR setting baudrate (tcsetattr)\n");
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* XXX if resetting the parser here, ensure it does exist (check for null pointer) */
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -33,7 +33,7 @@
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
/**
|
||||
* @file gps_helper.h
|
||||
*/
|
||||
|
||||
|
||||
@ -48,9 +48,9 @@
|
||||
|
||||
|
||||
MTK::MTK(const int &fd, struct vehicle_gps_position_s *gps_position) :
|
||||
_fd(fd),
|
||||
_gps_position(gps_position),
|
||||
_mtk_revision(0)
|
||||
_fd(fd),
|
||||
_gps_position(gps_position),
|
||||
_mtk_revision(0)
|
||||
{
|
||||
decode_init();
|
||||
}
|
||||
@ -73,24 +73,28 @@ MTK::configure(unsigned &baudrate)
|
||||
warnx("mtk: config write failed");
|
||||
return -1;
|
||||
}
|
||||
|
||||
usleep(10000);
|
||||
|
||||
if (strlen(MTK_SET_BINARY) != write(_fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY))) {
|
||||
warnx("mtk: config write failed");
|
||||
return -1;
|
||||
}
|
||||
|
||||
usleep(10000);
|
||||
|
||||
if (strlen(SBAS_ON) != write(_fd, SBAS_ON, strlen(SBAS_ON))) {
|
||||
warnx("mtk: config write failed");
|
||||
return -1;
|
||||
}
|
||||
|
||||
usleep(10000);
|
||||
|
||||
if (strlen(WAAS_ON) != write(_fd, WAAS_ON, strlen(WAAS_ON))) {
|
||||
warnx("mtk: config write failed");
|
||||
return -1;
|
||||
}
|
||||
|
||||
usleep(10000);
|
||||
|
||||
if (strlen(MTK_NAVTHRES_OFF) != write(_fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) {
|
||||
@ -128,12 +132,15 @@ MTK::receive(unsigned timeout)
|
||||
handle_message(packet);
|
||||
return 1;
|
||||
}
|
||||
|
||||
/* in case we keep trying but only get crap from GPS */
|
||||
if (time_started + timeout*1000 < hrt_absolute_time() ) {
|
||||
if (time_started + timeout * 1000 < hrt_absolute_time()) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
j++;
|
||||
}
|
||||
|
||||
/* everything is read */
|
||||
j = count = 0;
|
||||
}
|
||||
@ -181,6 +188,7 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet)
|
||||
if (b == MTK_SYNC1_V16) {
|
||||
_decode_state = MTK_DECODE_GOT_CK_A;
|
||||
_mtk_revision = 16;
|
||||
|
||||
} else if (b == MTK_SYNC1_V19) {
|
||||
_decode_state = MTK_DECODE_GOT_CK_A;
|
||||
_mtk_revision = 19;
|
||||
@ -201,7 +209,7 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet)
|
||||
add_byte_to_checksum(b);
|
||||
|
||||
// Fill packet buffer
|
||||
((uint8_t*)(&packet))[_rx_count] = b;
|
||||
((uint8_t *)(&packet))[_rx_count] = b;
|
||||
_rx_count++;
|
||||
|
||||
/* Packet size minus checksum, XXX ? */
|
||||
@ -209,14 +217,17 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet)
|
||||
/* Compare checksum */
|
||||
if (_rx_ck_a == packet.ck_a && _rx_ck_b == packet.ck_b) {
|
||||
ret = 1;
|
||||
|
||||
} else {
|
||||
warnx("MTK Checksum invalid");
|
||||
ret = -1;
|
||||
}
|
||||
|
||||
// Reset state machine to decode next packet
|
||||
decode_init();
|
||||
}
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
@ -226,19 +237,22 @@ MTK::handle_message(gps_mtk_packet_t &packet)
|
||||
if (_mtk_revision == 16) {
|
||||
_gps_position->lat = packet.latitude * 10; // from degrees*1e6 to degrees*1e7
|
||||
_gps_position->lon = packet.longitude * 10; // from degrees*1e6 to degrees*1e7
|
||||
|
||||
} else if (_mtk_revision == 19) {
|
||||
_gps_position->lat = packet.latitude; // both degrees*1e7
|
||||
_gps_position->lon = packet.longitude; // both degrees*1e7
|
||||
|
||||
} else {
|
||||
warnx("mtk: unknown revision");
|
||||
_gps_position->lat = 0;
|
||||
_gps_position->lon = 0;
|
||||
}
|
||||
|
||||
_gps_position->alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm
|
||||
_gps_position->fix_type = packet.fix_type;
|
||||
_gps_position->eph_m = packet.hdop; // XXX: Check this because eph_m is in m and hdop is without unit
|
||||
_gps_position->epv_m = 0.0; //unknown in mtk custom mode
|
||||
_gps_position->vel_m_s = ((float)packet.ground_speed)*1e-2f; // from cm/s to m/s
|
||||
_gps_position->vel_m_s = ((float)packet.ground_speed) * 1e-2f; // from cm/s to m/s
|
||||
_gps_position->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad
|
||||
_gps_position->satellites_visible = packet.satellites;
|
||||
|
||||
|
||||
@ -60,13 +60,14 @@
|
||||
#define UBX_CONFIG_TIMEOUT 200 // ms, timeout for waiting ACK
|
||||
#define UBX_PACKET_TIMEOUT 2 // ms, if now data during this delay assume that full update received
|
||||
#define UBX_WAIT_BEFORE_READ 20 // ms, wait before reading to save read() calls
|
||||
#define DISABLE_MSG_INTERVAL 1000000 // us, try to disable message with this interval
|
||||
|
||||
UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) :
|
||||
_fd(fd),
|
||||
_gps_position(gps_position),
|
||||
_configured(false),
|
||||
_waiting_for_ack(false),
|
||||
_disable_cmd_counter(0)
|
||||
_disable_cmd_last(0)
|
||||
{
|
||||
decode_init();
|
||||
}
|
||||
@ -191,35 +192,35 @@ UBX::configure(unsigned &baudrate)
|
||||
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_POSLLH, 1);
|
||||
|
||||
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
|
||||
warnx("ubx: msg rate configuration failed: NAV POSLLH\n");
|
||||
warnx("ubx: msg rate configuration failed: NAV POSLLH");
|
||||
return 1;
|
||||
}
|
||||
|
||||
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, 1);
|
||||
|
||||
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
|
||||
warnx("ubx: msg rate configuration failed: NAV TIMEUTC\n");
|
||||
warnx("ubx: msg rate configuration failed: NAV TIMEUTC");
|
||||
return 1;
|
||||
}
|
||||
|
||||
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL, 1);
|
||||
|
||||
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
|
||||
warnx("ubx: msg rate configuration failed: NAV SOL\n");
|
||||
warnx("ubx: msg rate configuration failed: NAV SOL");
|
||||
return 1;
|
||||
}
|
||||
|
||||
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED, 1);
|
||||
|
||||
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
|
||||
warnx("ubx: msg rate configuration failed: NAV VELNED\n");
|
||||
warnx("ubx: msg rate configuration failed: NAV VELNED");
|
||||
return 1;
|
||||
}
|
||||
|
||||
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5);
|
||||
|
||||
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
|
||||
warnx("ubx: msg rate configuration failed: NAV SVINFO\n");
|
||||
warnx("ubx: msg rate configuration failed: NAV SVINFO");
|
||||
return 1;
|
||||
}
|
||||
|
||||
@ -271,11 +272,18 @@ UBX::receive(unsigned timeout)
|
||||
|
||||
if (ret < 0) {
|
||||
/* something went wrong when polling */
|
||||
warnx("ubx: poll error");
|
||||
return -1;
|
||||
|
||||
} else if (ret == 0) {
|
||||
/* return success after short delay after receiving a packet or timeout after long delay */
|
||||
return handled ? 1 : -1;
|
||||
if (handled) {
|
||||
return 1;
|
||||
|
||||
} else {
|
||||
warnx("ubx: timeout - no messages");
|
||||
return -1;
|
||||
}
|
||||
|
||||
} else if (ret > 0) {
|
||||
/* if we have new data from GPS, go handle it */
|
||||
@ -292,8 +300,6 @@ UBX::receive(unsigned timeout)
|
||||
/* pass received bytes to the packet decoder */
|
||||
for (int i = 0; i < count; i++) {
|
||||
if (parse_char(buf[i]) > 0) {
|
||||
/* return to configure during configuration or to the gps driver during normal work
|
||||
* if a packet has arrived */
|
||||
if (handle_message() > 0)
|
||||
handled = true;
|
||||
}
|
||||
@ -303,6 +309,7 @@ UBX::receive(unsigned timeout)
|
||||
|
||||
/* abort after timeout if no useful packets received */
|
||||
if (time_started + timeout * 1000 < hrt_absolute_time()) {
|
||||
warnx("ubx: timeout - no useful messages");
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
@ -453,16 +460,16 @@ UBX::handle_message()
|
||||
timeinfo.tm_min = packet->min;
|
||||
timeinfo.tm_sec = packet->sec;
|
||||
time_t epoch = mktime(&timeinfo);
|
||||
|
||||
|
||||
#ifndef CONFIG_RTC
|
||||
//Since we lack a hardware RTC, set the system time clock based on GPS UTC
|
||||
//TODO generalize this by moving into gps.cpp?
|
||||
timespec ts;
|
||||
ts.tv_sec = epoch;
|
||||
ts.tv_nsec = packet->time_nanoseconds;
|
||||
clock_settime(CLOCK_REALTIME,&ts);
|
||||
//Since we lack a hardware RTC, set the system time clock based on GPS UTC
|
||||
//TODO generalize this by moving into gps.cpp?
|
||||
timespec ts;
|
||||
ts.tv_sec = epoch;
|
||||
ts.tv_nsec = packet->time_nanoseconds;
|
||||
clock_settime(CLOCK_REALTIME, &ts);
|
||||
#endif
|
||||
|
||||
|
||||
_gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this
|
||||
_gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f);
|
||||
_gps_position->timestamp_time = hrt_absolute_time();
|
||||
@ -564,10 +571,13 @@ UBX::handle_message()
|
||||
|
||||
if (ret == 0) {
|
||||
/* message not handled */
|
||||
warnx("ubx: unknown message received: 0x%02x-0x%02x\n", (unsigned)_message_class, (unsigned)_message_id);
|
||||
warnx("ubx: unknown message received: 0x%02x-0x%02x", (unsigned)_message_class, (unsigned)_message_id);
|
||||
|
||||
if ((_disable_cmd_counter = _disable_cmd_counter++ % 10) == 0) {
|
||||
hrt_abstime t = hrt_absolute_time();
|
||||
|
||||
if (t > _disable_cmd_last + DISABLE_MSG_INTERVAL) {
|
||||
/* don't attempt for every message to disable, some might not be disabled */
|
||||
_disable_cmd_last = t;
|
||||
warnx("ubx: disabling message 0x%02x-0x%02x", (unsigned)_message_class, (unsigned)_message_id);
|
||||
configure_message_rate(_message_class, _message_id, 0);
|
||||
}
|
||||
@ -640,7 +650,7 @@ UBX::add_checksum_to_message(uint8_t *message, const unsigned length)
|
||||
ck_b = ck_b + ck_a;
|
||||
}
|
||||
|
||||
/* The checksum is written to the last to bytes of a message */
|
||||
/* the checksum is written to the last to bytes of a message */
|
||||
message[length - 2] = ck_a;
|
||||
message[length - 1] = ck_b;
|
||||
}
|
||||
@ -669,17 +679,17 @@ UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length)
|
||||
{
|
||||
ssize_t ret = 0;
|
||||
|
||||
/* Calculate the checksum now */
|
||||
/* calculate the checksum now */
|
||||
add_checksum_to_message(packet, length);
|
||||
|
||||
const uint8_t sync_bytes[] = {UBX_SYNC1, UBX_SYNC2};
|
||||
|
||||
/* Start with the two sync bytes */
|
||||
/* start with the two sync bytes */
|
||||
ret += write(fd, sync_bytes, sizeof(sync_bytes));
|
||||
ret += write(fd, packet, length);
|
||||
|
||||
if (ret != (int)length + (int)sizeof(sync_bytes)) // XXX is there a neater way to get rid of the unsigned signed warning?
|
||||
warnx("ubx: config write fail");
|
||||
warnx("ubx: configuration write fail");
|
||||
}
|
||||
|
||||
void
|
||||
@ -696,7 +706,7 @@ UBX::send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size)
|
||||
add_checksum((uint8_t *)&header.msg_class, sizeof(header) - 2, ck_a, ck_b);
|
||||
add_checksum((uint8_t *)msg, size, ck_a, ck_b);
|
||||
|
||||
// Configure receive check
|
||||
/* configure ACK check */
|
||||
_message_class_needed = msg_class;
|
||||
_message_id_needed = msg_id;
|
||||
|
||||
|
||||
@ -347,7 +347,7 @@ private:
|
||||
/**
|
||||
* Add the two checksum bytes to an outgoing message
|
||||
*/
|
||||
void add_checksum_to_message(uint8_t* message, const unsigned length);
|
||||
void add_checksum_to_message(uint8_t *message, const unsigned length);
|
||||
|
||||
/**
|
||||
* Helper to send a config packet
|
||||
@ -358,7 +358,7 @@ private:
|
||||
|
||||
void send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size);
|
||||
|
||||
void add_checksum(uint8_t* message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b);
|
||||
void add_checksum(uint8_t *message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b);
|
||||
|
||||
int wait_for_ack(unsigned timeout);
|
||||
|
||||
@ -376,7 +376,7 @@ private:
|
||||
uint8_t _message_class;
|
||||
uint8_t _message_id;
|
||||
unsigned _payload_size;
|
||||
uint8_t _disable_cmd_counter;
|
||||
uint8_t _disable_cmd_last;
|
||||
};
|
||||
|
||||
#endif /* UBX_H_ */
|
||||
|
||||
@ -2245,7 +2245,7 @@ px4io_main(int argc, char *argv[])
|
||||
}
|
||||
|
||||
PX4IO_Uploader *up;
|
||||
const char *fn[5];
|
||||
const char *fn[3];
|
||||
|
||||
/* work out what we're uploading... */
|
||||
if (argc > 2) {
|
||||
@ -2253,11 +2253,19 @@ px4io_main(int argc, char *argv[])
|
||||
fn[1] = nullptr;
|
||||
|
||||
} else {
|
||||
fn[0] = "/etc/extras/px4io-v2_default.bin";
|
||||
fn[1] = "/etc/extras/px4io-v1_default.bin";
|
||||
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
|
||||
fn[0] = "/etc/extras/px4io-v1_default.bin";
|
||||
fn[1] = "/fs/microsd/px4io1.bin";
|
||||
fn[2] = "/fs/microsd/px4io.bin";
|
||||
fn[3] = "/fs/microsd/px4io2.bin";
|
||||
fn[4] = nullptr;
|
||||
fn[3] = nullptr;
|
||||
#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
|
||||
fn[0] = "/etc/extras/px4io-v2_default.bin";
|
||||
fn[1] = "/fs/microsd/px4io2.bin";
|
||||
fn[2] = "/fs/microsd/px4io.bin";
|
||||
fn[3] = nullptr;
|
||||
#else
|
||||
#error "unknown board"
|
||||
#endif
|
||||
}
|
||||
|
||||
up = new PX4IO_Uploader;
|
||||
|
||||
@ -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 ¬_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 ¬_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 ¬_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);
|
||||
}
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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);
|
||||
|
||||
/**
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -99,6 +99,8 @@ struct listener {
|
||||
uintptr_t arg;
|
||||
};
|
||||
|
||||
uint16_t cm_uint16_from_m_float(float m);
|
||||
|
||||
static void l_sensor_combined(const struct listener *l);
|
||||
static void l_vehicle_attitude(const struct listener *l);
|
||||
static void l_vehicle_gps_position(const struct listener *l);
|
||||
@ -150,6 +152,19 @@ static const struct listener listeners[] = {
|
||||
|
||||
static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]);
|
||||
|
||||
uint16_t
|
||||
cm_uint16_from_m_float(float m)
|
||||
{
|
||||
if (m < 0.0f) {
|
||||
return 0;
|
||||
|
||||
} else if (m > 655.35f) {
|
||||
return 65535;
|
||||
}
|
||||
|
||||
return (uint16_t)(m * 100.0f);
|
||||
}
|
||||
|
||||
void
|
||||
l_sensor_combined(const struct listener *l)
|
||||
{
|
||||
@ -235,8 +250,10 @@ l_vehicle_gps_position(const struct listener *l)
|
||||
|
||||
/* GPS COG is 0..2PI in degrees * 1e2 */
|
||||
float cog_deg = gps.cog_rad;
|
||||
|
||||
if (cog_deg > M_PI_F)
|
||||
cog_deg -= 2.0f * M_PI_F;
|
||||
|
||||
cog_deg *= M_RAD_TO_DEG_F;
|
||||
|
||||
|
||||
@ -247,10 +264,10 @@ l_vehicle_gps_position(const struct listener *l)
|
||||
gps.lat,
|
||||
gps.lon,
|
||||
gps.alt,
|
||||
gps.eph_m * 1e2f, // from m to cm
|
||||
gps.epv_m * 1e2f, // from m to cm
|
||||
cm_uint16_from_m_float(gps.eph_m),
|
||||
cm_uint16_from_m_float(gps.epv_m),
|
||||
gps.vel_m_s * 1e2f, // from m/s to cm/s
|
||||
cog_deg * 1e2f, // from rad to deg * 100
|
||||
cog_deg * 1e2f, // from deg to deg * 100
|
||||
gps.satellites_visible);
|
||||
|
||||
/* update SAT info every 10 seconds */
|
||||
|
||||
@ -509,31 +509,57 @@ int
|
||||
param_save_default(void)
|
||||
{
|
||||
int result;
|
||||
unsigned retries = 0;
|
||||
|
||||
/* delete the file in case it exists */
|
||||
struct stat buffer;
|
||||
if (stat(param_get_default_file(), &buffer) == 0) {
|
||||
result = unlink(param_get_default_file());
|
||||
|
||||
do {
|
||||
result = unlink(param_get_default_file());
|
||||
if (result != 0) {
|
||||
retries++;
|
||||
usleep(1000 * retries);
|
||||
}
|
||||
} while (result != OK && retries < 10);
|
||||
|
||||
if (result != OK)
|
||||
warnx("unlinking file %s failed.", param_get_default_file());
|
||||
}
|
||||
|
||||
/* create the file */
|
||||
int fd = open(param_get_default_file(), O_WRONLY | O_CREAT | O_EXCL);
|
||||
int fd;
|
||||
|
||||
do {
|
||||
/* do another attempt in case the unlink call is not synced yet */
|
||||
fd = open(param_get_default_file(), O_WRONLY | O_CREAT | O_EXCL);
|
||||
if (fd < 0) {
|
||||
retries++;
|
||||
usleep(1000 * retries);
|
||||
}
|
||||
|
||||
} while (fd < 0 && retries < 10);
|
||||
|
||||
if (fd < 0) {
|
||||
/* do another attempt in case the unlink call is not synced yet */
|
||||
usleep(5000);
|
||||
fd = open(param_get_default_file(), O_WRONLY | O_CREAT | O_EXCL);
|
||||
|
||||
|
||||
warn("opening '%s' for writing failed", param_get_default_file());
|
||||
return fd;
|
||||
}
|
||||
|
||||
result = param_export(fd, false);
|
||||
do {
|
||||
result = param_export(fd, false);
|
||||
|
||||
if (result != OK) {
|
||||
retries++;
|
||||
usleep(1000 * retries);
|
||||
}
|
||||
|
||||
} while (result != 0 && retries < 10);
|
||||
|
||||
|
||||
close(fd);
|
||||
|
||||
if (result != 0) {
|
||||
if (result != OK) {
|
||||
warn("error exporting parameters to '%s'", param_get_default_file());
|
||||
(void)unlink(param_get_default_file());
|
||||
return result;
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user