fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-31 04:31:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

U-blox driver rework, step 3

Browse Source 
More sat info isolation
Flush input after changing baudrate to allow driver restart w/o GNSS
module restart
This commit is contained in:
Kynos
2014-06-08 14:05:35 +02:00
parent 9bad828bc0
commit 9f754e0e9a
4 changed files with 126 additions and 118 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/gps/gps.cpp
+58 -40
View File
@@ -63,6 +63,7 @@
#include <drivers/drv_gps.h> #include <drivers/drv_gps.h>
#include <uORB/uORB.h> #include <uORB/uORB.h>
#include <uORB/topics/vehicle_gps_position.h> #include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/satellite_info.h>
#include <board_config.h> #include <board_config.h>
@@ -106,8 +107,10 @@ private:
bool _mode_changed; ///< flag that the GPS mode has changed bool _mode_changed; ///< flag that the GPS mode has changed
gps_driver_mode_t _mode; ///< current mode gps_driver_mode_t _mode; ///< current mode
GPS_Helper *_Helper; ///< instance of GPS parser GPS_Helper *_Helper; ///< instance of GPS parser
struct vehicle_gps_position_s _report; ///< uORB topic for gps position struct vehicle_gps_position_s _report_gps_pos; ///< uORB topic for gps position
orb_advert_t _report_pub; ///< uORB pub for gps position orb_advert_t _report_gps_pos_pub; ///< uORB pub for gps position
struct satellite_info_s _report_sat_info; ///< uORB topic for satellite info
orb_advert_t _report_sat_info_pub; ///< uORB pub for satellite info
float _rate; ///< position update rate float _rate; ///< position update rate
bool _fake_gps; ///< fake gps output bool _fake_gps; ///< fake gps output
@@ -161,7 +164,8 @@ GPS::GPS(const char *uart_path, bool fake_gps) :
_mode_changed(false), _mode_changed(false),
_mode(GPS_DRIVER_MODE_UBX), _mode(GPS_DRIVER_MODE_UBX),
_Helper(nullptr), _Helper(nullptr),
_report_pub(-1), _report_gps_pos_pub(-1),
_report_sat_info_pub(-1),
_rate(0.0f), _rate(0.0f),
_fake_gps(fake_gps) _fake_gps(fake_gps)
{ {
@@ -172,7 +176,8 @@ GPS::GPS(const char *uart_path, bool fake_gps) :
/* we need this potentially before it could be set in task_main */ /* we need this potentially before it could be set in task_main */
g_dev = this; g_dev = this;
memset(&_report, 0, sizeof(_report)); memset(&_report_gps_pos, 0, sizeof(_report_gps_pos));
memset(&_report_sat_info, 0, sizeof(_report_sat_info));
_debug_enabled = true; _debug_enabled = true;
} }
@@ -271,33 +276,33 @@ GPS::task_main()
if (_fake_gps) { if (_fake_gps) {
_report.timestamp_position = hrt_absolute_time(); _report_gps_pos.timestamp_position = hrt_absolute_time();
_report.lat = (int32_t)47.378301e7f; _report_gps_pos.lat = (int32_t)47.378301e7f;
_report.lon = (int32_t)8.538777e7f; _report_gps_pos.lon = (int32_t)8.538777e7f;
_report.alt = (int32_t)1200e3f; _report_gps_pos.alt = (int32_t)1200e3f;
_report.timestamp_variance = hrt_absolute_time(); _report_gps_pos.timestamp_variance = hrt_absolute_time();
_report.s_variance_m_s = 10.0f; _report_gps_pos.s_variance_m_s = 10.0f;
_report.p_variance_m = 10.0f; _report_gps_pos.p_variance_m = 10.0f;
_report.c_variance_rad = 0.1f; _report_gps_pos.c_variance_rad = 0.1f;
_report.fix_type = 3; _report_gps_pos.fix_type = 3;
_report.eph_m = 0.9f; _report_gps_pos.eph_m = 0.9f;
_report.epv_m = 1.8f; _report_gps_pos.epv_m = 1.8f;
_report.timestamp_velocity = hrt_absolute_time(); _report_gps_pos.timestamp_velocity = hrt_absolute_time();
_report.vel_n_m_s = 0.0f; _report_gps_pos.vel_n_m_s = 0.0f;
_report.vel_e_m_s = 0.0f; _report_gps_pos.vel_e_m_s = 0.0f;
_report.vel_d_m_s = 0.0f; _report_gps_pos.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_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.cog_rad = 0.0f; _report_gps_pos.cog_rad = 0.0f;
_report.vel_ned_valid = true; _report_gps_pos.vel_ned_valid = true;
//no time and satellite information simulated //no time and satellite information simulated
if (!(_pub_blocked)) { if (!(_pub_blocked)) {
if (_report_pub > 0) { if (_report_gps_pos_pub > 0) {
orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
} else { } 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 +318,11 @@ GPS::task_main()
switch (_mode) { switch (_mode) {
case GPS_DRIVER_MODE_UBX: case GPS_DRIVER_MODE_UBX:
_Helper = new UBX(_serial_fd, &_report); _Helper = new UBX(_serial_fd, &_report_gps_pos, &_report_sat_info, UBX_ENABLE_NAV_SVINFO);
break; break;
case GPS_DRIVER_MODE_MTK: case GPS_DRIVER_MODE_MTK:
_Helper = new MTK(_serial_fd, &_report); _Helper = new MTK(_serial_fd, &_report_gps_pos);
break; break;
default: default:
@@ -332,20 +337,33 @@ GPS::task_main()
// GPS is obviously detected successfully, reset statistics // GPS is obviously detected successfully, reset statistics
_Helper->reset_update_rates(); _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(); // lock();
/* opportunistic publishing - else invalid data would end up on the bus */ /* opportunistic publishing - else invalid data would end up on the bus */
if (!(_pub_blocked)) { if (!(_pub_blocked)) {
if (_report_pub > 0) { if (helper_ret & 1) {
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 { } 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);
}
}
if (helper_ret & 2) {
if (_report_sat_info_pub > 0) {
orb_publish(ORB_ID(satellite_info), _report_sat_info_pub, &_report_sat_info);
} else {
_report_sat_info_pub = orb_advertise(ORB_ID(satellite_info), &_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 */ /* measure update rate every 5 seconds */
if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) { if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) {
@@ -357,7 +375,7 @@ GPS::task_main()
} }
if (!_healthy) { if (!_healthy) {
char *mode_str = "unknown"; const char *mode_str = "unknown";
switch (_mode) { switch (_mode) {
case GPS_DRIVER_MODE_UBX: case GPS_DRIVER_MODE_UBX:
@@ -447,11 +465,11 @@ GPS::print_info()
warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK"); warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK");
if (_report.timestamp_position != 0) { if (_report_gps_pos.timestamp_position != 0) {
warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report.fix_type, warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report_gps_pos.fix_type,
_report.satellites_used, (double)(hrt_absolute_time() - _report.timestamp_position) / 1000.0f); _report_gps_pos.satellites_used, (double)(hrt_absolute_time() - _report_gps_pos.timestamp_position) / 1000.0);
warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt); 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.eph_m, (double)_report.epv_m); warnx("eph: %.2fm, epv: %.2fm", (double)_report_gps_pos.eph_m, (double)_report_gps_pos.epv_m);
warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate()); 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 velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate());
warnx("rate publication:\t%6.2f Hz", (double)_rate); warnx("rate publication:\t%6.2f Hz", (double)_rate);
@@ -578,7 +596,7 @@ gps_main(int argc, char *argv[])
{ {
/* set to default */ /* set to default */
char *device_name = GPS_DEFAULT_UART_PORT; const char *device_name = GPS_DEFAULT_UART_PORT;
bool fake_gps = false; bool fake_gps = false;
/* /*
src/drivers/gps/ubx.cpp
+53 -66
View File
@@ -65,9 +65,13 @@
#define UBX_WAIT_BEFORE_READ 20 // ms, wait before reading to save read() calls #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 #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) : #define MIN(X,Y) ((X) < (Y) ? (X) : (Y))
UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info, const bool &enable_sat_info) :
_fd(fd), _fd(fd),
_gps_position(gps_position), _gps_position(gps_position),
_satellite_info(satellite_info),
_enable_sat_info(enable_sat_info),
_configured(false), _configured(false),
_waiting_for_ack(false), _waiting_for_ack(false),
_disable_cmd_last(0) _disable_cmd_last(0)
@@ -84,14 +88,19 @@ UBX::configure(unsigned &baudrate)
{ {
_configured = false; _configured = false;
/* try different baudrates */ /* try different baudrates */
const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200}; const unsigned baudrates[] = {9600, 38400, 19200, 57600, 115200};
int baud_i; unsigned baud_i;
for (baud_i = 0; baud_i < 5; baud_i++) { for (baud_i = 0; baud_i < sizeof(baudrates) / sizeof(baudrates[0]); baud_i++) {
baudrate = baudrates_to_try[baud_i]; baudrate = baudrates[baud_i];
set_baudrate(_fd, baudrate); set_baudrate(_fd, baudrate);
/* flush input and wait for at least 20 ms silence */
decode_init();
wait_for_ack(20);
decode_init();
/* Send a CFG-PRT message to set the UBX protocol for in and out /* Send a CFG-PRT message to set the UBX protocol for in and out
* and leave the baudrate as it is, we just want an ACK-ACK from this * and leave the baudrate as it is, we just want an ACK-ACK from this
*/ */
@@ -107,7 +116,7 @@ UBX::configure(unsigned &baudrate)
cfg_prt_packet.msgID = UBX_MESSAGE_CFG_PRT; cfg_prt_packet.msgID = UBX_MESSAGE_CFG_PRT;
cfg_prt_packet.length = UBX_CFG_PRT_LENGTH; cfg_prt_packet.length = UBX_CFG_PRT_LENGTH;
cfg_prt_packet.portID = UBX_CFG_PRT_PAYLOAD_PORTID; cfg_prt_packet.portID = UBX_CFG_PRT_PAYLOAD_PORTID;
cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE; cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE;
cfg_prt_packet.baudRate = baudrate; cfg_prt_packet.baudRate = baudrate;
cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK;
cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK;
@@ -125,7 +134,7 @@ UBX::configure(unsigned &baudrate)
cfg_prt_packet.msgID = UBX_MESSAGE_CFG_PRT; cfg_prt_packet.msgID = UBX_MESSAGE_CFG_PRT;
cfg_prt_packet.length = UBX_CFG_PRT_LENGTH; cfg_prt_packet.length = UBX_CFG_PRT_LENGTH;
cfg_prt_packet.portID = UBX_CFG_PRT_PAYLOAD_PORTID; cfg_prt_packet.portID = UBX_CFG_PRT_PAYLOAD_PORTID;
cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE; cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE;
cfg_prt_packet.baudRate = UBX_CFG_PRT_PAYLOAD_BAUDRATE; cfg_prt_packet.baudRate = UBX_CFG_PRT_PAYLOAD_BAUDRATE;
cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK;
cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK;
@@ -144,7 +153,7 @@ UBX::configure(unsigned &baudrate)
break; break;
} }
if (baud_i >= 5) { if (baud_i >= sizeof(baudrates) / sizeof(baudrates[0])) {
return 1; return 1;
} }
@@ -220,14 +229,14 @@ UBX::configure(unsigned &baudrate)
return 1; return 1;
} }
#ifdef UBX_ENABLE_NAV_SVINFO if (_enable_sat_info) {
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5); configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
warnx("MSG CFG FAIL: NAV SVINFO"); warnx("MSG CFG FAIL: NAV SVINFO");
return 1; return 1;
}
} }
#endif
configure_message_rate(UBX_CLASS_MON, UBX_MESSAGE_MON_HW, 1); configure_message_rate(UBX_CLASS_MON, UBX_MESSAGE_MON_HW, 1);
@@ -244,14 +253,13 @@ int
UBX::wait_for_ack(unsigned timeout) UBX::wait_for_ack(unsigned timeout)
{ {
_waiting_for_ack = true; _waiting_for_ack = true;
uint64_t time_started = hrt_absolute_time(); uint64_t time_end = hrt_absolute_time() + timeout * 1000;
while (hrt_absolute_time() < time_started + timeout * 1000) { while ((timeout = (time_end - hrt_absolute_time()) / 1000) > 0) {
if (receive(timeout) > 0) { if (receive(timeout) > 0) {
if (!_waiting_for_ack) { if (!_waiting_for_ack) {
return 1; return 1;
} }
} else { } else {
return -1; // timeout or error receiving, or NAK return -1; // timeout or error receiving, or NAK
} }
@@ -275,7 +283,7 @@ UBX::receive(unsigned timeout)
ssize_t count = 0; ssize_t count = 0;
bool handled = false; int handled = 0;
while (true) { while (true) {
@@ -290,7 +298,7 @@ UBX::receive(unsigned timeout)
} else if (ret == 0) { } else if (ret == 0) {
/* return success after short delay after receiving a packet or timeout after long delay */ /* return success after short delay after receiving a packet or timeout after long delay */
if (handled) { if (handled) {
return 1; return handled;
} else { } else {
return -1; return -1;
@@ -311,8 +319,7 @@ UBX::receive(unsigned timeout)
/* pass received bytes to the packet decoder */ /* pass received bytes to the packet decoder */
for (int i = 0; i < count; i++) { for (int i = 0; i < count; i++) {
if (parse_char(buf[i]) > 0) { if (parse_char(buf[i]) > 0) {
if (handle_message() > 0) handled |= handle_message();
handled = true;
} }
} }
} }
@@ -334,6 +341,7 @@ UBX::parse_char(uint8_t b)
case UBX_DECODE_UNINIT: case UBX_DECODE_UNINIT:
if (b == UBX_SYNC1) { if (b == UBX_SYNC1) {
_decode_state = UBX_DECODE_GOT_SYNC1; _decode_state = UBX_DECODE_GOT_SYNC1;
//### printf("\nA");
} }
break; break;
@@ -342,11 +350,13 @@ UBX::parse_char(uint8_t b)
case UBX_DECODE_GOT_SYNC1: case UBX_DECODE_GOT_SYNC1:
if (b == UBX_SYNC2) { if (b == UBX_SYNC2) {
_decode_state = UBX_DECODE_GOT_SYNC2; _decode_state = UBX_DECODE_GOT_SYNC2;
//### printf("B");
} else { } else {
/* Second start symbol was wrong, reset state machine */ /* Second start symbol was wrong, reset state machine */
decode_init(); decode_init();
/* don't return error, it can be just false sync1 */ /* don't return error, it can be just false sync1 */
//### printf("-");
} }
break; break;
@@ -357,24 +367,28 @@ UBX::parse_char(uint8_t b)
add_byte_to_checksum(b); add_byte_to_checksum(b);
_message_class = b; _message_class = b;
_decode_state = UBX_DECODE_GOT_CLASS; _decode_state = UBX_DECODE_GOT_CLASS;
//### printf("C");
break; break;
case UBX_DECODE_GOT_CLASS: case UBX_DECODE_GOT_CLASS:
add_byte_to_checksum(b); add_byte_to_checksum(b);
_message_id = b; _message_id = b;
_decode_state = UBX_DECODE_GOT_MESSAGEID; _decode_state = UBX_DECODE_GOT_MESSAGEID;
//### printf("D");
break; break;
case UBX_DECODE_GOT_MESSAGEID: case UBX_DECODE_GOT_MESSAGEID:
add_byte_to_checksum(b); add_byte_to_checksum(b);
_payload_size = b; //this is the first length byte _payload_size = b; //this is the first length byte
_decode_state = UBX_DECODE_GOT_LENGTH1; _decode_state = UBX_DECODE_GOT_LENGTH1;
//### printf("E");
break; break;
case UBX_DECODE_GOT_LENGTH1: case UBX_DECODE_GOT_LENGTH1:
add_byte_to_checksum(b); add_byte_to_checksum(b);
_payload_size += b << 8; // here comes the second byte of length _payload_size += b << 8; // here comes the second byte of length
_decode_state = UBX_DECODE_GOT_LENGTH2; _decode_state = UBX_DECODE_GOT_LENGTH2;
//### printf("F");
break; break;
case UBX_DECODE_GOT_LENGTH2: case UBX_DECODE_GOT_LENGTH2:
@@ -389,6 +403,7 @@ UBX::parse_char(uint8_t b)
if (_rx_count >= _payload_size + 1) { //+1 because of 2 checksum bytes if (_rx_count >= _payload_size + 1) { //+1 because of 2 checksum bytes
/* compare checksum */ /* compare checksum */
if (_rx_ck_a == _rx_buffer[_rx_count - 1] && _rx_ck_b == _rx_buffer[_rx_count]) { if (_rx_ck_a == _rx_buffer[_rx_count - 1] && _rx_ck_b == _rx_buffer[_rx_count]) {
//### printf("O\n");
return 1; // message received successfully return 1; // message received successfully
} else { } else {
@@ -399,6 +414,7 @@ UBX::parse_char(uint8_t b)
} else if (_rx_count < RECV_BUFFER_SIZE) { } else if (_rx_count < RECV_BUFFER_SIZE) {
_rx_count++; _rx_count++;
//### printf(".");
} else { } else {
warnx("ubx: buffer full 0x%02x-0x%02x L%u", (unsigned)_message_class, (unsigned)_message_id, _payload_size); warnx("ubx: buffer full 0x%02x-0x%02x L%u", (unsigned)_message_class, (unsigned)_message_id, _payload_size);
@@ -489,65 +505,37 @@ UBX::handle_message()
break; break;
} }
#ifdef UBX_ENABLE_NAV_SVINFO
case UBX_MESSAGE_NAV_SVINFO: { case UBX_MESSAGE_NAV_SVINFO: {
//printf("GOT NAV_SVINFO\n"); //printf("GOT NAV_SVINFO\n");
if (!_enable_sat_info) { // if satellite info processing not enabled:
break; // ignore and disable NAV-SVINFO message
}
const int length_part1 = 8; const int length_part1 = 8;
gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer; gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer;
const int length_part2 = 12; const int length_part2 = 12;
gps_bin_nav_svinfo_part2_packet_t *packet_part2; gps_bin_nav_svinfo_part2_packet_t *packet_part2;
uint8_t satellites_used = 0;
int i;
//printf("Number of Channels: %d\n", packet_part1->numCh); //printf("Number of Channels: %d\n", packet_part1->numCh);
for (i = 0; i < packet_part1->numCh; i++) { for (_satellite_info->count = 0;
/* set pointer to sattelite_i information */ _satellite_info->count < MIN(packet_part1->numCh, sizeof(_satellite_info->snr) / sizeof(_satellite_info->snr[0]));
packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) & (_rx_buffer[length_part1 + i * length_part2]); _satellite_info->count++) {
/* set pointer to satellite_i information */
packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) & (_rx_buffer[length_part1 + _satellite_info->count* length_part2]);
/* write satellite information to global storage */ /* write satellite information to global storage */
uint8_t sv_used = packet_part2->flags & 0x01; _satellite_info->used[_satellite_info->count] = packet_part2->flags & 0x01;
_satellite_info->snr[_satellite_info->count] = packet_part2->cno;
if (sv_used) { _satellite_info->elevation[_satellite_info->count] = (uint8_t)(packet_part2->elev);
/* count SVs used for NAV */ _satellite_info->azimuth[_satellite_info->count] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f);
satellites_used++; _satellite_info->svid[_satellite_info->count] = packet_part2->svid;
} //printf("SAT %d: %d %d %d %d\n", _satellite_info->count, (int)sv_used, (int)packet_part2->cno, (int)(uint8_t)(packet_part2->elev), (int)packet_part2->svid);
/* record info for all channels, whether or not the SV is used for NAV */
_gps_position->satellite_used[i] = sv_used;
_gps_position->satellite_snr[i] = packet_part2->cno;
_gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev);
_gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f);
_gps_position->satellite_prn[i] = packet_part2->svid;
//printf("SAT %d: %d %d %d %d\n", i, (int)sv_used, (int)packet_part2->cno, (int)(uint8_t)(packet_part2->elev), (int)packet_part2->svid);
} }
for (i = packet_part1->numCh; i < 20; i++) { _satellite_info->timestamp = hrt_absolute_time();
/* unused channels have to be set to zero for e.g. MAVLink */
_gps_position->satellite_prn[i] = 0;
_gps_position->satellite_used[i] = 0;
_gps_position->satellite_snr[i] = 0;
_gps_position->satellite_elevation[i] = 0;
_gps_position->satellite_azimuth[i] = 0;
}
/* Note: _gps_position->satellites_visible is taken from NAV-SOL now. */ ret = 2;
/* _gps_position->satellites_visible = satellites_used; */ // visible ~= used but we are interested in the used ones
/* TODO: satellites_used may be written into a new field after sat monitoring data got separated from _gps_position */
if (packet_part1->numCh > 0) {
_gps_position->satellite_info_available = true;
} else {
_gps_position->satellite_info_available = false;
}
_gps_position->timestamp_satellites = hrt_absolute_time();
ret = 1;
break; break;
} }
#endif /* #ifdef UBX_ENABLE_NAV_SVINFO */
case UBX_MESSAGE_NAV_VELNED: { case UBX_MESSAGE_NAV_VELNED: {
// printf("GOT NAV_VELNED\n"); // printf("GOT NAV_VELNED\n");
@@ -650,7 +638,6 @@ UBX::handle_message()
warnx("ubx: not acknowledged"); warnx("ubx: not acknowledged");
/* configuration obviously not successful */ /* configuration obviously not successful */
_waiting_for_ack = false; _waiting_for_ack = false;
ret = -1;
break; break;
} }
src/drivers/gps/ubx.h
+5 -5
View File
@@ -48,9 +48,7 @@
#include "gps_helper.h" #include "gps_helper.h"
/* TODO: processing of NAV_SVINFO disabled, has to be re-written as an optional feature */ #define UBX_ENABLE_NAV_SVINFO 1 /* TODO: make this a command line option, allocate buffer(s) dynamically */
/* TODO: make this a command line option, allocate buffer dynamically */
#undef UBX_ENABLE_NAV_SVINFO
#define UBX_SYNC1 0xB5 #define UBX_SYNC1 0xB5
#define UBX_SYNC2 0x62 #define UBX_SYNC2 0x62
@@ -378,7 +376,7 @@ typedef enum {
/* calculate parser rx buffer size dependent on NAV_SVINFO enabled or not */ /* calculate parser rx buffer size dependent on NAV_SVINFO enabled or not */
/* TODO: make this a command line option, allocate buffer dynamically */ /* TODO: make this a command line option, allocate buffer dynamically */
#define RECV_BUFFER_OVERHEAD 10 // add this to the maximum Rx msg payload size to account for msg overhead */ #define RECV_BUFFER_OVERHEAD 10 // add this to the maximum Rx msg payload size to account for msg overhead */
#ifdef UBX_ENABLE_NAV_SVINFO #if (UBX_ENABLE_NAV_SVINFO != 0)
#define RECV_BUFFER_SIZE (UBX_NAV_SVINFO_RX_PAYLOAD_SIZE + RECV_BUFFER_OVERHEAD) #define RECV_BUFFER_SIZE (UBX_NAV_SVINFO_RX_PAYLOAD_SIZE + RECV_BUFFER_OVERHEAD)
#else #else
#define RECV_BUFFER_SIZE (UBX_MAX_RX_PAYLOAD_SIZE + RECV_BUFFER_OVERHEAD) #define RECV_BUFFER_SIZE (UBX_MAX_RX_PAYLOAD_SIZE + RECV_BUFFER_OVERHEAD)
@@ -387,7 +385,7 @@ typedef enum {
class UBX : public GPS_Helper class UBX : public GPS_Helper
{ {
public: 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, const bool &enable_sat_info);
~UBX(); ~UBX();
int receive(unsigned timeout); int receive(unsigned timeout);
int configure(unsigned &baudrate); int configure(unsigned &baudrate);
@@ -434,6 +432,8 @@ private:
int _fd; int _fd;
struct vehicle_gps_position_s *_gps_position; struct vehicle_gps_position_s *_gps_position;
struct satellite_info_s *_satellite_info;
bool _enable_sat_info;
bool _configured; bool _configured;
bool _waiting_for_ack; bool _waiting_for_ack;
uint8_t _message_class_needed; uint8_t _message_class_needed;
src/modules/uORB/topics/satellite_info.h
+10 -7
View File
@@ -53,14 +53,17 @@
/** /**
* GNSS Satellite Info. * GNSS Satellite Info.
*/ */
#define SAT_INFO_MAX_SATELLITES 20
struct satellite_info_s { struct satellite_info_s {
uint64_t timestamp; /**< Timestamp of satellite information */ uint64_t timestamp; /**< Timestamp of satellite info */
uint8_t count; /**< Number of satellites in satellite_...[] arrays */ uint8_t count; /**< Number of satellites in satellite info */
uint8_t svid[20]; /**< Space vehicle ID [1..255], see scheme below */ uint8_t svid[SAT_INFO_MAX_SATELLITES]; /**< Space vehicle ID [1..255], see scheme below */
uint8_t used[20]; /**< 0: Satellite not used, 1: used for navigation */ uint8_t used[SAT_INFO_MAX_SATELLITES]; /**< 0: Satellite not used, 1: used for navigation */
uint8_t elevation[20]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */ uint8_t elevation[SAT_INFO_MAX_SATELLITES]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */
uint8_t azimuth[20]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */ uint8_t azimuth[SAT_INFO_MAX_SATELLITES]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */
uint8_t snr[20]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99, zero when not tracking this satellite. */ 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. */
}; };
/** /**