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

Merge remote-tracking branch 'upstream/master' into hkmicropcb

This commit is contained in:
Thomas Gubler 2014-07-03 09:33:04 +02:00
parent 0d7ada14bb f428ebb04f
commit 9d9b08cc75
28 changed files with 1285 additions and 824 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ROMFS/px4fmu_common/init.d/10015_tbs_discovery
View File

@ -20,7 +20,7 @@ then
param set MC_PITCHRATE_D 0.0025
param set MC_YAW_P 2.8
param set MC_YAWRATE_P 0.28
param set MC_YAWRATE_I 0.0
param set MC_YAWRATE_I 0.1
param set MC_YAWRATE_D 0.0
fi

2
ROMFS/px4fmu_common/init.d/4010_dji_f330
View File

@ -19,7 +19,7 @@ then
param set MC_PITCHRATE_D 0.003
param set MC_YAW_P 2.8
param set MC_YAWRATE_P 0.2
param set MC_YAWRATE_I 0.0
param set MC_YAWRATE_I 0.1
param set MC_YAWRATE_D 0.0
fi

2
ROMFS/px4fmu_common/init.d/4011_dji_f450
View File

@ -20,7 +20,7 @@ then
param set MC_PITCHRATE_D 0.003
param set MC_YAW_P 2.8
param set MC_YAWRATE_P 0.2
param set MC_YAWRATE_I 0.0
param set MC_YAWRATE_I 0.1
param set MC_YAWRATE_D 0.0
fi

2
ROMFS/px4fmu_common/init.d/rc.mc_defaults
View File

@ -14,7 +14,7 @@ then
param set MC_PITCHRATE_D 0.003
param set MC_YAW_P 2.8
param set MC_YAWRATE_P 0.2
param set MC_YAWRATE_I 0.0
param set MC_YAWRATE_I 0.1
param set MC_YAWRATE_D 0.0
param set MC_YAW_FF 0.5

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

@ -132,6 +132,10 @@ then
#
if param compare SYS_AUTOCONFIG 1
then
# We can't be sure the defaults haven't changed, so
# if someone requests a re-configuration, we do it
# cleanly from scratch (except autostart / autoconfig)
param reset_nostart
set DO_AUTOCONFIG yes
else
set DO_AUTOCONFIG no

2
Tools/px_generate_xml.sh Normal file
View File

@ -0,0 +1,2 @@
#!/bin/sh
python px_process_params.py --xml

8
src/drivers/blinkm/blinkm.cpp
View File

@ -559,13 +559,7 @@ BlinkM::led()
}
/* get number of used satellites in navigation */
num_of_used_sats = 0;
for(unsigned satloop=0; satloop<sizeof(vehicle_gps_position_raw.satellite_used); satloop++) {
if(vehicle_gps_position_raw.satellite_used[satloop] == 1) {
num_of_used_sats++;
}
}
num_of_used_sats = vehicle_gps_position_raw.satellites_used;
if (new_data_vehicle_status || no_data_vehicle_status < 3) {
if (num_of_cells == 0) {

139
src/drivers/gps/gps.cpp
View File

@ -63,6 +63,7 @@
#include <drivers/drv_gps.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/satellite_info.h>
#include <board_config.h>
@ -79,10 +80,18 @@
#endif
static const int ERROR = -1;
/* class for dynamic allocation of satellite info data */
class GPS_Sat_Info
{
public:
struct satellite_info_s _data;
};
class GPS : public device::CDev
{
public:
GPS(const char *uart_path, bool fake_gps);
GPS(const char *uart_path, bool fake_gps, bool enable_sat_info);
virtual ~GPS();
virtual int init();
@ -100,14 +109,17 @@ private:
int _serial_fd; ///< serial interface to GPS
unsigned _baudrate; ///< current baudrate
char _port[20]; ///< device / serial port path
volatile int _task; //< worker task
volatile int _task; ///< worker task
bool _healthy; ///< flag to signal if the GPS is ok
bool _baudrate_changed; ///< flag to signal that the baudrate with the GPS has changed
bool _baudrate_changed; ///< flag to signal that the baudrate with the GPS has changed
bool _mode_changed; ///< flag that the GPS mode has changed
gps_driver_mode_t _mode; ///< current mode
GPS_Helper *_Helper; ///< instance of GPS parser
struct vehicle_gps_position_s _report; ///< uORB topic for gps position
orb_advert_t _report_pub; ///< uORB pub for gps position
GPS_Sat_Info *_Sat_Info; ///< instance of GPS sat info data object
struct vehicle_gps_position_s _report_gps_pos; ///< uORB topic for gps position
orb_advert_t _report_gps_pos_pub; ///< uORB pub for gps position
struct satellite_info_s *_p_report_sat_info; ///< pointer to uORB topic for satellite info
orb_advert_t _report_sat_info_pub; ///< uORB pub for satellite info
float _rate; ///< position update rate
bool _fake_gps; ///< fake gps output
@ -154,14 +166,17 @@ GPS *g_dev;
}
GPS::GPS(const char *uart_path, bool fake_gps) :
GPS::GPS(const char *uart_path, bool fake_gps, bool enable_sat_info) :
CDev("gps", GPS_DEVICE_PATH),
_task_should_exit(false),
_healthy(false),
_mode_changed(false),
_mode(GPS_DRIVER_MODE_UBX),
_Helper(nullptr),
_report_pub(-1),
_Sat_Info(nullptr),
_report_gps_pos_pub(-1),
_p_report_sat_info(nullptr),
_report_sat_info_pub(-1),
_rate(0.0f),
_fake_gps(fake_gps)
{
@ -172,7 +187,14 @@ GPS::GPS(const char *uart_path, bool fake_gps) :
/* we need this potentially before it could be set in task_main */
g_dev = this;
memset(&_report, 0, sizeof(_report));
memset(&_report_gps_pos, 0, sizeof(_report_gps_pos));
/* create satellite info data object if requested */
if (enable_sat_info) {
_Sat_Info = new(GPS_Sat_Info);
_p_report_sat_info = &_Sat_Info->_data;
memset(_p_report_sat_info, 0, sizeof(*_p_report_sat_info));
}
_debug_enabled = true;
}
@ -207,7 +229,7 @@ GPS::init()
/* start the GPS driver worker task */
_task = task_spawn_cmd("gps", SCHED_DEFAULT,
SCHED_PRIORITY_SLOW_DRIVER, 2000, (main_t)&GPS::task_main_trampoline, nullptr);
SCHED_PRIORITY_SLOW_DRIVER, 1500, (main_t)&GPS::task_main_trampoline, nullptr);
if (_task < 0) {
warnx("task start failed: %d", errno);
@ -271,33 +293,33 @@ GPS::task_main()
if (_fake_gps) {
_report.timestamp_position = hrt_absolute_time();
_report.lat = (int32_t)47.378301e7f;
_report.lon = (int32_t)8.538777e7f;
_report.alt = (int32_t)1200e3f;
_report.timestamp_variance = hrt_absolute_time();
_report.s_variance_m_s = 10.0f;
_report.p_variance_m = 10.0f;
_report.c_variance_rad = 0.1f;
_report.fix_type = 3;
_report.eph = 0.9f;
_report.epv = 1.8f;
_report.timestamp_velocity = hrt_absolute_time();
_report.vel_n_m_s = 0.0f;
_report.vel_e_m_s = 0.0f;
_report.vel_d_m_s = 0.0f;
_report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s);
_report.cog_rad = 0.0f;
_report.vel_ned_valid = true;
_report_gps_pos.timestamp_position = hrt_absolute_time();
_report_gps_pos.lat = (int32_t)47.378301e7f;
_report_gps_pos.lon = (int32_t)8.538777e7f;
_report_gps_pos.alt = (int32_t)1200e3f;
_report_gps_pos.timestamp_variance = hrt_absolute_time();
_report_gps_pos.s_variance_m_s = 10.0f;
_report_gps_pos.p_variance_m = 10.0f;
_report_gps_pos.c_variance_rad = 0.1f;
_report_gps_pos.fix_type = 3;
_report_gps_pos.eph = 0.9f;
_report_gps_pos.epv = 1.8f;
_report_gps_pos.timestamp_velocity = hrt_absolute_time();
_report_gps_pos.vel_n_m_s = 0.0f;
_report_gps_pos.vel_e_m_s = 0.0f;
_report_gps_pos.vel_d_m_s = 0.0f;
_report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s * _report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s);
_report_gps_pos.cog_rad = 0.0f;
_report_gps_pos.vel_ned_valid = true;
//no time and satellite information simulated
if (!(_pub_blocked)) {
if (_report_pub > 0) {
orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
if (_report_gps_pos_pub > 0) {
orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
} else {
_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
_report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos);
}
}
@ -313,11 +335,11 @@ GPS::task_main()
switch (_mode) {
case GPS_DRIVER_MODE_UBX:
_Helper = new UBX(_serial_fd, &_report);
_Helper = new UBX(_serial_fd, &_report_gps_pos, _p_report_sat_info);
break;
case GPS_DRIVER_MODE_MTK:
_Helper = new MTK(_serial_fd, &_report);
_Helper = new MTK(_serial_fd, &_report_gps_pos);
break;
default:
@ -332,20 +354,33 @@ GPS::task_main()
// GPS is obviously detected successfully, reset statistics
_Helper->reset_update_rates();
while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) {
int helper_ret;
while ((helper_ret = _Helper->receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) {
// lock();
/* opportunistic publishing - else invalid data would end up on the bus */
if (!(_pub_blocked)) {
if (_report_pub > 0) {
orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
if (helper_ret & 1) {
if (_report_gps_pos_pub > 0) {
orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
} else {
_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
} else {
_report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos);
}
}
if (_p_report_sat_info && (helper_ret & 2)) {
if (_report_sat_info_pub > 0) {
orb_publish(ORB_ID(satellite_info), _report_sat_info_pub, _p_report_sat_info);
} else {
_report_sat_info_pub = orb_advertise(ORB_ID(satellite_info), _p_report_sat_info);
}
}
}
last_rate_count++;
if (helper_ret & 1) { // consider only pos info updates for rate calculation */
last_rate_count++;
}
/* measure update rate every 5 seconds */
if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) {
@ -446,12 +481,13 @@ GPS::print_info()
}
warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK");
warnx("sat info: %s", (_p_report_sat_info != nullptr) ? "enabled" : "disabled");
if (_report.timestamp_position != 0) {
warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report.fix_type,
_report.satellites_visible, (double)(hrt_absolute_time() - _report.timestamp_position) / 1000.0d);
warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt);
warnx("eph: %.2fm, epv: %.2fm", (double)_report.eph, (double)_report.epv);
if (_report_gps_pos.timestamp_position != 0) {
warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report_gps_pos.fix_type,
_report_gps_pos.satellites_used, (double)(hrt_absolute_time() - _report_gps_pos.timestamp_position) / 1000.0);
warnx("lat: %d, lon: %d, alt: %d", _report_gps_pos.lat, _report_gps_pos.lon, _report_gps_pos.alt);
warnx("eph: %.2fm, epv: %.2fm", (double)_report_gps_pos.eph, (double)_report_gps_pos.epv);
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);
@ -469,7 +505,7 @@ namespace gps
GPS *g_dev;
void start(const char *uart_path, bool fake_gps);
void start(const char *uart_path, bool fake_gps, bool enable_sat_info);
void stop();
void test();
void reset();
@ -479,7 +515,7 @@ void info();
* Start the driver.
*/
void
start(const char *uart_path, bool fake_gps)
start(const char *uart_path, bool fake_gps, bool enable_sat_info)
{
int fd;
@ -487,7 +523,7 @@ start(const char *uart_path, bool fake_gps)
errx(1, "already started");
/* create the driver */
g_dev = new GPS(uart_path, fake_gps);
g_dev = new GPS(uart_path, fake_gps, enable_sat_info);
if (g_dev == nullptr)
goto fail;
@ -580,6 +616,7 @@ gps_main(int argc, char *argv[])
/* set to default */
const char *device_name = GPS_DEFAULT_UART_PORT;
bool fake_gps = false;
bool enable_sat_info = false;
/*
* Start/load the driver.
@ -601,7 +638,13 @@ gps_main(int argc, char *argv[])
fake_gps = true;
}
gps::start(device_name, fake_gps);
/* Detect sat info option */
for (int i = 2; i < argc; i++) {
if (!strcmp(argv[i], "-s"))
enable_sat_info = true;
}
gps::start(device_name, fake_gps, enable_sat_info);
}
if (!strcmp(argv[1], "stop"))
@ -626,5 +669,5 @@ gps_main(int argc, char *argv[])
gps::info();
out:
errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n][-f]");
errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n][-f][-s]");
}

4
src/drivers/gps/gps_helper.h
View File

@ -62,8 +62,8 @@ protected:
uint8_t _rate_count_lat_lon;
uint8_t _rate_count_vel;
float _rate_lat_lon;
float _rate_vel;
float _rate_lat_lon = 0.0f;
float _rate_vel = 0.0f;
uint64_t _interval_rate_start;
};

2
src/drivers/gps/mtk.cpp
View File

@ -263,7 +263,7 @@ MTK::handle_message(gps_mtk_packet_t &packet)
_gps_position->epv = _gps_position->eph; // unknown in mtk custom mode, so we cheat with eph
_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;
_gps_position->satellites_used = packet.satellites;
/* convert time and date information to unix timestamp */
struct tm timeinfo; //TODO: test this conversion

1052
src/drivers/gps/ubx.cpp
View File

File diff suppressed because it is too large Load Diff

673
src/drivers/gps/ubx.h
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2012, 2013, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -34,13 +34,16 @@
/**
* @file ubx.h
*
* U-Blox protocol definition. Following u-blox 6/7 Receiver Description
* U-Blox protocol definition. Following u-blox 6/7/8 Receiver Description
* including Prototol Specification.
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*
* @author Hannes Delago
* (rework, add ubx7+ compatibility)
*
*/
#ifndef UBX_H_
@ -51,319 +54,423 @@
#define UBX_SYNC1 0xB5
#define UBX_SYNC2 0x62
/* ClassIDs (the ones that are used) */
#define UBX_CLASS_NAV 0x01
//#define UBX_CLASS_RXM 0x02
#define UBX_CLASS_ACK 0x05
#define UBX_CLASS_CFG 0x06
#define UBX_CLASS_MON 0x0A
/* Message Classes */
#define UBX_CLASS_NAV 0x01
#define UBX_CLASS_ACK 0x05
#define UBX_CLASS_CFG 0x06
#define UBX_CLASS_MON 0x0A
/* MessageIDs (the ones that are used) */
#define UBX_MESSAGE_NAV_POSLLH 0x02
//#define UBX_MESSAGE_NAV_DOP 0x04
#define UBX_MESSAGE_NAV_SOL 0x06
#define UBX_MESSAGE_NAV_VELNED 0x12
//#define UBX_MESSAGE_RXM_SVSI 0x20
#define UBX_MESSAGE_NAV_TIMEUTC 0x21
#define UBX_MESSAGE_NAV_SVINFO 0x30
#define UBX_MESSAGE_ACK_NAK 0x00
#define UBX_MESSAGE_ACK_ACK 0x01
#define UBX_MESSAGE_CFG_PRT 0x00
#define UBX_MESSAGE_CFG_MSG 0x01
#define UBX_MESSAGE_CFG_RATE 0x08
#define UBX_MESSAGE_CFG_NAV5 0x24
/* Message IDs */
#define UBX_ID_NAV_POSLLH 0x02
#define UBX_ID_NAV_SOL 0x06
#define UBX_ID_NAV_PVT 0x07
#define UBX_ID_NAV_VELNED 0x12
#define UBX_ID_NAV_TIMEUTC 0x21
#define UBX_ID_NAV_SVINFO 0x30
#define UBX_ID_ACK_NAK 0x00
#define UBX_ID_ACK_ACK 0x01
#define UBX_ID_CFG_PRT 0x00
#define UBX_ID_CFG_MSG 0x01
#define UBX_ID_CFG_RATE 0x08
#define UBX_ID_CFG_NAV5 0x24
#define UBX_ID_MON_VER 0x04
#define UBX_ID_MON_HW 0x09
#define UBX_MESSAGE_MON_HW 0x09
/* Message Classes & IDs */
#define UBX_MSG_NAV_POSLLH ((UBX_CLASS_NAV) | UBX_ID_NAV_POSLLH << 8)
#define UBX_MSG_NAV_SOL ((UBX_CLASS_NAV) | UBX_ID_NAV_SOL << 8)
#define UBX_MSG_NAV_PVT ((UBX_CLASS_NAV) | UBX_ID_NAV_PVT << 8)
#define UBX_MSG_NAV_VELNED ((UBX_CLASS_NAV) | UBX_ID_NAV_VELNED << 8)
#define UBX_MSG_NAV_TIMEUTC ((UBX_CLASS_NAV) | UBX_ID_NAV_TIMEUTC << 8)
#define UBX_MSG_NAV_SVINFO ((UBX_CLASS_NAV) | UBX_ID_NAV_SVINFO << 8)
#define UBX_MSG_ACK_NAK ((UBX_CLASS_ACK) | UBX_ID_ACK_NAK << 8)
#define UBX_MSG_ACK_ACK ((UBX_CLASS_ACK) | UBX_ID_ACK_ACK << 8)
#define UBX_MSG_CFG_PRT ((UBX_CLASS_CFG) | UBX_ID_CFG_PRT << 8)
#define UBX_MSG_CFG_MSG ((UBX_CLASS_CFG) | UBX_ID_CFG_MSG << 8)
#define UBX_MSG_CFG_RATE ((UBX_CLASS_CFG) | UBX_ID_CFG_RATE << 8)
#define UBX_MSG_CFG_NAV5 ((UBX_CLASS_CFG) | UBX_ID_CFG_NAV5 << 8)
#define UBX_MSG_MON_HW ((UBX_CLASS_MON) | UBX_ID_MON_HW << 8)
#define UBX_MSG_MON_VER ((UBX_CLASS_MON) | UBX_ID_MON_VER << 8)
#define UBX_CFG_PRT_LENGTH 20
#define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< UART1 */
#define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */
#define UBX_CFG_PRT_PAYLOAD_BAUDRATE 38400 /**< choose 38400 as GPS baudrate */
#define UBX_CFG_PRT_PAYLOAD_INPROTOMASK 0x01 /**< UBX in */
#define UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK 0x01 /**< UBX out */
/* RX NAV-PVT message content details */
/* Bitfield "valid" masks */
#define UBX_RX_NAV_PVT_VALID_VALIDDATE 0x01 /**< validDate (Valid UTC Date) */
#define UBX_RX_NAV_PVT_VALID_VALIDTIME 0x02 /**< validTime (Valid UTC Time) */
#define UBX_RX_NAV_PVT_VALID_FULLYRESOLVED 0x04 /**< fullyResolved (1 = UTC Time of Day has been fully resolved (no seconds uncertainty)) */
#define UBX_CFG_RATE_LENGTH 6
#define UBX_CFG_RATE_PAYLOAD_MEASINTERVAL 200 /**< 200ms for 5Hz */
#define UBX_CFG_RATE_PAYLOAD_NAVRATE 1 /**< cannot be changed */
#define UBX_CFG_RATE_PAYLOAD_TIMEREF 0 /**< 0: UTC, 1: GPS time */
/* Bitfield "flags" masks */
#define UBX_RX_NAV_PVT_FLAGS_GNSSFIXOK 0x01 /**< gnssFixOK (A valid fix (i.e within DOP & accuracy masks)) */
#define UBX_RX_NAV_PVT_FLAGS_DIFFSOLN 0x02 /**< diffSoln (1 if differential corrections were applied) */
#define UBX_RX_NAV_PVT_FLAGS_PSMSTATE 0x1C /**< psmState (Power Save Mode state (see Power Management)) */
#define UBX_RX_NAV_PVT_FLAGS_HEADVEHVALID 0x20 /**< headVehValid (Heading of vehicle is valid) */
/* TX CFG-PRT message contents */
#define UBX_TX_CFG_PRT_PORTID 0x01 /**< UART1 */
#define UBX_TX_CFG_PRT_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */
#define UBX_TX_CFG_PRT_BAUDRATE 38400 /**< choose 38400 as GPS baudrate */
#define UBX_TX_CFG_PRT_INPROTOMASK 0x01 /**< UBX in */
#define UBX_TX_CFG_PRT_OUTPROTOMASK 0x01 /**< UBX out */
/* TX CFG-RATE message contents */
#define UBX_TX_CFG_RATE_MEASINTERVAL 200 /**< 200ms for 5Hz */
#define UBX_TX_CFG_RATE_NAVRATE 1 /**< cannot be changed */
#define UBX_TX_CFG_RATE_TIMEREF 0 /**< 0: UTC, 1: GPS time */
/* TX CFG-NAV5 message contents */
#define UBX_TX_CFG_NAV5_MASK 0x0005 /**< Only update dynamic model and fix mode */
#define UBX_TX_CFG_NAV5_DYNMODEL 7 /**< 0 Portable, 2 Stationary, 3 Pedestrian, 4 Automotive, 5 Sea, 6 Airborne <1g, 7 Airborne <2g, 8 Airborne <4g */
#define UBX_TX_CFG_NAV5_FIXMODE 2 /**< 1 2D only, 2 3D only, 3 Auto 2D/3D */
/* TX CFG-MSG message contents */
#define UBX_TX_CFG_MSG_RATE1_5HZ 0x01 /**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
#define UBX_TX_CFG_MSG_RATE1_1HZ 0x05 /**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
#define UBX_TX_CFG_MSG_RATE1_05HZ 10
#define UBX_CFG_NAV5_LENGTH 36
#define UBX_CFG_NAV5_PAYLOAD_MASK 0x0005 /**< XXX only update dynamic model and fix mode */
#define UBX_CFG_NAV5_PAYLOAD_DYNMODEL 7 /**< 0: portable, 2: stationary, 3: pedestrian, 4: automotive, 5: sea, 6: airborne <1g, 7: airborne <2g, 8: airborne <4g */
#define UBX_CFG_NAV5_PAYLOAD_FIXMODE 2 /**< 1: 2D only, 2: 3D only, 3: Auto 2D/3D */
#define UBX_CFG_MSG_LENGTH 8
#define UBX_CFG_MSG_PAYLOAD_RATE1_5HZ 0x01 /**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
#define UBX_CFG_MSG_PAYLOAD_RATE1_1HZ 0x05 /**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
#define UBX_CFG_MSG_PAYLOAD_RATE1_05HZ 10
#define UBX_MAX_PAYLOAD_LENGTH 500
// ************
/** the structures of the binary packets */
/*** u-blox protocol binary message and payload definitions ***/
#pragma pack(push, 1)
struct ubx_header {
uint8_t sync1;
uint8_t sync2;
uint8_t msg_class;
uint8_t msg_id;
uint16_t length;
};
/* General: Header */
typedef struct {
uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */
int32_t lon; /**< Longitude * 1e-7, deg */
int32_t lat; /**< Latitude * 1e-7, deg */
int32_t height; /**< Height above Ellipsoid, mm */
int32_t height_msl; /**< Height above mean sea level, mm */
uint32_t hAcc; /**< Horizontal Accuracy Estimate, mm */
uint32_t vAcc; /**< Vertical Accuracy Estimate, mm */
uint8_t ck_a;
uint8_t ck_b;
} gps_bin_nav_posllh_packet_t;
uint8_t sync1;
uint8_t sync2;
uint16_t msg;
uint16_t length;
} ubx_header_t;
/* General: Checksum */
typedef struct {
uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */
int32_t time_nanoseconds; /**< Fractional Nanoseconds remainder of rounded ms above, range -500000 .. 500000 */
int16_t week; /**< GPS week (GPS time) */
uint8_t gpsFix; /**< GPS Fix: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GPS + dead reckoning, 5 = time only fix */
uint8_t flags;
int32_t ecefX;
int32_t ecefY;
int32_t ecefZ;
uint32_t pAcc;
int32_t ecefVX;
int32_t ecefVY;
int32_t ecefVZ;
uint32_t sAcc;
uint16_t pDOP;
uint8_t reserved1;
uint8_t numSV;
uint32_t reserved2;
uint8_t ck_a;
uint8_t ck_b;
} gps_bin_nav_sol_packet_t;
uint8_t ck_a;
uint8_t ck_b;
} ubx_checksum_t ;
/* Rx NAV-POSLLH */
typedef struct {
uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */
uint32_t time_accuracy; /**< Time Accuracy Estimate, ns */
int32_t time_nanoseconds; /**< Nanoseconds of second, range -1e9 .. 1e9 (UTC) */
uint16_t year; /**< Year, range 1999..2099 (UTC) */
uint8_t month; /**< Month, range 1..12 (UTC) */
uint8_t day; /**< Day of Month, range 1..31 (UTC) */
uint8_t hour; /**< Hour of Day, range 0..23 (UTC) */
uint8_t min; /**< Minute of Hour, range 0..59 (UTC) */
uint8_t sec; /**< Seconds of Minute, range 0..59 (UTC) */
uint8_t valid_flag; /**< Validity Flags (see ubx documentation) */
uint8_t ck_a;
uint8_t ck_b;
} gps_bin_nav_timeutc_packet_t;
//typedef struct {
// uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */
// uint16_t gDOP; /**< Geometric DOP (scaling 0.01) */
// uint16_t pDOP; /**< Position DOP (scaling 0.01) */
// uint16_t tDOP; /**< Time DOP (scaling 0.01) */
// uint16_t vDOP; /**< Vertical DOP (scaling 0.01) */
// uint16_t hDOP; /**< Horizontal DOP (scaling 0.01) */
// uint16_t nDOP; /**< Northing DOP (scaling 0.01) */
// uint16_t eDOP; /**< Easting DOP (scaling 0.01) */
// uint8_t ck_a;
// uint8_t ck_b;
//} gps_bin_nav_dop_packet_t;
uint32_t iTOW; /**< GPS Time of Week [ms] */
int32_t lon; /**< Longitude [1e-7 deg] */
int32_t lat; /**< Latitude [1e-7 deg] */
int32_t height; /**< Height above ellipsoid [mm] */
int32_t hMSL; /**< Height above mean sea level [mm] */
uint32_t hAcc; /**< Horizontal accuracy estimate [mm] */
uint32_t vAcc; /**< Vertical accuracy estimate [mm] */
} ubx_payload_rx_nav_posllh_t;
/* Rx NAV-SOL */
typedef struct {
uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */
uint8_t numCh; /**< Number of channels */
uint8_t globalFlags;
uint16_t reserved2;
} gps_bin_nav_svinfo_part1_packet_t;
uint32_t iTOW; /**< GPS Time of Week [ms] */
int32_t fTOW; /**< Fractional part of iTOW (range: +/-500000) [ns] */
int16_t week; /**< GPS week */
uint8_t gpsFix; /**< GPSfix type: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GPS + dead reckoning, 5 = time only fix */
uint8_t flags;
int32_t ecefX;
int32_t ecefY;
int32_t ecefZ;
uint32_t pAcc;
int32_t ecefVX;
int32_t ecefVY;
int32_t ecefVZ;
uint32_t sAcc;
uint16_t pDOP;
uint8_t reserved1;
uint8_t numSV; /**< Number of SVs used in Nav Solution */
uint32_t reserved2;
} ubx_payload_rx_nav_sol_t;
/* Rx NAV-PVT */
typedef struct {
uint8_t chn; /**< Channel number, 255 for SVs not assigned to a channel */
uint8_t svid; /**< Satellite ID */
uint8_t flags;
uint8_t quality;
uint8_t cno; /**< Carrier to Noise Ratio (Signal Strength), dbHz */
int8_t elev; /**< Elevation in integer degrees */
int16_t azim; /**< Azimuth in integer degrees */
int32_t prRes; /**< Pseudo range residual in centimetres */
} gps_bin_nav_svinfo_part2_packet_t;
uint32_t iTOW; /**< GPS Time of Week [ms] */
uint16_t year; /**< Year (UTC)*/
uint8_t month; /**< Month, range 1..12 (UTC) */
uint8_t day; /**< Day of month, range 1..31 (UTC) */
uint8_t hour; /**< Hour of day, range 0..23 (UTC) */
uint8_t min; /**< Minute of hour, range 0..59 (UTC) */
uint8_t sec; /**< Seconds of minute, range 0..60 (UTC) */
uint8_t valid; /**< Validity flags (see UBX_RX_NAV_PVT_VALID_...) */
uint32_t tAcc; /**< Time accuracy estimate (UTC) [ns] */
int32_t nano; /**< Fraction of second (UTC) [-1e9...1e9 ns] */
uint8_t fixType; /**< GNSSfix type: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GNSS + dead reckoning, 5 = time only fix */
uint8_t flags; /**< Fix Status Flags (see UBX_RX_NAV_PVT_FLAGS_...) */
uint8_t reserved1;
uint8_t numSV; /**< Number of SVs used in Nav Solution */
int32_t lon; /**< Longitude [1e-7 deg] */
int32_t lat; /**< Latitude [1e-7 deg] */
int32_t height; /**< Height above ellipsoid [mm] */
int32_t hMSL; /**< Height above mean sea level [mm] */
uint32_t hAcc; /**< Horizontal accuracy estimate [mm] */
uint32_t vAcc; /**< Vertical accuracy estimate [mm] */
int32_t velN; /**< NED north velocity [mm/s]*/
int32_t velE; /**< NED east velocity [mm/s]*/
int32_t velD; /**< NED down velocity [mm/s]*/
int32_t gSpeed; /**< Ground Speed (2-D) [mm/s] */
int32_t headMot; /**< Heading of motion (2-D) [1e-5 deg] */
uint32_t sAcc; /**< Speed accuracy estimate [mm/s] */
uint32_t headAcc; /**< Heading accuracy estimate (motion and vehicle) [1e-5 deg] */
uint16_t pDOP; /**< Position DOP [0.01] */
uint16_t reserved2;
uint32_t reserved3;
int32_t headVeh; /**< Heading of vehicle (2-D) [1e-5 deg] */
uint32_t reserved4;
} ubx_payload_rx_nav_pvt_t;
/* Rx NAV-TIMEUTC */
typedef struct {
uint8_t ck_a;
uint8_t ck_b;
} gps_bin_nav_svinfo_part3_packet_t;
uint32_t iTOW; /**< GPS Time of Week [ms] */
uint32_t tAcc; /**< Time accuracy estimate (UTC) [ns] */
int32_t nano; /**< Fraction of second, range -1e9 .. 1e9 (UTC) [ns] */
uint16_t year; /**< Year, range 1999..2099 (UTC) */
uint8_t month; /**< Month, range 1..12 (UTC) */
uint8_t day; /**< Day of month, range 1..31 (UTC) */
uint8_t hour; /**< Hour of day, range 0..23 (UTC) */
uint8_t min; /**< Minute of hour, range 0..59 (UTC) */
uint8_t sec; /**< Seconds of minute, range 0..60 (UTC) */
uint8_t valid; /**< Validity Flags (see ubx documentation) */
} ubx_payload_rx_nav_timeutc_t;
/* Rx NAV-SVINFO Part 1 */
typedef struct {
uint32_t time_milliseconds; // GPS Millisecond Time of Week
int32_t velN; //NED north velocity, cm/s
int32_t velE; //NED east velocity, cm/s
int32_t velD; //NED down velocity, cm/s
uint32_t speed; //Speed (3-D), cm/s
uint32_t gSpeed; //Ground Speed (2-D), cm/s
int32_t heading; //Heading of motion 2-D, deg, scaling: 1e-5
uint32_t sAcc; //Speed Accuracy Estimate, cm/s
uint32_t cAcc; //Course / Heading Accuracy Estimate, scaling: 1e-5
uint8_t ck_a;
uint8_t ck_b;
} gps_bin_nav_velned_packet_t;
struct gps_bin_mon_hw_packet {
uint32_t pinSel;
uint32_t pinBank;
uint32_t pinDir;
uint32_t pinVal;
uint16_t noisePerMS;
uint16_t agcCnt;
uint8_t aStatus;
uint8_t aPower;
uint8_t flags;
uint8_t __reserved1;
uint32_t usedMask;
uint8_t VP[25];
uint8_t jamInd;
uint16_t __reserved3;
uint32_t pinIrq;
uint32_t pulLH;
uint32_t pullL;
};
//typedef struct {
// int32_t time_milliseconds; /**< Measurement integer millisecond GPS time of week */
// int16_t week; /**< Measurement GPS week number */
// uint8_t numVis; /**< Number of visible satellites */
//
// //... rest of package is not used in this implementation
//
//} gps_bin_rxm_svsi_packet_t;
uint32_t iTOW; /**< GPS Time of Week [ms] */
uint8_t numCh; /**< Number of channels */
uint8_t globalFlags;
uint16_t reserved2;
} ubx_payload_rx_nav_svinfo_part1_t;
/* Rx NAV-SVINFO Part 2 (repeated) */
typedef struct {
uint8_t clsID;
uint8_t msgID;
uint8_t ck_a;
uint8_t ck_b;
} gps_bin_ack_ack_packet_t;
uint8_t chn; /**< Channel number, 255 for SVs not assigned to a channel */
uint8_t svid; /**< Satellite ID */
uint8_t flags;
uint8_t quality;
uint8_t cno; /**< Carrier to Noise Ratio (Signal Strength) [dbHz] */
int8_t elev; /**< Elevation [deg] */
int16_t azim; /**< Azimuth [deg] */
int32_t prRes; /**< Pseudo range residual [cm] */
} ubx_payload_rx_nav_svinfo_part2_t;
/* Rx NAV-VELNED */
typedef struct {
uint8_t clsID;
uint8_t msgID;
uint8_t ck_a;
uint8_t ck_b;
} gps_bin_ack_nak_packet_t;
uint32_t iTOW; /**< GPS Time of Week [ms] */
int32_t velN; /**< North velocity component [cm/s]*/
int32_t velE; /**< East velocity component [cm/s]*/
int32_t velD; /**< Down velocity component [cm/s]*/
uint32_t speed; /**< Speed (3-D) [cm/s] */
uint32_t gSpeed; /**< Ground speed (2-D) [cm/s] */
int32_t heading; /**< Heading of motion 2-D [1e-5 deg] */
uint32_t sAcc; /**< Speed accuracy estimate [cm/s] */
uint32_t cAcc; /**< Course / Heading accuracy estimate [1e-5 deg] */
} ubx_payload_rx_nav_velned_t;
/* Rx MON-HW (ubx6) */
typedef struct {
uint8_t clsID;
uint8_t msgID;
uint16_t length;
uint8_t portID;
uint8_t res0;
uint16_t res1;
uint32_t mode;
uint32_t baudRate;
uint16_t inProtoMask;
uint16_t outProtoMask;
uint16_t flags;
uint16_t pad;
uint8_t ck_a;
uint8_t ck_b;
} type_gps_bin_cfg_prt_packet_t;
uint32_t pinSel;
uint32_t pinBank;
uint32_t pinDir;
uint32_t pinVal;
uint16_t noisePerMS;
uint16_t agcCnt;
uint8_t aStatus;
uint8_t aPower;
uint8_t flags;
uint8_t reserved1;
uint32_t usedMask;
uint8_t VP[25];
uint8_t jamInd;
uint16_t reserved3;
uint32_t pinIrq;
uint32_t pullH;
uint32_t pullL;
} ubx_payload_rx_mon_hw_ubx6_t;
/* Rx MON-HW (ubx7+) */
typedef struct {
uint8_t clsID;
uint8_t msgID;
uint16_t length;
uint16_t measRate;
uint16_t navRate;
uint16_t timeRef;
uint8_t ck_a;
uint8_t ck_b;
} type_gps_bin_cfg_rate_packet_t;
uint32_t pinSel;
uint32_t pinBank;
uint32_t pinDir;
uint32_t pinVal;
uint16_t noisePerMS;
uint16_t agcCnt;
uint8_t aStatus;
uint8_t aPower;
uint8_t flags;
uint8_t reserved1;
uint32_t usedMask;
uint8_t VP[17];
uint8_t jamInd;
uint16_t reserved3;
uint32_t pinIrq;
uint32_t pullH;
uint32_t pullL;
} ubx_payload_rx_mon_hw_ubx7_t;
/* Rx MON-VER Part 1 */
typedef struct {
uint8_t clsID;
uint8_t msgID;
uint16_t length;
uint16_t mask;
uint8_t dynModel;
uint8_t fixMode;
int32_t fixedAlt;
uint32_t fixedAltVar;
int8_t minElev;
uint8_t drLimit;
uint16_t pDop;
uint16_t tDop;
uint16_t pAcc;
uint16_t tAcc;
uint8_t staticHoldThresh;
uint8_t dgpsTimeOut;
uint32_t reserved2;
uint32_t reserved3;
uint32_t reserved4;
uint8_t ck_a;
uint8_t ck_b;
} type_gps_bin_cfg_nav5_packet_t;
uint8_t swVersion[30];
uint8_t hwVersion[10];
} ubx_payload_rx_mon_ver_part1_t;
/* Rx MON-VER Part 2 (repeated) */
typedef struct {
uint8_t clsID;
uint8_t msgID;
uint16_t length;
uint8_t msgClass_payload;
uint8_t msgID_payload;
uint8_t extension[30];
} ubx_payload_rx_mon_ver_part2_t;
/* Rx ACK-ACK */
typedef union {
uint16_t msg;
struct {
uint8_t clsID;
uint8_t msgID;
};
} ubx_payload_rx_ack_ack_t;
/* Rx ACK-NAK */
typedef union {
uint16_t msg;
struct {
uint8_t clsID;
uint8_t msgID;
};
} ubx_payload_rx_ack_nak_t;
/* Tx CFG-PRT */
typedef struct {
uint8_t portID;
uint8_t reserved0;
uint16_t txReady;
uint32_t mode;
uint32_t baudRate;
uint16_t inProtoMask;
uint16_t outProtoMask;
uint16_t flags;
uint16_t reserved5;
} ubx_payload_tx_cfg_prt_t;
/* Tx CFG-RATE */
typedef struct {
uint16_t measRate; /**< Measurement Rate, GPS measurements are taken every measRate milliseconds */
uint16_t navRate; /**< Navigation Rate, in number of measurement cycles. This parameter cannot be changed, and must be set to 1 */
uint16_t timeRef; /**< Alignment to reference time: 0 = UTC time, 1 = GPS time */
} ubx_payload_tx_cfg_rate_t;
/* Tx CFG-NAV5 */
typedef struct {
uint16_t mask;
uint8_t dynModel; /**< Dynamic Platform model: 0 Portable, 2 Stationary, 3 Pedestrian, 4 Automotive, 5 Sea, 6 Airborne <1g, 7 Airborne <2g, 8 Airborne <4g */
uint8_t fixMode; /**< Position Fixing Mode: 1 2D only, 2 3D only, 3 Auto 2D/3D */
int32_t fixedAlt;
uint32_t fixedAltVar;
int8_t minElev;
uint8_t drLimit;
uint16_t pDop;
uint16_t tDop;
uint16_t pAcc;
uint16_t tAcc;
uint8_t staticHoldThresh;
uint8_t dgpsTimeOut;
uint8_t cnoThreshNumSVs; /**< (ubx7+ only, else 0) */
uint8_t cnoThresh; /**< (ubx7+ only, else 0) */
uint16_t reserved;
uint16_t staticHoldMaxDist; /**< (ubx8+ only, else 0) */
uint8_t utcStandard; /**< (ubx8+ only, else 0) */
uint8_t reserved3;
uint32_t reserved4;
} ubx_payload_tx_cfg_nav5_t;
/* Tx CFG-MSG */
typedef struct {
union {
uint16_t msg;
struct {
uint8_t msgClass;
uint8_t msgID;
};
};
uint8_t rate;
uint8_t ck_a;
uint8_t ck_b;
} type_gps_bin_cfg_msg_packet_t;
} ubx_payload_tx_cfg_msg_t;
struct ubx_cfg_msg_rate {
uint8_t msg_class;
uint8_t msg_id;
uint8_t rate;
};
/* General message and payload buffer union */
typedef union {
ubx_payload_rx_nav_posllh_t payload_rx_nav_posllh;
ubx_payload_rx_nav_sol_t payload_rx_nav_sol;
ubx_payload_rx_nav_timeutc_t payload_rx_nav_timeutc;
ubx_payload_rx_nav_svinfo_part1_t payload_rx_nav_svinfo_part1;
ubx_payload_rx_nav_svinfo_part2_t payload_rx_nav_svinfo_part2;
ubx_payload_rx_nav_velned_t payload_rx_nav_velned;
ubx_payload_rx_mon_hw_ubx6_t payload_rx_mon_hw_ubx6;
ubx_payload_rx_mon_hw_ubx7_t payload_rx_mon_hw_ubx7;
ubx_payload_rx_mon_ver_part1_t payload_rx_mon_ver_part1;
ubx_payload_rx_mon_ver_part2_t payload_rx_mon_ver_part2;
ubx_payload_rx_ack_ack_t payload_rx_ack_ack;
ubx_payload_rx_ack_nak_t payload_rx_ack_nak;
ubx_payload_tx_cfg_prt_t payload_tx_cfg_prt;
ubx_payload_tx_cfg_rate_t payload_tx_cfg_rate;
ubx_payload_tx_cfg_nav5_t payload_tx_cfg_nav5;
ubx_payload_tx_cfg_msg_t payload_tx_cfg_msg;
uint8_t raw[];
} ubx_buf_t;
#pragma pack(pop)
/*** END OF u-blox protocol binary message and payload definitions ***/
// END the structures of the binary packets
// ************
/* Decoder state */
typedef enum {
UBX_DECODE_UNINIT = 0,
UBX_DECODE_GOT_SYNC1,
UBX_DECODE_GOT_SYNC2,
UBX_DECODE_GOT_CLASS,
UBX_DECODE_GOT_MESSAGEID,
UBX_DECODE_GOT_LENGTH1,
UBX_DECODE_GOT_LENGTH2
UBX_DECODE_SYNC1 = 0,
UBX_DECODE_SYNC2,
UBX_DECODE_CLASS,
UBX_DECODE_ID,
UBX_DECODE_LENGTH1,
UBX_DECODE_LENGTH2,
UBX_DECODE_PAYLOAD,
UBX_DECODE_CHKSUM1,
UBX_DECODE_CHKSUM2
} ubx_decode_state_t;
//typedef type_gps_bin_ubx_state gps_bin_ubx_state_t;
#pragma pack(pop)
/* Rx message state */
typedef enum {
UBX_RXMSG_IGNORE = 0,
UBX_RXMSG_HANDLE,
UBX_RXMSG_DISABLE,
UBX_RXMSG_ERROR_LENGTH
} ubx_rxmsg_state_t;
/* ACK state */
typedef enum {
UBX_ACK_IDLE = 0,
UBX_ACK_WAITING,
UBX_ACK_GOT_ACK,
UBX_ACK_GOT_NAK
} ubx_ack_state_t;
#define RECV_BUFFER_SIZE 300 //The NAV-SOL messages really need such a big buffer
class UBX : public GPS_Helper
{
public:
UBX(const int &fd, struct vehicle_gps_position_s *gps_position);
UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info);
~UBX();
int receive(unsigned timeout);
int receive(const unsigned timeout);
int configure(unsigned &baudrate);
private:
/**
* Parse the binary MTK packet
* Parse the binary UBX packet
*/
int parse_char(uint8_t b);
int parse_char(const uint8_t b);
/**
* Handle the package once it has arrived
* Start payload rx
*/
int handle_message(void);
int payload_rx_init(void);
/**
* Add payload rx byte
*/
int payload_rx_add(const uint8_t b);
int payload_rx_add_nav_svinfo(const uint8_t b);
int payload_rx_add_mon_ver(const uint8_t b);
/**
* Finish payload rx
*/
int payload_rx_done(void);
/**
* Reset the parse state machine for a fresh start
@ -373,44 +480,52 @@ private:
/**
* While parsing add every byte (except the sync bytes) to the checksum
*/
void add_byte_to_checksum(uint8_t);
void add_byte_to_checksum(const uint8_t);
/**
* Add the two checksum bytes to an outgoing message
* Send a message
*/
void add_checksum_to_message(uint8_t *message, const unsigned length);
void send_message(const uint16_t msg, const uint8_t *payload, const uint16_t length);
/**
* Helper to send a config packet
* Configure message rate
*/
void send_config_packet(const int &fd, uint8_t *packet, const unsigned length);
void configure_message_rate(const uint16_t msg, const uint8_t rate);
void configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate);
/**
* Calculate & add checksum for given buffer
*/
void calc_checksum(const uint8_t *buffer, const uint16_t length, ubx_checksum_t *checksum);
void send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size);
/**
* Wait for message acknowledge
*/
int wait_for_ack(const uint16_t msg, const unsigned timeout, const bool report);
void add_checksum(uint8_t *message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b);
int wait_for_ack(unsigned timeout);
/**
* Calculate FNV1 hash
*/
uint32_t fnv1_32_str(uint8_t *str, uint32_t hval);
int _fd;
struct vehicle_gps_position_s *_gps_position;
struct satellite_info_s *_satellite_info;
bool _enable_sat_info;
bool _configured;
bool _waiting_for_ack;
ubx_ack_state_t _ack_state;
bool _got_posllh;
bool _got_velned;
bool _got_timeutc;
uint8_t _message_class_needed;
uint8_t _message_id_needed;
ubx_decode_state_t _decode_state;
uint8_t _rx_buffer[RECV_BUFFER_SIZE];
unsigned _rx_count;
uint16_t _rx_msg;
ubx_rxmsg_state_t _rx_state;
uint16_t _rx_payload_length;
uint16_t _rx_payload_index;
uint8_t _rx_ck_a;
uint8_t _rx_ck_b;
uint8_t _message_class;
uint8_t _message_id;
unsigned _payload_size;
hrt_abstime _disable_cmd_last;
uint16_t _ack_waiting_msg;
ubx_buf_t _buf;
uint32_t _ubx_version;
};
#endif /* UBX_H_ */

2
src/drivers/hott/messages.cpp
View File

@ -226,7 +226,7 @@ build_gps_response(uint8_t *buffer, size_t *size)
msg.sensor_id = GPS_SENSOR_ID;
msg.sensor_text_id = GPS_SENSOR_TEXT_ID;
msg.gps_num_sat = gps.satellites_visible;
msg.gps_num_sat = gps.satellites_used;
/* The GPS fix type: 0 = none, 2 = 2D, 3 = 3D */
msg.gps_fix_char = (uint8_t)(gps.fix_type + 48);

2
src/lib/external_lgpl/tecs/tecs.cpp
View File

@ -310,7 +310,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
STEdot_dem = STEdot_dem + _rollComp * (1.0f / constrain(cosPhi , 0.1f, 1.0f) - 1.0f);
if (STEdot_dem >= 0) {
ff_throttle = nomThr + STEdot_dem / _STEdot_max * (1.0f - nomThr);
ff_throttle = nomThr + STEdot_dem / _STEdot_max * (_THRmaxf - nomThr);
} else {
ff_throttle = nomThr - STEdot_dem / _STEdot_min * nomThr;

4
src/modules/commander/state_machine_helper.cpp
View File

@ -182,7 +182,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current
if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f) &&
(status->avionics_power_rail_voltage < 4.9f)) {
mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", status->avionics_power_rail_voltage);
mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage);
valid_transition = false;
}
}
@ -638,7 +638,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
}
if (!status->is_rotary_wing) {
int fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY);
fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED SENSOR MISSING");

2
src/modules/mavlink/mavlink_messages.cpp
View File

@ -677,7 +677,7 @@ protected:
cm_uint16_from_m_float(gps.epv),
gps.vel_m_s * 100.0f,
_wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f,
gps.satellites_visible);
gps.satellites_used);
}
}
};

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

@ -751,9 +751,8 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
hil_gps.vel_ned_valid = true;
hil_gps.cog_rad = _wrap_pi(gps.cog * M_DEG_TO_RAD_F * 1e-2f);
hil_gps.timestamp_satellites = timestamp;
hil_gps.fix_type = gps.fix_type;
hil_gps.satellites_visible = gps.satellites_visible;
hil_gps.satellites_used = gps.satellites_visible; //TODO: rename mavlink_hil_gps_t sats visible to used?
if (_gps_pub < 0) {
_gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps);

7
src/modules/position_estimator_inav/position_estimator_inav_main.c
View File

@ -915,6 +915,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
memcpy(x_est_prev, x_est, sizeof(x_est));
memcpy(y_est_prev, y_est, sizeof(y_est));
}
} else {
/* gradually reset xy velocity estimates */
inertial_filter_correct(-x_est[1], dt, x_est, 1, params.w_xy_res_v);
inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_res_v);
}
/* detect land */
@ -930,6 +934,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
landed = false;
landed_time = 0;
}
/* reset xy velocity estimates when landed */
x_est[1] = 0.0f;
y_est[1] = 0.0f;
} else {
if (alt_disp2 < land_disp2 && thrust < params.land_thr) {

3
src/modules/position_estimator_inav/position_estimator_inav_params.c
View File

@ -46,6 +46,7 @@ PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f);
PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f);
PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f);
PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f);
PARAM_DEFINE_FLOAT(INAV_W_XY_RES_V, 0.5f);
PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f);
PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.05f);
PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.15f);
@ -65,6 +66,7 @@ int parameters_init(struct position_estimator_inav_param_handles *h)
h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P");
h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V");
h->w_xy_flow = param_find("INAV_W_XY_FLOW");
h->w_xy_res_v = param_find("INAV_W_XY_RES_V");
h->w_gps_flow = param_find("INAV_W_GPS_FLOW");
h->w_acc_bias = param_find("INAV_W_ACC_BIAS");
h->flow_k = param_find("INAV_FLOW_K");
@ -87,6 +89,7 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str
param_get(h->w_xy_gps_p, &(p->w_xy_gps_p));
param_get(h->w_xy_gps_v, &(p->w_xy_gps_v));
param_get(h->w_xy_flow, &(p->w_xy_flow));
param_get(h->w_xy_res_v, &(p->w_xy_res_v));
param_get(h->w_gps_flow, &(p->w_gps_flow));
param_get(h->w_acc_bias, &(p->w_acc_bias));
param_get(h->flow_k, &(p->flow_k));

2
src/modules/position_estimator_inav/position_estimator_inav_params.h
View File

@ -47,6 +47,7 @@ struct position_estimator_inav_params {
float w_xy_gps_p;
float w_xy_gps_v;
float w_xy_flow;
float w_xy_res_v;
float w_gps_flow;
float w_acc_bias;
float flow_k;
@ -66,6 +67,7 @@ struct position_estimator_inav_param_handles {
param_t w_xy_gps_p;
param_t w_xy_gps_v;
param_t w_xy_flow;
param_t w_xy_res_v;
param_t w_gps_flow;
param_t w_acc_bias;
param_t flow_k;

50
src/modules/sdlog2/sdlog2.c
View File

@ -74,6 +74,7 @@
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/satellite_info.h>
#include <uORB/topics/vehicle_vicon_position.h>
#include <uORB/topics/vehicle_global_velocity_setpoint.h>
#include <uORB/topics/optical_flow.h>
@ -141,6 +142,8 @@ PARAM_DEFINE_INT32(SDLOG_EXT, -1);
fds[fdsc_count].events = POLLIN; \
fdsc_count++;
#define MIN(X,Y) ((X) < (Y) ? (X) : (Y))
static bool main_thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
@ -944,6 +947,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct tecs_status_s tecs_status;
struct system_power_s system_power;
struct servorail_status_s servorail_status;
struct satellite_info_s sat_info;
struct wind_estimate_s wind_estimate;
} buf;
@ -1007,6 +1011,7 @@ int sdlog2_thread_main(int argc, char *argv[])
int global_pos_sub;
int triplet_sub;
int gps_pos_sub;
int sat_info_sub;
int vicon_pos_sub;
int flow_sub;
int rc_sub;
@ -1026,6 +1031,7 @@ int sdlog2_thread_main(int argc, char *argv[])
subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
subs.status_sub = orb_subscribe(ORB_ID(vehicle_status));
subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
subs.sat_info_sub = orb_subscribe(ORB_ID(satellite_info));
subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
subs.att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
@ -1066,11 +1072,14 @@ int sdlog2_thread_main(int argc, char *argv[])
hrt_abstime barometer_timestamp = 0;
hrt_abstime differential_pressure_timestamp = 0;
/* initialize calculated mean SNR */
float snr_mean = 0.0f;
/* enable logging on start if needed */
if (log_on_start) {
/* check GPS topic to get GPS time */
if (log_name_timestamp) {
if (copy_if_updated(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos)) {
if (!orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos)) {
gps_time = buf_gps_pos.time_gps_usec;
}
}
@ -1128,14 +1137,6 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- GPS POSITION - UNIT #1 --- */
if (gps_pos_updated) {
float snr_mean = 0.0f;
for (unsigned i = 0; i < buf_gps_pos.satellites_visible; i++) {
snr_mean += buf_gps_pos.satellite_snr[i];
}
snr_mean /= buf_gps_pos.satellites_visible;
log_msg.msg_type = LOG_GPS_MSG;
log_msg.body.log_GPS.gps_time = buf_gps_pos.time_gps_usec;
log_msg.body.log_GPS.fix_type = buf_gps_pos.fix_type;
@ -1148,44 +1149,55 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_GPS.vel_e = buf_gps_pos.vel_e_m_s;
log_msg.body.log_GPS.vel_d = buf_gps_pos.vel_d_m_s;
log_msg.body.log_GPS.cog = buf_gps_pos.cog_rad;
log_msg.body.log_GPS.sats = buf_gps_pos.satellites_visible;
log_msg.body.log_GPS.sats = buf_gps_pos.satellites_used;
log_msg.body.log_GPS.snr_mean = snr_mean;
log_msg.body.log_GPS.noise_per_ms = buf_gps_pos.noise_per_ms;
log_msg.body.log_GPS.jamming_indicator = buf_gps_pos.jamming_indicator;
LOGBUFFER_WRITE_AND_COUNT(GPS);
}
/* --- SATELLITE INFO - UNIT #1 --- */
if (_extended_logging) {
if (copy_if_updated(ORB_ID(satellite_info), subs.sat_info_sub, &buf.sat_info)) {
if (_extended_logging) {
/* log the SNR of each satellite for a detailed view of signal quality */
unsigned gps_msg_max_snr = sizeof(buf_gps_pos.satellite_snr) / sizeof(buf_gps_pos.satellite_snr[0]);
unsigned sat_info_count = MIN(buf.sat_info.count, sizeof(buf.sat_info.snr) / sizeof(buf.sat_info.snr[0]));
unsigned log_max_snr = sizeof(log_msg.body.log_GS0A.satellite_snr) / sizeof(log_msg.body.log_GS0A.satellite_snr[0]);
log_msg.msg_type = LOG_GS0A_MSG;
memset(&log_msg.body.log_GS0A, 0, sizeof(log_msg.body.log_GS0A));
/* fill set A */
for (unsigned i = 0; i < gps_msg_max_snr; i++) {
snr_mean = 0.0f;
int satindex = buf_gps_pos.satellite_prn[i] - 1;
/* fill set A and calculate mean SNR */
for (unsigned i = 0; i < sat_info_count; i++) {
snr_mean += buf.sat_info.snr[i];
int satindex = buf.sat_info.svid[i] - 1;
/* handles index exceeding and wraps to to arithmetic errors */
if ((satindex >= 0) && (satindex < (int)log_max_snr)) {
/* map satellites by their ID so that logs from two receivers can be compared */
log_msg.body.log_GS0A.satellite_snr[satindex] = buf_gps_pos.satellite_snr[i];
log_msg.body.log_GS0A.satellite_snr[satindex] = buf.sat_info.snr[i];
}
}
LOGBUFFER_WRITE_AND_COUNT(GS0A);
snr_mean /= sat_info_count;
log_msg.msg_type = LOG_GS0B_MSG;
memset(&log_msg.body.log_GS0B, 0, sizeof(log_msg.body.log_GS0B));
/* fill set B */
for (unsigned i = 0; i < gps_msg_max_snr; i++) {
for (unsigned i = 0; i < sat_info_count; i++) {
/* get second bank of satellites, thus deduct bank size from index */
int satindex = buf_gps_pos.satellite_prn[i] - 1 - log_max_snr;
int satindex = buf.sat_info.svid[i] - 1 - log_max_snr;
/* handles index exceeding and wraps to to arithmetic errors */
if ((satindex >= 0) && (satindex < (int)log_max_snr)) {
/* map satellites by their ID so that logs from two receivers can be compared */
log_msg.body.log_GS0B.satellite_snr[satindex] = buf_gps_pos.satellite_snr[i];
log_msg.body.log_GS0B.satellite_snr[satindex] = buf.sat_info.snr[i];
}
}
LOGBUFFER_WRITE_AND_COUNT(GS0B);

2
src/modules/systemlib/circuit_breaker.c
View File

@ -86,7 +86,7 @@ PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 0);
bool circuit_breaker_enabled(const char* breaker, int32_t magic)
{
int32_t val;
(void)param_get(param_find(breaker), val);
(void)param_get(param_find(breaker), &val);
return (val == magic);
}

9
src/modules/systemlib/circuit_breaker.h
View File

@ -40,6 +40,15 @@
#ifndef CIRCUIT_BREAKER_H_
#define CIRCUIT_BREAKER_H_
/* SAFETY WARNING -- SAFETY WARNING -- SAFETY WARNING
*
* OBEY THE DOCUMENTATION FOR ALL CIRCUIT BREAKERS HERE,
* ENSURE TO READ CAREFULLY ALL SAFETY WARNINGS.
* http://pixhawk.org/dev/circuit_breakers
*
* CIRCUIT BREAKERS ARE NOT PART OF THE STANDARD OPERATION PROCEDURE
* AND MAY DISABLE CHECKS THAT ARE VITAL FOR SAFE FLIGHT.
*/
#define CBRK_SUPPLY_CHK_KEY 894281
#define CBRK_RATE_CTRL_KEY 140253
#define CBRK_IO_SAFETY_KEY 22027

2
src/modules/uORB/Subscription.cpp
View File

@ -40,6 +40,7 @@
#include "topics/parameter_update.h"
#include "topics/actuator_controls.h"
#include "topics/vehicle_gps_position.h"
#include "topics/satellite_info.h"
#include "topics/sensor_combined.h"
#include "topics/vehicle_attitude.h"
#include "topics/vehicle_global_position.h"
@ -88,6 +89,7 @@ T Subscription<T>::getData() {
template class __EXPORT Subscription<parameter_update_s>;
template class __EXPORT Subscription<actuator_controls_s>;
template class __EXPORT Subscription<vehicle_gps_position_s>;
template class __EXPORT Subscription<satellite_info_s>;
template class __EXPORT Subscription<sensor_combined_s>;
template class __EXPORT Subscription<vehicle_attitude_s>;
template class __EXPORT Subscription<vehicle_global_position_s>;

3
src/modules/uORB/objects_common.cpp
View File

@ -75,6 +75,9 @@ ORB_DEFINE(sensor_combined, struct sensor_combined_s);
#include "topics/vehicle_gps_position.h"
ORB_DEFINE(vehicle_gps_position, struct vehicle_gps_position_s);
#include "topics/satellite_info.h"
ORB_DEFINE(satellite_info, struct satellite_info_s);
#include "topics/home_position.h"
ORB_DEFINE(home_position, struct home_position_s);

89
src/modules/uORB/topics/satellite_info.h Normal file
View File

@ -0,0 +1,89 @@
/****************************************************************************
*
* Copyright (C) 2014 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file satellite_info.h
* Definition of the GNSS satellite info uORB topic.
*/
#ifndef TOPIC_SAT_INFO_H_
#define TOPIC_SAT_INFO_H_
#include <stdint.h>
#include "../uORB.h"
/**
* @addtogroup topics
* @{
*/
/**
* GNSS Satellite Info.
*/
#define SAT_INFO_MAX_SATELLITES 20
struct satellite_info_s {
uint64_t timestamp; /**< Timestamp of satellite info */
uint8_t count; /**< Number of satellites in satellite info */
uint8_t svid[SAT_INFO_MAX_SATELLITES]; /**< Space vehicle ID [1..255], see scheme below */
uint8_t used[SAT_INFO_MAX_SATELLITES]; /**< 0: Satellite not used, 1: used for navigation */
uint8_t elevation[SAT_INFO_MAX_SATELLITES]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */
uint8_t azimuth[SAT_INFO_MAX_SATELLITES]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */
uint8_t snr[SAT_INFO_MAX_SATELLITES]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99, zero when not tracking this satellite. */
};
/**
* NAV_SVINFO space vehicle ID (svid) scheme according to u-blox protocol specs
* u-bloxM8-V15_ReceiverDescriptionProtocolSpec_Public_(UBX-13003221).pdf
*
* GPS 1-32
* SBAS 120-158
* Galileo 211-246
* BeiDou 159-163, 33-64
* QZSS 193-197
* GLONASS 65-96, 255
*
*/
/**
* @}
*/
/* register this as object request broker structure */
ORB_DECLARE(satellite_info);
#endif

9
src/modules/uORB/topics/vehicle_gps_position.h
View File

@ -82,14 +82,7 @@ struct vehicle_gps_position_s {
uint64_t timestamp_time; /**< Timestamp for time information */
uint64_t time_gps_usec; /**< Timestamp (microseconds in GPS format), this is the timestamp which comes from the gps module */
uint64_t timestamp_satellites; /**< Timestamp for sattelite information */
uint8_t satellites_visible; /**< Number of satellites visible. If unknown, set to 255 */
uint8_t satellite_prn[20]; /**< Global satellite ID */
uint8_t satellite_used[20]; /**< 0: Satellite not used, 1: used for localization */
uint8_t satellite_elevation[20]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */
uint8_t satellite_azimuth[20]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */
uint8_t satellite_snr[20]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99, zero when not tracking this satellite. */
bool satellite_info_available; /**< 0 for no info, 1 for info available */
uint8_t satellites_used; /**< Number of satellites used */
};
/**

28
src/systemcmds/param/param.c
View File

@ -64,6 +64,7 @@ static void do_show_print(void *arg, param_t param);
static void do_set(const char* name, const char* val, bool fail_on_not_found);
static void do_compare(const char* name, const char* vals[], unsigned comparisons);
static void do_reset(void);
static void do_reset_nostart(void);
int
param_main(int argc, char *argv[])
@ -142,6 +143,10 @@ param_main(int argc, char *argv[])
if (!strcmp(argv[1], "reset")) {
do_reset();
}
if (!strcmp(argv[1], "reset_nostart")) {
do_reset_nostart();
}
}
errx(1, "expected a command, try 'load', 'import', 'show', 'set', 'compare', 'select' or 'save'");
@ -427,3 +432,26 @@ do_reset(void)
exit(0);
}
}
static void
do_reset_nostart(void)
{
int32_t autostart;
int32_t autoconfig;
(void)param_get(param_find("SYS_AUTOSTART"), &autostart);
(void)param_get(param_find("SYS_AUTOCONFIG"), &autoconfig);
param_reset_all();
(void)param_set(param_find("SYS_AUTOSTART"), &autostart);
(void)param_set(param_find("SYS_AUTOCONFIG"), &autoconfig);
if (param_save_default()) {
warnx("Param export failed.");
exit(1);
} else {
exit(0);
}
}