mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-18 18:59:07 +08:00
GPS: Fixed code style
This commit is contained in:
Lorenz Meier
parent
25434055c6
commit
ff6676ef8c
@ -37,7 +37,7 @@ ASHTECH::~ASHTECH()
|
||||
|
||||
int ASHTECH::handle_message(int len)
|
||||
{
|
||||
char * endp;
|
||||
char *endp;
|
||||
|
||||
if (len < 7) { return 0; }
|
||||
|
||||
@ -69,7 +69,8 @@ int ASHTECH::handle_message(int len)
|
||||
Fields 5 and 6 together yield the total offset. For example, if field 5 is -5 and field 6 is +15, local time is 5 hours and 15 minutes earlier than GMT.
|
||||
*/
|
||||
double ashtech_time = 0.0;
|
||||
int day = 0, month = 0, year = 0, local_time_off_hour __attribute__((unused)) = 0, local_time_off_min __attribute__((unused)) = 0;
|
||||
int day = 0, month = 0, year = 0, local_time_off_hour __attribute__((unused)) = 0,
|
||||
local_time_off_min __attribute__((unused)) = 0;
|
||||
|
||||
if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; }
|
||||
|
||||
@ -110,12 +111,14 @@ int ASHTECH::handle_message(int len)
|
||||
timespec ts;
|
||||
ts.tv_sec = epoch;
|
||||
ts.tv_nsec = usecs * 1000;
|
||||
|
||||
if (clock_settime(CLOCK_REALTIME, &ts)) {
|
||||
warn("failed setting clock");
|
||||
}
|
||||
|
||||
_gps_position->time_utc_usec = static_cast<uint64_t>(epoch) * 1000000ULL;
|
||||
_gps_position->time_utc_usec += usecs;
|
||||
|
||||
} else {
|
||||
_gps_position->time_utc_usec = 0;
|
||||
}
|
||||
@ -266,7 +269,8 @@ int ASHTECH::handle_message(int len)
|
||||
double ashtech_time __attribute__((unused)) = 0.0, lat = 0.0, lon = 0.0, alt = 0.0;
|
||||
int num_of_sv __attribute__((unused)) = 0, fix_quality = 0;
|
||||
double track_true = 0.0, ground_speed = 0.0 , age_of_corr __attribute__((unused)) = 0.0;
|
||||
double hdop __attribute__((unused)) = 99.9, vdop __attribute__((unused)) = 99.9, pdop __attribute__((unused)) = 99.9, tdop __attribute__((unused)) = 99.9, vertic_vel = 0.0;
|
||||
double hdop __attribute__((unused)) = 99.9, vdop __attribute__((unused)) = 99.9, pdop __attribute__((unused)) = 99.9,
|
||||
tdop __attribute__((unused)) = 99.9, vertic_vel = 0.0;
|
||||
char ns = '?', ew = '?';
|
||||
|
||||
if (bufptr && *(++bufptr) != ',') { fix_quality = strtol(bufptr, &endp, 10); bufptr = endp; }
|
||||
@ -281,7 +285,9 @@ int ASHTECH::handle_message(int len)
|
||||
* or strtod won't find anything and endp will point exactly where bufptr is. The same is for lon and alt.
|
||||
*/
|
||||
lat = strtod(bufptr, &endp);
|
||||
|
||||
if (bufptr != endp) {coordinatesFound++;}
|
||||
|
||||
bufptr = endp;
|
||||
}
|
||||
|
||||
@ -289,7 +295,9 @@ int ASHTECH::handle_message(int len)
|
||||
|
||||
if (bufptr && *(++bufptr) != ',') {
|
||||
lon = strtod(bufptr, &endp);
|
||||
|
||||
if (bufptr != endp) {coordinatesFound++;}
|
||||
|
||||
bufptr = endp;
|
||||
}
|
||||
|
||||
@ -297,7 +305,9 @@ int ASHTECH::handle_message(int len)
|
||||
|
||||
if (bufptr && *(++bufptr) != ',') {
|
||||
alt = strtod(bufptr, &endp);
|
||||
|
||||
if (bufptr != endp) {coordinatesFound++;}
|
||||
|
||||
bufptr = endp;
|
||||
}
|
||||
|
||||
@ -349,7 +359,8 @@ int ASHTECH::handle_message(int len)
|
||||
_gps_position->vel_n_m_s = velocity_north; /** GPS ground speed in m/s */
|
||||
_gps_position->vel_e_m_s = velocity_east; /** GPS ground speed in m/s */
|
||||
_gps_position->vel_d_m_s = static_cast<float>(-vertic_vel); /** GPS ground speed in m/s */
|
||||
_gps_position->cog_rad = track_rad; /** Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */
|
||||
_gps_position->cog_rad =
|
||||
track_rad; /** Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */
|
||||
_gps_position->vel_ned_valid = true; /** Flag to indicate if NED speed is valid */
|
||||
_gps_position->c_variance_rad = 0.1f;
|
||||
_gps_position->timestamp_velocity = hrt_absolute_time();
|
||||
@ -381,7 +392,8 @@ int ASHTECH::handle_message(int len)
|
||||
9 The checksum data, always begins with *
|
||||
*/
|
||||
double ashtech_time __attribute__((unused)) = 0.0, lat_err = 0.0, lon_err = 0.0, alt_err = 0.0;
|
||||
double min_err __attribute__((unused)) = 0.0, maj_err __attribute__((unused)) = 0.0, deg_from_north __attribute__((unused)) = 0.0, rms_err __attribute__((unused)) = 0.0;
|
||||
double min_err __attribute__((unused)) = 0.0, maj_err __attribute__((unused)) = 0.0,
|
||||
deg_from_north __attribute__((unused)) = 0.0, rms_err __attribute__((unused)) = 0.0;
|
||||
|
||||
if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; }
|
||||
|
||||
@ -400,7 +412,7 @@ int ASHTECH::handle_message(int len)
|
||||
if (bufptr && *(++bufptr) != ',') { alt_err = strtod(bufptr, &endp); bufptr = endp; }
|
||||
|
||||
_gps_position->eph = sqrtf(static_cast<float>(lat_err) * static_cast<float>(lat_err)
|
||||
+ static_cast<float>(lon_err) * static_cast<float>(lon_err));
|
||||
+ static_cast<float>(lon_err) * static_cast<float>(lon_err));
|
||||
_gps_position->epv = static_cast<float>(alt_err);
|
||||
|
||||
_gps_position->s_variance_m_s = 0;
|
||||
@ -571,7 +583,7 @@ int ASHTECH::parse_char(uint8_t b)
|
||||
int iRet = 0;
|
||||
|
||||
switch (_decode_state) {
|
||||
/* First, look for sync1 */
|
||||
/* First, look for sync1 */
|
||||
case NME_DECODE_UNINIT:
|
||||
if (b == '$') {
|
||||
_decode_state = NME_DECODE_GOT_SYNC1;
|
||||
@ -636,13 +648,13 @@ void ASHTECH::decode_init(void)
|
||||
*/
|
||||
|
||||
const char comm[] = "$PASHS,POP,20\r\n"\
|
||||
"$PASHS,NME,ZDA,B,ON,3\r\n"\
|
||||
"$PASHS,NME,GGA,B,OFF\r\n"\
|
||||
"$PASHS,NME,GST,B,ON,3\r\n"\
|
||||
"$PASHS,NME,POS,B,ON,0.05\r\n"\
|
||||
"$PASHS,NME,GSV,B,ON,3\r\n"\
|
||||
"$PASHS,SPD,A,8\r\n"\
|
||||
"$PASHS,SPD,B,9\r\n";
|
||||
"$PASHS,NME,ZDA,B,ON,3\r\n"\
|
||||
"$PASHS,NME,GGA,B,OFF\r\n"\
|
||||
"$PASHS,NME,GST,B,ON,3\r\n"\
|
||||
"$PASHS,NME,POS,B,ON,0.05\r\n"\
|
||||
"$PASHS,NME,GSV,B,ON,3\r\n"\
|
||||
"$PASHS,SPD,A,8\r\n"\
|
||||
"$PASHS,SPD,B,9\r\n";
|
||||
|
||||
int ASHTECH::configure(unsigned &baudrate)
|
||||
{
|
||||
|
||||
@ -211,8 +211,9 @@ GPS::~GPS()
|
||||
}
|
||||
|
||||
/* well, kill it anyway, though this will probably crash */
|
||||
if (_task != -1)
|
||||
if (_task != -1) {
|
||||
task_delete(_task);
|
||||
}
|
||||
|
||||
g_dev = nullptr;
|
||||
|
||||
@ -224,12 +225,13 @@ GPS::init()
|
||||
int ret = ERROR;
|
||||
|
||||
/* do regular cdev init */
|
||||
if (CDev::init() != OK)
|
||||
if (CDev::init() != OK) {
|
||||
goto out;
|
||||
}
|
||||
|
||||
/* start the GPS driver worker task */
|
||||
_task = px4_task_spawn_cmd("gps", SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_SLOW_DRIVER, 1200, (main_t)&GPS::task_main_trampoline, nullptr);
|
||||
SCHED_PRIORITY_SLOW_DRIVER, 1200, (main_t)&GPS::task_main_trampoline, nullptr);
|
||||
|
||||
if (_task < 0) {
|
||||
warnx("task start failed: %d", errno);
|
||||
@ -305,9 +307,10 @@ GPS::task_main()
|
||||
_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.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;
|
||||
_report_gps_pos.vel_ned_valid = true;
|
||||
|
||||
/* no time and satellite information simulated */
|
||||
|
||||
@ -392,14 +395,16 @@ GPS::task_main()
|
||||
}
|
||||
|
||||
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 */
|
||||
|
||||
if (!(_pub_blocked)) {
|
||||
if (helper_ret & 1) {
|
||||
orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
|
||||
}
|
||||
|
||||
if (_p_report_sat_info && (helper_ret & 2)) {
|
||||
if (_report_sat_info_pub != nullptr) {
|
||||
orb_publish(ORB_ID(satellite_info), _report_sat_info_pub, _p_report_sat_info);
|
||||
@ -508,7 +513,7 @@ void
|
||||
GPS::print_info()
|
||||
{
|
||||
//GPS Mode
|
||||
if(_fake_gps) {
|
||||
if (_fake_gps) {
|
||||
warnx("protocol: SIMULATED");
|
||||
}
|
||||
|
||||
@ -522,27 +527,27 @@ GPS::print_info()
|
||||
warnx("protocol: MTK");
|
||||
break;
|
||||
|
||||
case GPS_DRIVER_MODE_ASHTECH:
|
||||
warnx("protocol: ASHTECH");
|
||||
break;
|
||||
case GPS_DRIVER_MODE_ASHTECH:
|
||||
warnx("protocol: ASHTECH");
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK");
|
||||
warnx("sat info: %s, noise: %d, jamming detected: %s",
|
||||
(_p_report_sat_info != nullptr) ? "enabled" : "disabled",
|
||||
_report_gps_pos.noise_per_ms,
|
||||
_report_gps_pos.jamming_indicator == 255 ? "YES" : "NO");
|
||||
warnx("sat info: %s, noise: %d, jamming detected: %s",
|
||||
(_p_report_sat_info != nullptr) ? "enabled" : "disabled",
|
||||
_report_gps_pos.noise_per_ms,
|
||||
_report_gps_pos.jamming_indicator == 255 ? "YES" : "NO");
|
||||
|
||||
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);
|
||||
_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("vel: %.2fm/s, %.2fm/s, %.2fm/s", (double)_report_gps_pos.vel_n_m_s,
|
||||
(double)_report_gps_pos.vel_e_m_s, (double)_report_gps_pos.vel_d_m_s);
|
||||
(double)_report_gps_pos.vel_e_m_s, (double)_report_gps_pos.vel_d_m_s);
|
||||
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());
|
||||
@ -575,17 +580,20 @@ start(const char *uart_path, bool fake_gps, bool enable_sat_info)
|
||||
{
|
||||
int fd;
|
||||
|
||||
if (g_dev != nullptr)
|
||||
if (g_dev != nullptr) {
|
||||
errx(1, "already started");
|
||||
}
|
||||
|
||||
/* create the driver */
|
||||
g_dev = new GPS(uart_path, fake_gps, enable_sat_info);
|
||||
|
||||
if (g_dev == nullptr)
|
||||
if (g_dev == nullptr) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
if (OK != g_dev->init())
|
||||
if (OK != g_dev->init()) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
/* set the poll rate to default, starts automatic data collection */
|
||||
fd = open(GPS0_DEVICE_PATH, O_RDONLY);
|
||||
@ -639,11 +647,13 @@ reset()
|
||||
{
|
||||
int fd = open(GPS0_DEVICE_PATH, O_RDONLY);
|
||||
|
||||
if (fd < 0)
|
||||
if (fd < 0) {
|
||||
err(1, "failed ");
|
||||
}
|
||||
|
||||
if (ioctl(fd, SENSORIOCRESET, 0) < 0)
|
||||
if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
|
||||
err(1, "reset failed");
|
||||
}
|
||||
|
||||
exit(0);
|
||||
}
|
||||
@ -654,8 +664,9 @@ reset()
|
||||
void
|
||||
info()
|
||||
{
|
||||
if (g_dev == nullptr)
|
||||
if (g_dev == nullptr) {
|
||||
errx(1, "not running");
|
||||
}
|
||||
|
||||
g_dev->print_info();
|
||||
|
||||
@ -690,39 +701,45 @@ gps_main(int argc, char *argv[])
|
||||
|
||||
/* Detect fake gps option */
|
||||
for (int i = 2; i < argc; i++) {
|
||||
if (!strcmp(argv[i], "-f"))
|
||||
if (!strcmp(argv[i], "-f")) {
|
||||
fake_gps = true;
|
||||
}
|
||||
}
|
||||
|
||||
/* Detect sat info option */
|
||||
for (int i = 2; i < argc; i++) {
|
||||
if (!strcmp(argv[i], "-s"))
|
||||
if (!strcmp(argv[i], "-s")) {
|
||||
enable_sat_info = true;
|
||||
}
|
||||
}
|
||||
|
||||
gps::start(device_name, fake_gps, enable_sat_info);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "stop"))
|
||||
if (!strcmp(argv[1], "stop")) {
|
||||
gps::stop();
|
||||
}
|
||||
|
||||
/*
|
||||
* Test the driver/device.
|
||||
*/
|
||||
if (!strcmp(argv[1], "test"))
|
||||
if (!strcmp(argv[1], "test")) {
|
||||
gps::test();
|
||||
}
|
||||
|
||||
/*
|
||||
* Reset the driver.
|
||||
*/
|
||||
if (!strcmp(argv[1], "reset"))
|
||||
if (!strcmp(argv[1], "reset")) {
|
||||
gps::reset();
|
||||
}
|
||||
|
||||
/*
|
||||
* Print driver status.
|
||||
*/
|
||||
if (!strcmp(argv[1], "status"))
|
||||
if (!strcmp(argv[1], "status")) {
|
||||
gps::info();
|
||||
}
|
||||
|
||||
out:
|
||||
errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status'\n [-d /dev/ttyS0-n][-f (for enabling fake)][-s (to enable sat info)]");
|
||||
|
||||
@ -66,8 +66,9 @@ int
|
||||
MTK::configure(unsigned &baudrate)
|
||||
{
|
||||
/* set baudrate first */
|
||||
if (GPS_Helper::set_baudrate(_fd, MTK_BAUDRATE) != 0)
|
||||
if (GPS_Helper::set_baudrate(_fd, MTK_BAUDRATE) != 0) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
baudrate = MTK_BAUDRATE;
|
||||
|
||||
@ -207,8 +208,9 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet)
|
||||
|
||||
} else if (_decode_state == MTK_DECODE_GOT_CK_B) {
|
||||
// Add to checksum
|
||||
if (_rx_count < 33)
|
||||
if (_rx_count < 33) {
|
||||
add_byte_to_checksum(b);
|
||||
}
|
||||
|
||||
// Fill packet buffer
|
||||
((uint8_t *)(&packet))[_rx_count] = b;
|
||||
@ -288,12 +290,14 @@ MTK::handle_message(gps_mtk_packet_t &packet)
|
||||
timespec ts;
|
||||
ts.tv_sec = epoch;
|
||||
ts.tv_nsec = timeinfo_conversion_temp * 1000000ULL;
|
||||
|
||||
if (clock_settime(CLOCK_REALTIME, &ts)) {
|
||||
warn("failed setting clock");
|
||||
}
|
||||
|
||||
_gps_position->time_utc_usec = static_cast<uint64_t>(epoch) * 1000000ULL;
|
||||
_gps_position->time_utc_usec += timeinfo_conversion_temp * 1000ULL;
|
||||
|
||||
} else {
|
||||
_gps_position->time_utc_usec = 0;
|
||||
}
|
||||
|
||||
@ -199,6 +199,7 @@ UBX::configure(unsigned &baudrate)
|
||||
if (wait_for_ack(UBX_MSG_CFG_SBAS, UBX_CONFIG_TIMEOUT, true) < 0) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
/* configure message rates */
|
||||
@ -207,41 +208,50 @@ UBX::configure(unsigned &baudrate)
|
||||
/* try to set rate for NAV-PVT */
|
||||
/* (implemented for ubx7+ modules only, use NAV-SOL, NAV-POSLLH, NAV-VELNED and NAV-TIMEUTC for ubx6) */
|
||||
configure_message_rate(UBX_MSG_NAV_PVT, 1);
|
||||
|
||||
if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
|
||||
_use_nav_pvt = false;
|
||||
|
||||
} else {
|
||||
_use_nav_pvt = true;
|
||||
}
|
||||
|
||||
UBX_DEBUG("%susing NAV-PVT", _use_nav_pvt ? "" : "not ");
|
||||
|
||||
if (!_use_nav_pvt) {
|
||||
configure_message_rate(UBX_MSG_NAV_TIMEUTC, 5);
|
||||
|
||||
if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
configure_message_rate(UBX_MSG_NAV_POSLLH, 1);
|
||||
|
||||
if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
configure_message_rate(UBX_MSG_NAV_SOL, 1);
|
||||
|
||||
if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
configure_message_rate(UBX_MSG_NAV_VELNED, 1);
|
||||
|
||||
if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
configure_message_rate(UBX_MSG_NAV_SVINFO, (_satellite_info != nullptr) ? 5 : 0);
|
||||
|
||||
if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
configure_message_rate(UBX_MSG_MON_HW, 1);
|
||||
|
||||
if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
|
||||
return 1;
|
||||
}
|
||||
@ -269,9 +279,11 @@ UBX::wait_for_ack(const uint16_t msg, const unsigned timeout, const bool report)
|
||||
|
||||
if (_ack_state == UBX_ACK_GOT_ACK) {
|
||||
ret = 0; // ACK received ok
|
||||
|
||||
} else if (report) {
|
||||
if (_ack_state == UBX_ACK_GOT_NAK) {
|
||||
UBX_DEBUG("ubx msg 0x%04x NAK", SWAP16((unsigned)msg));
|
||||
|
||||
} else {
|
||||
UBX_DEBUG("ubx msg 0x%04x ACK timeout", SWAP16((unsigned)msg));
|
||||
}
|
||||
@ -359,6 +371,7 @@ UBX::parse_char(const uint8_t b)
|
||||
UBX_TRACE_PARSER("\nA");
|
||||
_decode_state = UBX_DECODE_SYNC2;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
/* Expecting Sync2 */
|
||||
@ -370,6 +383,7 @@ UBX::parse_char(const uint8_t b)
|
||||
} else { // Sync1 not followed by Sync2: reset parser
|
||||
decode_init();
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
/* Expecting Class */
|
||||
@ -401,38 +415,48 @@ UBX::parse_char(const uint8_t b)
|
||||
UBX_TRACE_PARSER("F");
|
||||
add_byte_to_checksum(b);
|
||||
_rx_payload_length |= b << 8; // calculate payload size
|
||||
|
||||
if (payload_rx_init() != 0) { // start payload reception
|
||||
// payload will not be handled, discard message
|
||||
decode_init();
|
||||
|
||||
} else {
|
||||
_decode_state = (_rx_payload_length > 0) ? UBX_DECODE_PAYLOAD : UBX_DECODE_CHKSUM1;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
/* Expecting payload */
|
||||
case UBX_DECODE_PAYLOAD:
|
||||
UBX_TRACE_PARSER(".");
|
||||
add_byte_to_checksum(b);
|
||||
|
||||
switch (_rx_msg) {
|
||||
case UBX_MSG_NAV_SVINFO:
|
||||
ret = payload_rx_add_nav_svinfo(b); // add a NAV-SVINFO payload byte
|
||||
break;
|
||||
|
||||
case UBX_MSG_MON_VER:
|
||||
ret = payload_rx_add_mon_ver(b); // add a MON-VER payload byte
|
||||
break;
|
||||
|
||||
default:
|
||||
ret = payload_rx_add(b); // add a payload byte
|
||||
break;
|
||||
}
|
||||
|
||||
if (ret < 0) {
|
||||
// payload not handled, discard message
|
||||
decode_init();
|
||||
|
||||
} else if (ret > 0) {
|
||||
// payload complete, expecting checksum
|
||||
_decode_state = UBX_DECODE_CHKSUM1;
|
||||
|
||||
} else {
|
||||
// expecting more payload, stay in state UBX_DECODE_PAYLOAD
|
||||
}
|
||||
|
||||
ret = 0;
|
||||
break;
|
||||
|
||||
@ -441,18 +465,22 @@ UBX::parse_char(const uint8_t b)
|
||||
if (_rx_ck_a != b) {
|
||||
UBX_WARN("ubx checksum err");
|
||||
decode_init();
|
||||
|
||||
} else {
|
||||
_decode_state = UBX_DECODE_CHKSUM2;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
/* Expecting second checksum byte */
|
||||
case UBX_DECODE_CHKSUM2:
|
||||
if (_rx_ck_b != b) {
|
||||
UBX_WARN("ubx checksum err");
|
||||
|
||||
} else {
|
||||
ret = payload_rx_done(); // finish payload processing
|
||||
}
|
||||
|
||||
decode_init();
|
||||
break;
|
||||
|
||||
@ -475,83 +503,116 @@ UBX::payload_rx_init()
|
||||
|
||||
switch (_rx_msg) {
|
||||
case UBX_MSG_NAV_PVT:
|
||||
if ( (_rx_payload_length != UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX7) /* u-blox 7 msg format */
|
||||
&& (_rx_payload_length != UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX8)) /* u-blox 8+ msg format */
|
||||
if ((_rx_payload_length != UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX7) /* u-blox 7 msg format */
|
||||
&& (_rx_payload_length != UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX8)) { /* u-blox 8+ msg format */
|
||||
_rx_state = UBX_RXMSG_ERROR_LENGTH;
|
||||
else if (!_configured)
|
||||
_rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
|
||||
else if (!_use_nav_pvt)
|
||||
_rx_state = UBX_RXMSG_DISABLE; // disable if not using NAV-PVT
|
||||
|
||||
} else if (!_configured) {
|
||||
_rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
|
||||
|
||||
} else if (!_use_nav_pvt) {
|
||||
_rx_state = UBX_RXMSG_DISABLE; // disable if not using NAV-PVT
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case UBX_MSG_NAV_POSLLH:
|
||||
if (_rx_payload_length != sizeof(ubx_payload_rx_nav_posllh_t))
|
||||
if (_rx_payload_length != sizeof(ubx_payload_rx_nav_posllh_t)) {
|
||||
_rx_state = UBX_RXMSG_ERROR_LENGTH;
|
||||
else if (!_configured)
|
||||
_rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
|
||||
else if (_use_nav_pvt)
|
||||
_rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead
|
||||
|
||||
} else if (!_configured) {
|
||||
_rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
|
||||
|
||||
} else if (_use_nav_pvt) {
|
||||
_rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case UBX_MSG_NAV_SOL:
|
||||
if (_rx_payload_length != sizeof(ubx_payload_rx_nav_sol_t))
|
||||
if (_rx_payload_length != sizeof(ubx_payload_rx_nav_sol_t)) {
|
||||
_rx_state = UBX_RXMSG_ERROR_LENGTH;
|
||||
else if (!_configured)
|
||||
_rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
|
||||
else if (_use_nav_pvt)
|
||||
_rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead
|
||||
|
||||
} else if (!_configured) {
|
||||
_rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
|
||||
|
||||
} else if (_use_nav_pvt) {
|
||||
_rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case UBX_MSG_NAV_TIMEUTC:
|
||||
if (_rx_payload_length != sizeof(ubx_payload_rx_nav_timeutc_t))
|
||||
if (_rx_payload_length != sizeof(ubx_payload_rx_nav_timeutc_t)) {
|
||||
_rx_state = UBX_RXMSG_ERROR_LENGTH;
|
||||
else if (!_configured)
|
||||
_rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
|
||||
else if (_use_nav_pvt)
|
||||
_rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead
|
||||
|
||||
} else if (!_configured) {
|
||||
_rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
|
||||
|
||||
} else if (_use_nav_pvt) {
|
||||
_rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case UBX_MSG_NAV_SVINFO:
|
||||
if (_satellite_info == nullptr)
|
||||
_rx_state = UBX_RXMSG_DISABLE; // disable if sat info not requested
|
||||
else if (!_configured)
|
||||
_rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
|
||||
else
|
||||
memset(_satellite_info, 0, sizeof(*_satellite_info)); // initialize sat info
|
||||
if (_satellite_info == nullptr) {
|
||||
_rx_state = UBX_RXMSG_DISABLE; // disable if sat info not requested
|
||||
|
||||
} else if (!_configured) {
|
||||
_rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
|
||||
|
||||
} else {
|
||||
memset(_satellite_info, 0, sizeof(*_satellite_info)); // initialize sat info
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case UBX_MSG_NAV_VELNED:
|
||||
if (_rx_payload_length != sizeof(ubx_payload_rx_nav_velned_t))
|
||||
if (_rx_payload_length != sizeof(ubx_payload_rx_nav_velned_t)) {
|
||||
_rx_state = UBX_RXMSG_ERROR_LENGTH;
|
||||
else if (!_configured)
|
||||
_rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
|
||||
else if (_use_nav_pvt)
|
||||
_rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead
|
||||
|
||||
} else if (!_configured) {
|
||||
_rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
|
||||
|
||||
} else if (_use_nav_pvt) {
|
||||
_rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case UBX_MSG_MON_VER:
|
||||
break; // unconditionally handle this message
|
||||
|
||||
case UBX_MSG_MON_HW:
|
||||
if ( (_rx_payload_length != sizeof(ubx_payload_rx_mon_hw_ubx6_t)) /* u-blox 6 msg format */
|
||||
&& (_rx_payload_length != sizeof(ubx_payload_rx_mon_hw_ubx7_t))) /* u-blox 7+ msg format */
|
||||
if ((_rx_payload_length != sizeof(ubx_payload_rx_mon_hw_ubx6_t)) /* u-blox 6 msg format */
|
||||
&& (_rx_payload_length != sizeof(ubx_payload_rx_mon_hw_ubx7_t))) { /* u-blox 7+ msg format */
|
||||
_rx_state = UBX_RXMSG_ERROR_LENGTH;
|
||||
else if (!_configured)
|
||||
_rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
|
||||
|
||||
} else if (!_configured) {
|
||||
_rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case UBX_MSG_ACK_ACK:
|
||||
if (_rx_payload_length != sizeof(ubx_payload_rx_ack_ack_t))
|
||||
if (_rx_payload_length != sizeof(ubx_payload_rx_ack_ack_t)) {
|
||||
_rx_state = UBX_RXMSG_ERROR_LENGTH;
|
||||
else if (_configured)
|
||||
_rx_state = UBX_RXMSG_IGNORE; // ignore if _configured
|
||||
|
||||
} else if (_configured) {
|
||||
_rx_state = UBX_RXMSG_IGNORE; // ignore if _configured
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case UBX_MSG_ACK_NAK:
|
||||
if (_rx_payload_length != sizeof(ubx_payload_rx_ack_nak_t))
|
||||
if (_rx_payload_length != sizeof(ubx_payload_rx_ack_nak_t)) {
|
||||
_rx_state = UBX_RXMSG_ERROR_LENGTH;
|
||||
else if (_configured)
|
||||
_rx_state = UBX_RXMSG_IGNORE; // ignore if _configured
|
||||
|
||||
} else if (_configured) {
|
||||
_rx_state = UBX_RXMSG_IGNORE; // ignore if _configured
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
@ -624,32 +685,39 @@ UBX::payload_rx_add_nav_svinfo(const uint8_t b)
|
||||
if (_rx_payload_index < sizeof(ubx_payload_rx_nav_svinfo_part1_t)) {
|
||||
// Fill Part 1 buffer
|
||||
_buf.raw[_rx_payload_index] = b;
|
||||
|
||||
} else {
|
||||
if (_rx_payload_index == sizeof(ubx_payload_rx_nav_svinfo_part1_t)) {
|
||||
// Part 1 complete: decode Part 1 buffer
|
||||
_satellite_info->count = MIN(_buf.payload_rx_nav_svinfo_part1.numCh, satellite_info_s::SAT_INFO_MAX_SATELLITES);
|
||||
UBX_TRACE_SVINFO("SVINFO len %u numCh %u\n", (unsigned)_rx_payload_length, (unsigned)_buf.payload_rx_nav_svinfo_part1.numCh);
|
||||
UBX_TRACE_SVINFO("SVINFO len %u numCh %u\n", (unsigned)_rx_payload_length,
|
||||
(unsigned)_buf.payload_rx_nav_svinfo_part1.numCh);
|
||||
}
|
||||
if (_rx_payload_index < sizeof(ubx_payload_rx_nav_svinfo_part1_t) + _satellite_info->count * sizeof(ubx_payload_rx_nav_svinfo_part2_t)) {
|
||||
|
||||
if (_rx_payload_index < sizeof(ubx_payload_rx_nav_svinfo_part1_t) + _satellite_info->count * sizeof(
|
||||
ubx_payload_rx_nav_svinfo_part2_t)) {
|
||||
// Still room in _satellite_info: fill Part 2 buffer
|
||||
unsigned buf_index = (_rx_payload_index - sizeof(ubx_payload_rx_nav_svinfo_part1_t)) % sizeof(ubx_payload_rx_nav_svinfo_part2_t);
|
||||
unsigned buf_index = (_rx_payload_index - sizeof(ubx_payload_rx_nav_svinfo_part1_t)) % sizeof(
|
||||
ubx_payload_rx_nav_svinfo_part2_t);
|
||||
_buf.raw[buf_index] = b;
|
||||
|
||||
if (buf_index == sizeof(ubx_payload_rx_nav_svinfo_part2_t) - 1) {
|
||||
// Part 2 complete: decode Part 2 buffer
|
||||
unsigned sat_index = (_rx_payload_index - sizeof(ubx_payload_rx_nav_svinfo_part1_t)) / sizeof(ubx_payload_rx_nav_svinfo_part2_t);
|
||||
unsigned sat_index = (_rx_payload_index - sizeof(ubx_payload_rx_nav_svinfo_part1_t)) / sizeof(
|
||||
ubx_payload_rx_nav_svinfo_part2_t);
|
||||
_satellite_info->used[sat_index] = (uint8_t)(_buf.payload_rx_nav_svinfo_part2.flags & 0x01);
|
||||
_satellite_info->snr[sat_index] = (uint8_t)(_buf.payload_rx_nav_svinfo_part2.cno);
|
||||
_satellite_info->elevation[sat_index] = (uint8_t)(_buf.payload_rx_nav_svinfo_part2.elev);
|
||||
_satellite_info->azimuth[sat_index] = (uint8_t)((float)_buf.payload_rx_nav_svinfo_part2.azim * 255.0f / 360.0f);
|
||||
_satellite_info->svid[sat_index] = (uint8_t)(_buf.payload_rx_nav_svinfo_part2.svid);
|
||||
UBX_TRACE_SVINFO("SVINFO #%02u used %u snr %3u elevation %3u azimuth %3u svid %3u\n",
|
||||
(unsigned)sat_index + 1,
|
||||
(unsigned)_satellite_info->used[sat_index],
|
||||
(unsigned)_satellite_info->snr[sat_index],
|
||||
(unsigned)_satellite_info->elevation[sat_index],
|
||||
(unsigned)_satellite_info->azimuth[sat_index],
|
||||
(unsigned)_satellite_info->svid[sat_index]
|
||||
);
|
||||
(unsigned)sat_index + 1,
|
||||
(unsigned)_satellite_info->used[sat_index],
|
||||
(unsigned)_satellite_info->snr[sat_index],
|
||||
(unsigned)_satellite_info->elevation[sat_index],
|
||||
(unsigned)_satellite_info->azimuth[sat_index],
|
||||
(unsigned)_satellite_info->svid[sat_index]
|
||||
);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -672,6 +740,7 @@ UBX::payload_rx_add_mon_ver(const uint8_t b)
|
||||
if (_rx_payload_index < sizeof(ubx_payload_rx_mon_ver_part1_t)) {
|
||||
// Fill Part 1 buffer
|
||||
_buf.raw[_rx_payload_index] = b;
|
||||
|
||||
} else {
|
||||
if (_rx_payload_index == sizeof(ubx_payload_rx_mon_ver_part1_t)) {
|
||||
// Part 1 complete: decode Part 1 buffer and calculate hash for SW&HW version strings
|
||||
@ -681,9 +750,12 @@ UBX::payload_rx_add_mon_ver(const uint8_t b)
|
||||
UBX_DEBUG("VER hw \"%10s\"", _buf.payload_rx_mon_ver_part1.hwVersion);
|
||||
UBX_DEBUG("VER sw \"%30s\"", _buf.payload_rx_mon_ver_part1.swVersion);
|
||||
}
|
||||
|
||||
// fill Part 2 buffer
|
||||
unsigned buf_index = (_rx_payload_index - sizeof(ubx_payload_rx_mon_ver_part1_t)) % sizeof(ubx_payload_rx_mon_ver_part2_t);
|
||||
unsigned buf_index = (_rx_payload_index - sizeof(ubx_payload_rx_mon_ver_part1_t)) % sizeof(
|
||||
ubx_payload_rx_mon_ver_part2_t);
|
||||
_buf.raw[buf_index] = b;
|
||||
|
||||
if (buf_index == sizeof(ubx_payload_rx_mon_ver_part2_t) - 1) {
|
||||
// Part 2 complete: decode Part 2 buffer
|
||||
UBX_DEBUG("VER ext \" %30s\"", _buf.payload_rx_mon_ver_part2.extension);
|
||||
@ -717,13 +789,11 @@ UBX::payload_rx_done(void)
|
||||
UBX_TRACE_RXMSG("Rx NAV-PVT\n");
|
||||
|
||||
//Check if position fix flag is good
|
||||
if ((_buf.payload_rx_nav_pvt.flags & UBX_RX_NAV_PVT_FLAGS_GNSSFIXOK) == 1)
|
||||
{
|
||||
if ((_buf.payload_rx_nav_pvt.flags & UBX_RX_NAV_PVT_FLAGS_GNSSFIXOK) == 1) {
|
||||
_gps_position->fix_type = _buf.payload_rx_nav_pvt.fixType;
|
||||
_gps_position->vel_ned_valid = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
|
||||
} else {
|
||||
_gps_position->fix_type = 0;
|
||||
_gps_position->vel_ned_valid = false;
|
||||
}
|
||||
@ -748,10 +818,9 @@ UBX::payload_rx_done(void)
|
||||
_gps_position->c_variance_rad = (float)_buf.payload_rx_nav_pvt.headAcc * M_DEG_TO_RAD_F * 1e-5f;
|
||||
|
||||
//Check if time and date fix flags are good
|
||||
if( (_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_VALIDDATE)
|
||||
&& (_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_VALIDTIME)
|
||||
&& (_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_FULLYRESOLVED))
|
||||
{
|
||||
if ((_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_VALIDDATE)
|
||||
&& (_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_VALIDTIME)
|
||||
&& (_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_FULLYRESOLVED)) {
|
||||
/* convert to unix timestamp */
|
||||
struct tm timeinfo;
|
||||
timeinfo.tm_year = _buf.payload_rx_nav_pvt.year - 1900;
|
||||
@ -770,12 +839,14 @@ UBX::payload_rx_done(void)
|
||||
timespec ts;
|
||||
ts.tv_sec = epoch;
|
||||
ts.tv_nsec = _buf.payload_rx_nav_pvt.nano;
|
||||
|
||||
if (clock_settime(CLOCK_REALTIME, &ts)) {
|
||||
warn("failed setting clock");
|
||||
}
|
||||
|
||||
_gps_position->time_utc_usec = static_cast<uint64_t>(epoch) * 1000000ULL;
|
||||
_gps_position->time_utc_usec += _buf.payload_rx_nav_timeutc.nano / 1000;
|
||||
|
||||
} else {
|
||||
_gps_position->time_utc_usec = 0;
|
||||
}
|
||||
@ -827,8 +898,7 @@ UBX::payload_rx_done(void)
|
||||
case UBX_MSG_NAV_TIMEUTC:
|
||||
UBX_TRACE_RXMSG("Rx NAV-TIMEUTC\n");
|
||||
|
||||
if(_buf.payload_rx_nav_timeutc.valid & UBX_RX_NAV_TIMEUTC_VALID_VALIDUTC)
|
||||
{
|
||||
if (_buf.payload_rx_nav_timeutc.valid & UBX_RX_NAV_TIMEUTC_VALID_VALIDUTC) {
|
||||
// convert to unix timestamp
|
||||
struct tm timeinfo;
|
||||
timeinfo.tm_year = _buf.payload_rx_nav_timeutc.year - 1900;
|
||||
@ -849,12 +919,14 @@ UBX::payload_rx_done(void)
|
||||
timespec ts;
|
||||
ts.tv_sec = epoch;
|
||||
ts.tv_nsec = _buf.payload_rx_nav_timeutc.nano;
|
||||
|
||||
if (clock_settime(CLOCK_REALTIME, &ts)) {
|
||||
warn("failed setting clock");
|
||||
}
|
||||
|
||||
_gps_position->time_utc_usec = static_cast<uint64_t>(epoch) * 1000000ULL;
|
||||
_gps_position->time_utc_usec += _buf.payload_rx_nav_timeutc.nano / 1000;
|
||||
|
||||
} else {
|
||||
_gps_position->time_utc_usec = 0;
|
||||
}
|
||||
@ -922,6 +994,7 @@ UBX::payload_rx_done(void)
|
||||
ret = 0; // don't handle message
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case UBX_MSG_ACK_ACK:
|
||||
@ -999,39 +1072,44 @@ UBX::send_message(const uint16_t msg, const uint8_t *payload, const uint16_t len
|
||||
header.length = length;
|
||||
|
||||
// Calculate checksum
|
||||
calc_checksum(((uint8_t*)&header) + 2, sizeof(header) - 2, &checksum); // skip 2 sync bytes
|
||||
if (payload != nullptr)
|
||||
calc_checksum(((uint8_t *)&header) + 2, sizeof(header) - 2, &checksum); // skip 2 sync bytes
|
||||
|
||||
if (payload != nullptr) {
|
||||
calc_checksum(payload, length, &checksum);
|
||||
}
|
||||
|
||||
// Send message
|
||||
write(_fd, (const void *)&header, sizeof(header));
|
||||
if (payload != nullptr)
|
||||
|
||||
if (payload != nullptr) {
|
||||
write(_fd, (const void *)payload, length);
|
||||
}
|
||||
|
||||
write(_fd, (const void *)&checksum, sizeof(checksum));
|
||||
}
|
||||
|
||||
uint32_t
|
||||
UBX::fnv1_32_str(uint8_t *str, uint32_t hval)
|
||||
{
|
||||
uint8_t *s = str;
|
||||
uint8_t *s = str;
|
||||
|
||||
/*
|
||||
* FNV-1 hash each octet in the buffer
|
||||
*/
|
||||
while (*s) {
|
||||
/*
|
||||
* FNV-1 hash each octet in the buffer
|
||||
*/
|
||||
while (*s) {
|
||||
|
||||
/* multiply by the 32 bit FNV magic prime mod 2^32 */
|
||||
/* multiply by the 32 bit FNV magic prime mod 2^32 */
|
||||
#if defined(NO_FNV_GCC_OPTIMIZATION)
|
||||
hval *= FNV1_32_PRIME;
|
||||
hval *= FNV1_32_PRIME;
|
||||
#else
|
||||
hval += (hval<<1) + (hval<<4) + (hval<<7) + (hval<<8) + (hval<<24);
|
||||
hval += (hval << 1) + (hval << 4) + (hval << 7) + (hval << 8) + (hval << 24);
|
||||
#endif
|
||||
|
||||
/* xor the bottom with the current octet */
|
||||
hval ^= (uint32_t)*s++;
|
||||
}
|
||||
/* xor the bottom with the current octet */
|
||||
hval ^= (uint32_t) * s++;
|
||||
}
|
||||
|
||||
/* return our new hash value */
|
||||
return hval;
|
||||
/* return our new hash value */
|
||||
return hval;
|
||||
}
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user