Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 17:38:32 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: Use state.instance instead of trying to track instance numbers inside of the ublox driver

Browse Source
This commit is contained in:
Michael du Breuil 2015-09-09 05:08:58 -07:00
parent 5adb6d2b89
commit 75c0644b9a
2 changed files with 8 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -33,12 +33,8 @@
#define UBLOX_DEBUGGING 0 #define UBLOX_DEBUGGING 0
#define UBLOX_FAKE_3DLOCK 0 #define UBLOX_FAKE_3DLOCK 0
#define UBX_MAX_GPS_INSTANCES 2
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
uint8_t AP_GPS_UBLOX::ublox_instances = 0;
#if UBLOX_DEBUGGING #if UBLOX_DEBUGGING
# define Debug(fmt, args ...) do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0) # define Debug(fmt, args ...) do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0)
#else #else
@ -53,9 +49,6 @@ AP_GPS_UBLOX::AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UART
_payload_counter(0), _payload_counter(0),
_fix_count(0), _fix_count(0),
_class(0), _class(0),
_gdop(0),
_pdop(0),
_vdop(0),
_new_position(0), _new_position(0),
_new_speed(0), _new_speed(0),
need_rate_update(false), need_rate_update(false),
@ -65,8 +58,6 @@ AP_GPS_UBLOX::AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UART
_last_5hz_time(0), _last_5hz_time(0),
noReceivedHdop(true) noReceivedHdop(true)
{ {
ublox_instance = ublox_instances++;
// stop any config strings that are pending // stop any config strings that are pending
gps.send_blob_start(state.instance, NULL, 0); gps.send_blob_start(state.instance, NULL, 0);
@ -279,12 +270,11 @@ AP_GPS_UBLOX::read(void)
void AP_GPS_UBLOX::log_mon_hw(void) void AP_GPS_UBLOX::log_mon_hw(void)
{ {
if (gps._DataFlash == NULL || !gps._DataFlash->logging_started() if (gps._DataFlash == NULL || !gps._DataFlash->logging_started()) {
|| ublox_instance >= UBX_MAX_GPS_INSTANCES) {
return; return;
} }
struct log_Ubx1 pkt = { struct log_Ubx1 pkt = {
LOG_PACKET_HEADER_INIT(ubx_msg_index(LOG_GPS_UBX1_MSG)), LOG_PACKET_HEADER_INIT(_ubx_msg_log_index(LOG_GPS_UBX1_MSG)),
time_us : hal.scheduler->micros64(), time_us : hal.scheduler->micros64(),
instance : state.instance, instance : state.instance,
noisePerMS : _buffer.mon_hw_60.noisePerMS, noisePerMS : _buffer.mon_hw_60.noisePerMS,
@ -303,13 +293,12 @@ void AP_GPS_UBLOX::log_mon_hw(void)
void AP_GPS_UBLOX::log_mon_hw2(void) void AP_GPS_UBLOX::log_mon_hw2(void)
{ {
if (gps._DataFlash == NULL || !gps._DataFlash->logging_started() if (gps._DataFlash == NULL || !gps._DataFlash->logging_started()) {
|| ublox_instance >= UBX_MAX_GPS_INSTANCES) {
return; return;
} }
struct log_Ubx2 pkt = { struct log_Ubx2 pkt = {
LOG_PACKET_HEADER_INIT(ubx_msg_index(LOG_GPS_UBX2_MSG)), LOG_PACKET_HEADER_INIT(_ubx_msg_log_index(LOG_GPS_UBX2_MSG)),
time_us : hal.scheduler->micros64(), time_us : hal.scheduler->micros64(),
instance : state.instance, instance : state.instance,
ofsI : _buffer.mon_hw2.ofsI, ofsI : _buffer.mon_hw2.ofsI,
@ -582,9 +571,6 @@ AP_GPS_UBLOX::_parse_gps(void)
noReceivedHdop = false; noReceivedHdop = false;
state.hdop = _buffer.dop.hDOP; state.hdop = _buffer.dop.hDOP;
state.vdop = _buffer.dop.vDOP; state.vdop = _buffer.dop.vDOP;
_vdop = _buffer.dop.vDOP;
_pdop = _buffer.dop.pDOP;
_gdop = _buffer.dop.gDOP;
#if UBLOX_FAKE_3DLOCK #if UBLOX_FAKE_3DLOCK
state.hdop = 130; state.hdop = 130;
state.hdop = 170; state.hdop = 170;

16
libraries/AP_GPS/AP_GPS_UBLOX.h
View File

@ -54,7 +54,7 @@
#endif #endif
#define UBLOX_MAX_GNSS_CONFIG_BLOCKS 7 #define UBLOX_MAX_GNSS_CONFIG_BLOCKS 7
#define UBX_MSG_TYPES 3 #define UBX_MSG_TYPES 2
class AP_GPS_UBLOX : public AP_GPS_Backend class AP_GPS_UBLOX : public AP_GPS_Backend
{ {
@ -386,11 +386,6 @@ private:
uint32_t _last_vel_time; uint32_t _last_vel_time;
uint32_t _last_pos_time; uint32_t _last_pos_time;
// UBX specific dilution of precision
uint16_t _gdop;
uint16_t _pdop;
uint16_t _vdop;
// do we have new position information? // do we have new position information?
bool _new_position:1; bool _new_position:1;
// do we have new speed information? // do we have new speed information?
@ -409,9 +404,6 @@ private:
uint32_t _last_5hz_time; uint32_t _last_5hz_time;
bool noReceivedHdop = true; bool noReceivedHdop = true;
// number of active ublox instances used to determine UBX processing
uint8_t ublox_instance;
static uint8_t ublox_instances;
void _configure_navigation_rate(uint16_t rate_ms); void _configure_navigation_rate(uint16_t rate_ms);
void _configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate); void _configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate);
@ -429,9 +421,9 @@ private:
void log_rxm_raw(const struct ubx_rxm_raw &raw); void log_rxm_raw(const struct ubx_rxm_raw &raw);
void log_rxm_rawx(const struct ubx_rxm_rawx &raw); void log_rxm_rawx(const struct ubx_rxm_rawx &raw);
// Actual LogMessage index we are going to use // Calculates the correct log message ID based on what GPS instance is being logged
uint8_t ubx_msg_index(uint8_t ubx_msg) { uint8_t _ubx_msg_log_index(uint8_t ubx_msg) {
return (uint8_t)(ubx_msg + (ublox_instance * UBX_MSG_TYPES)); return (uint8_t)(ubx_msg + (state.instance * UBX_MSG_TYPES));
} }
}; };