forked from Archive/PX4-Autopilot
bugfixes in UBX driver, emit new SNR, Jamming and noise count indices
This commit is contained in:
parent
7bf1f82f61
commit
c4d79b84b4
|
@ -448,10 +448,10 @@ 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: %fms ago", (int)_report.fix_type,
|
||||
_report.satellites_visible, (hrt_absolute_time() - _report.timestamp_position) / 1000.0f);
|
||||
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.0f);
|
||||
warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt);
|
||||
warnx("eph: %.2fm, epv: %.2fm", _report.eph_m, _report.epv_m);
|
||||
warnx("eph: %.2fm, epv: %.2fm", (double)_report.eph_m, (double)_report.epv_m);
|
||||
warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate());
|
||||
warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate());
|
||||
warnx("rate publication:\t%6.2f Hz", (double)_rate);
|
||||
|
|
|
@ -56,7 +56,7 @@ GPS_Helper::get_velocity_update_rate()
|
|||
return _rate_vel;
|
||||
}
|
||||
|
||||
float
|
||||
void
|
||||
GPS_Helper::reset_update_rates()
|
||||
{
|
||||
_rate_count_vel = 0;
|
||||
|
@ -64,7 +64,7 @@ GPS_Helper::reset_update_rates()
|
|||
_interval_rate_start = hrt_absolute_time();
|
||||
}
|
||||
|
||||
float
|
||||
void
|
||||
GPS_Helper::store_update_rates()
|
||||
{
|
||||
_rate_vel = _rate_count_vel / (((float)(hrt_absolute_time() - _interval_rate_start)) / 1000000.0f);
|
||||
|
|
|
@ -46,13 +46,17 @@
|
|||
class GPS_Helper
|
||||
{
|
||||
public:
|
||||
|
||||
GPS_Helper() {};
|
||||
virtual ~GPS_Helper() {};
|
||||
|
||||
virtual int configure(unsigned &baud) = 0;
|
||||
virtual int receive(unsigned timeout) = 0;
|
||||
int set_baudrate(const int &fd, unsigned baud);
|
||||
float get_position_update_rate();
|
||||
float get_velocity_update_rate();
|
||||
float reset_update_rates();
|
||||
float store_update_rates();
|
||||
void reset_update_rates();
|
||||
void store_update_rates();
|
||||
|
||||
protected:
|
||||
uint8_t _rate_count_lat_lon;
|
||||
|
|
|
@ -226,6 +226,13 @@ UBX::configure(unsigned &baudrate)
|
|||
return 1;
|
||||
}
|
||||
|
||||
configure_message_rate(UBX_CLASS_MON, UBX_MESSAGE_MON_HW, 1);
|
||||
|
||||
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
|
||||
warnx("MSG CFG FAIL: MON HW");
|
||||
return 1;
|
||||
}
|
||||
|
||||
_configured = true;
|
||||
return 0;
|
||||
}
|
||||
|
@ -566,6 +573,24 @@ UBX::handle_message()
|
|||
break;
|
||||
}
|
||||
|
||||
case UBX_CLASS_MON: {
|
||||
switch (_message_id) {
|
||||
case UBX_MESSAGE_MON_HW: {
|
||||
|
||||
struct gps_bin_mon_hw_packet *p = (struct gps_bin_mon_hw_packet*) _rx_buffer;
|
||||
|
||||
_gps_position->noise_per_ms = p->noisePerMS;
|
||||
_gps_position->jamming_indicator = p->jamInd;
|
||||
|
||||
ret = 1;
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
|
|
@ -56,6 +56,7 @@
|
|||
//#define UBX_CLASS_RXM 0x02
|
||||
#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
|
||||
|
@ -72,6 +73,8 @@
|
|||
#define UBX_MESSAGE_CFG_RATE 0x08
|
||||
#define UBX_MESSAGE_CFG_NAV5 0x24
|
||||
|
||||
#define UBX_MESSAGE_MON_HW 0x09
|
||||
|
||||
#define UBX_CFG_PRT_LENGTH 20
|
||||
#define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< UART1 */
|
||||
#define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */
|
||||
|
@ -210,6 +213,27 @@ typedef struct {
|
|||
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 */
|
||||
|
@ -319,7 +343,7 @@ typedef enum {
|
|||
//typedef type_gps_bin_ubx_state gps_bin_ubx_state_t;
|
||||
#pragma pack(pop)
|
||||
|
||||
#define RECV_BUFFER_SIZE 500 //The NAV-SOL messages really need such a big buffer
|
||||
#define RECV_BUFFER_SIZE 300 //The NAV-SOL messages really need such a big buffer
|
||||
|
||||
class UBX : public GPS_Helper
|
||||
{
|
||||
|
@ -383,7 +407,7 @@ private:
|
|||
uint8_t _message_class;
|
||||
uint8_t _message_id;
|
||||
unsigned _payload_size;
|
||||
uint8_t _disable_cmd_last;
|
||||
hrt_abstime _disable_cmd_last;
|
||||
};
|
||||
|
||||
#endif /* UBX_H_ */
|
||||
|
|
Loading…
Reference in New Issue