mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-29 19:04:08 +08:00
U-blox driver rework, step 3
More sat info isolation Flush input after changing baudrate to allow driver restart w/o GNSS module restart
This commit is contained in:
Kynos
parent
9bad828bc0
commit
9f754e0e9a
@ -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>
|
||||
|
||||
@ -106,8 +107,10 @@ private:
|
||||
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
|
||||
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 _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
|
||||
bool _fake_gps; ///< fake gps output
|
||||
|
||||
@ -161,7 +164,8 @@ GPS::GPS(const char *uart_path, bool fake_gps) :
|
||||
_mode_changed(false),
|
||||
_mode(GPS_DRIVER_MODE_UBX),
|
||||
_Helper(nullptr),
|
||||
_report_pub(-1),
|
||||
_report_gps_pos_pub(-1),
|
||||
_report_sat_info_pub(-1),
|
||||
_rate(0.0f),
|
||||
_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 */
|
||||
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;
|
||||
}
|
||||
@ -271,33 +276,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_m = 0.9f;
|
||||
_report.epv_m = 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_m = 0.9f;
|
||||
_report_gps_pos.epv_m = 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 +318,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, &_report_sat_info, UBX_ENABLE_NAV_SVINFO);
|
||||
break;
|
||||
|
||||
case GPS_DRIVER_MODE_MTK:
|
||||
_Helper = new MTK(_serial_fd, &_report);
|
||||
_Helper = new MTK(_serial_fd, &_report_gps_pos);
|
||||
break;
|
||||
|
||||
default:
|
||||
@ -332,20 +337,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 (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 */
|
||||
if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) {
|
||||
@ -357,7 +375,7 @@ GPS::task_main()
|
||||
}
|
||||
|
||||
if (!_healthy) {
|
||||
char *mode_str = "unknown";
|
||||
const char *mode_str = "unknown";
|
||||
|
||||
switch (_mode) {
|
||||
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");
|
||||
|
||||
if (_report.timestamp_position != 0) {
|
||||
warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report.fix_type,
|
||||
_report.satellites_used, (double)(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", (double)_report.eph_m, (double)_report.epv_m);
|
||||
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_m, (double)_report_gps_pos.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);
|
||||
@ -578,7 +596,7 @@ gps_main(int argc, char *argv[])
|
||||
{
|
||||
|
||||
/* set to default */
|
||||
char *device_name = GPS_DEFAULT_UART_PORT;
|
||||
const char *device_name = GPS_DEFAULT_UART_PORT;
|
||||
bool fake_gps = false;
|
||||
|
||||
/*
|
||||
|
||||
@ -65,9 +65,13 @@
|
||||
#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) :
|
||||
#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),
|
||||
_gps_position(gps_position),
|
||||
_satellite_info(satellite_info),
|
||||
_enable_sat_info(enable_sat_info),
|
||||
_configured(false),
|
||||
_waiting_for_ack(false),
|
||||
_disable_cmd_last(0)
|
||||
@ -84,14 +88,19 @@ UBX::configure(unsigned &baudrate)
|
||||
{
|
||||
_configured = false;
|
||||
/* 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++) {
|
||||
baudrate = baudrates_to_try[baud_i];
|
||||
for (baud_i = 0; baud_i < sizeof(baudrates) / sizeof(baudrates[0]); baud_i++) {
|
||||
baudrate = baudrates[baud_i];
|
||||
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
|
||||
* 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.length = UBX_CFG_PRT_LENGTH;
|
||||
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.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK;
|
||||
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.length = UBX_CFG_PRT_LENGTH;
|
||||
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.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK;
|
||||
cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK;
|
||||
@ -144,7 +153,7 @@ UBX::configure(unsigned &baudrate)
|
||||
break;
|
||||
}
|
||||
|
||||
if (baud_i >= 5) {
|
||||
if (baud_i >= sizeof(baudrates) / sizeof(baudrates[0])) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
@ -220,14 +229,14 @@ UBX::configure(unsigned &baudrate)
|
||||
return 1;
|
||||
}
|
||||
|
||||
#ifdef UBX_ENABLE_NAV_SVINFO
|
||||
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5);
|
||||
if (_enable_sat_info) {
|
||||
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5);
|
||||
|
||||
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
|
||||
warnx("MSG CFG FAIL: NAV SVINFO");
|
||||
return 1;
|
||||
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
|
||||
warnx("MSG CFG FAIL: NAV SVINFO");
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
configure_message_rate(UBX_CLASS_MON, UBX_MESSAGE_MON_HW, 1);
|
||||
|
||||
@ -244,14 +253,13 @@ int
|
||||
UBX::wait_for_ack(unsigned timeout)
|
||||
{
|
||||
_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 (!_waiting_for_ack) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
} else {
|
||||
return -1; // timeout or error receiving, or NAK
|
||||
}
|
||||
@ -275,7 +283,7 @@ UBX::receive(unsigned timeout)
|
||||
|
||||
ssize_t count = 0;
|
||||
|
||||
bool handled = false;
|
||||
int handled = 0;
|
||||
|
||||
while (true) {
|
||||
|
||||
@ -290,7 +298,7 @@ UBX::receive(unsigned timeout)
|
||||
} else if (ret == 0) {
|
||||
/* return success after short delay after receiving a packet or timeout after long delay */
|
||||
if (handled) {
|
||||
return 1;
|
||||
return handled;
|
||||
|
||||
} else {
|
||||
return -1;
|
||||
@ -311,8 +319,7 @@ UBX::receive(unsigned timeout)
|
||||
/* pass received bytes to the packet decoder */
|
||||
for (int i = 0; i < count; i++) {
|
||||
if (parse_char(buf[i]) > 0) {
|
||||
if (handle_message() > 0)
|
||||
handled = true;
|
||||
handled |= handle_message();
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -334,6 +341,7 @@ UBX::parse_char(uint8_t b)
|
||||
case UBX_DECODE_UNINIT:
|
||||
if (b == UBX_SYNC1) {
|
||||
_decode_state = UBX_DECODE_GOT_SYNC1;
|
||||
//### printf("\nA");
|
||||
}
|
||||
|
||||
break;
|
||||
@ -342,11 +350,13 @@ UBX::parse_char(uint8_t b)
|
||||
case UBX_DECODE_GOT_SYNC1:
|
||||
if (b == UBX_SYNC2) {
|
||||
_decode_state = UBX_DECODE_GOT_SYNC2;
|
||||
//### printf("B");
|
||||
|
||||
} else {
|
||||
/* Second start symbol was wrong, reset state machine */
|
||||
decode_init();
|
||||
/* don't return error, it can be just false sync1 */
|
||||
//### printf("-");
|
||||
}
|
||||
|
||||
break;
|
||||
@ -357,24 +367,28 @@ UBX::parse_char(uint8_t b)
|
||||
add_byte_to_checksum(b);
|
||||
_message_class = b;
|
||||
_decode_state = UBX_DECODE_GOT_CLASS;
|
||||
//### printf("C");
|
||||
break;
|
||||
|
||||
case UBX_DECODE_GOT_CLASS:
|
||||
add_byte_to_checksum(b);
|
||||
_message_id = b;
|
||||
_decode_state = UBX_DECODE_GOT_MESSAGEID;
|
||||
//### printf("D");
|
||||
break;
|
||||
|
||||
case UBX_DECODE_GOT_MESSAGEID:
|
||||
add_byte_to_checksum(b);
|
||||
_payload_size = b; //this is the first length byte
|
||||
_decode_state = UBX_DECODE_GOT_LENGTH1;
|
||||
//### printf("E");
|
||||
break;
|
||||
|
||||
case UBX_DECODE_GOT_LENGTH1:
|
||||
add_byte_to_checksum(b);
|
||||
_payload_size += b << 8; // here comes the second byte of length
|
||||
_decode_state = UBX_DECODE_GOT_LENGTH2;
|
||||
//### printf("F");
|
||||
break;
|
||||
|
||||
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
|
||||
/* compare checksum */
|
||||
if (_rx_ck_a == _rx_buffer[_rx_count - 1] && _rx_ck_b == _rx_buffer[_rx_count]) {
|
||||
//### printf("O\n");
|
||||
return 1; // message received successfully
|
||||
|
||||
} else {
|
||||
@ -399,6 +414,7 @@ UBX::parse_char(uint8_t b)
|
||||
|
||||
} else if (_rx_count < RECV_BUFFER_SIZE) {
|
||||
_rx_count++;
|
||||
//### printf(".");
|
||||
|
||||
} else {
|
||||
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;
|
||||
}
|
||||
|
||||
#ifdef UBX_ENABLE_NAV_SVINFO
|
||||
case UBX_MESSAGE_NAV_SVINFO: {
|
||||
//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;
|
||||
gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer;
|
||||
const int length_part2 = 12;
|
||||
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);
|
||||
for (i = 0; i < packet_part1->numCh; i++) {
|
||||
/* set pointer to sattelite_i information */
|
||||
packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) & (_rx_buffer[length_part1 + i * length_part2]);
|
||||
for (_satellite_info->count = 0;
|
||||
_satellite_info->count < MIN(packet_part1->numCh, sizeof(_satellite_info->snr) / sizeof(_satellite_info->snr[0]));
|
||||
_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 */
|
||||
uint8_t sv_used = packet_part2->flags & 0x01;
|
||||
|
||||
if (sv_used) {
|
||||
/* count SVs used for NAV */
|
||||
satellites_used++;
|
||||
}
|
||||
|
||||
/* 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);
|
||||
_satellite_info->used[_satellite_info->count] = packet_part2->flags & 0x01;
|
||||
_satellite_info->snr[_satellite_info->count] = packet_part2->cno;
|
||||
_satellite_info->elevation[_satellite_info->count] = (uint8_t)(packet_part2->elev);
|
||||
_satellite_info->azimuth[_satellite_info->count] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f);
|
||||
_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);
|
||||
}
|
||||
|
||||
for (i = packet_part1->numCh; i < 20; i++) {
|
||||
/* 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;
|
||||
}
|
||||
_satellite_info->timestamp = hrt_absolute_time();
|
||||
|
||||
/* Note: _gps_position->satellites_visible is taken from NAV-SOL now. */
|
||||
/* _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;
|
||||
ret = 2;
|
||||
break;
|
||||
}
|
||||
#endif /* #ifdef UBX_ENABLE_NAV_SVINFO */
|
||||
|
||||
case UBX_MESSAGE_NAV_VELNED: {
|
||||
// printf("GOT NAV_VELNED\n");
|
||||
@ -650,7 +638,6 @@ UBX::handle_message()
|
||||
warnx("ubx: not acknowledged");
|
||||
/* configuration obviously not successful */
|
||||
_waiting_for_ack = false;
|
||||
ret = -1;
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
@ -48,9 +48,7 @@
|
||||
|
||||
#include "gps_helper.h"
|
||||
|
||||
/* TODO: processing of NAV_SVINFO disabled, has to be re-written as an optional feature */
|
||||
/* TODO: make this a command line option, allocate buffer dynamically */
|
||||
#undef UBX_ENABLE_NAV_SVINFO
|
||||
#define UBX_ENABLE_NAV_SVINFO 1 /* TODO: make this a command line option, allocate buffer(s) dynamically */
|
||||
|
||||
#define UBX_SYNC1 0xB5
|
||||
#define UBX_SYNC2 0x62
|
||||
@ -378,7 +376,7 @@ typedef enum {
|
||||
/* calculate parser rx buffer size dependent on NAV_SVINFO enabled or not */
|
||||
/* 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 */
|
||||
#ifdef UBX_ENABLE_NAV_SVINFO
|
||||
#if (UBX_ENABLE_NAV_SVINFO != 0)
|
||||
#define RECV_BUFFER_SIZE (UBX_NAV_SVINFO_RX_PAYLOAD_SIZE + RECV_BUFFER_OVERHEAD)
|
||||
#else
|
||||
#define RECV_BUFFER_SIZE (UBX_MAX_RX_PAYLOAD_SIZE + RECV_BUFFER_OVERHEAD)
|
||||
@ -387,7 +385,7 @@ typedef enum {
|
||||
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, const bool &enable_sat_info);
|
||||
~UBX();
|
||||
int receive(unsigned timeout);
|
||||
int configure(unsigned &baudrate);
|
||||
@ -434,6 +432,8 @@ private:
|
||||
|
||||
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;
|
||||
uint8_t _message_class_needed;
|
||||
|
||||
@ -53,14 +53,17 @@
|
||||
/**
|
||||
* GNSS Satellite Info.
|
||||
*/
|
||||
|
||||
#define SAT_INFO_MAX_SATELLITES 20
|
||||
|
||||
struct satellite_info_s {
|
||||
uint64_t timestamp; /**< Timestamp of satellite information */
|
||||
uint8_t count; /**< Number of satellites in satellite_...[] arrays */
|
||||
uint8_t svid[20]; /**< Space vehicle ID [1..255], see scheme below */
|
||||
uint8_t used[20]; /**< 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 azimuth[20]; /**< 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. */
|
||||
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. */
|
||||
};
|
||||
|
||||
/**
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user