Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-03 04:03:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: added Ublox' hardware generation autodetection

Browse Source
This commit is contained in:
Staroselskii Georgii 2015-04-01 14:49:34 +03:00 committed by Andrew Tridgell
parent baf2b4da4e
commit 47a336d192
2 changed files with 55 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -23,6 +23,8 @@
#include "AP_GPS_UBLOX.h" #include "AP_GPS_UBLOX.h"
#include <DataFlash.h> #include <DataFlash.h>
#define UBLOX_VERSION_AUTODETECTION 1
#define UBLOX_DEBUGGING 0 #define UBLOX_DEBUGGING 0
#define UBLOX_FAKE_3DLOCK 0 #define UBLOX_FAKE_3DLOCK 0
@ -109,6 +111,11 @@ AP_GPS_UBLOX::send_next_rate_update(void)
// gather MON_HW2 at 0.5Hz // gather MON_HW2 at 0.5Hz
_configure_message_rate(CLASS_MON, MSG_MON_HW2, 2); // 24+8 bytes _configure_message_rate(CLASS_MON, MSG_MON_HW2, 2); // 24+8 bytes
break; break;
#endif
#if UBLOX_VERSION_AUTODETECTION
case 7:
_request_version();
break;
#endif #endif
default: default:
need_rate_update = false; need_rate_update = false;
@ -485,6 +492,31 @@ AP_GPS_UBLOX::_parse_gps(void)
state.speed_accuracy = _buffer.velned.speed_accuracy*0.01f; state.speed_accuracy = _buffer.velned.speed_accuracy*0.01f;
_new_speed = true; _new_speed = true;
break; break;
#if UBLOX_VERSION_AUTODETECTION
case MSG_NAV_SVINFO:
{
Debug("MSG_NAV_SVINFO\n");
static const uint8_t HardwareGenerationMask = 0x07;
uint8_t hardware_generation = _buffer.svinfo_header.globalFlags & HardwareGenerationMask;
switch (hardware_generation) {
case UBLOX_5:
case UBLOX_6:
/*speed already configured */;
break;
case UBLOX_7:
case UBLOX_M8:
port->begin(4000000U);
Debug("Changed speed to 5Mhzfor SPI-driven UBlox\n");
break;
default:
hal.console->printf("Wrong Ublox' Hardware Version%u\n", hardware_generation);
break;
};
/* We don't need that anymore */
_configure_message_rate(CLASS_NAV, MSG_NAV_SVINFO, 0);
break;
}
#endif
default: default:
Debug("Unexpected NAV message 0x%02x", (unsigned)_msg_id); Debug("Unexpected NAV message 0x%02x", (unsigned)_msg_id);
if (++_disable_counter == 0) { if (++_disable_counter == 0) {
@ -676,3 +708,9 @@ reset:
} }
return false; return false;
} }
void
AP_GPS_UBLOX::_request_version(void)
{
_configure_message_rate(CLASS_NAV, MSG_NAV_SVINFO, 2);
}

18
libraries/AP_GPS/AP_GPS_UBLOX.h
View File

@ -194,6 +194,12 @@ private:
uint32_t postStatus; uint32_t postStatus;
uint32_t reserved2; uint32_t reserved2;
}; };
struct PACKED ubx_nav_svinfo_header {
uint32_t itow;
uint8_t numCh;
uint8_t globalFlags;
uint16_t reserved;
};
// Receive buffer // Receive buffer
union PACKED { union PACKED {
ubx_nav_posllh posllh; ubx_nav_posllh posllh;
@ -205,6 +211,7 @@ private:
ubx_mon_hw_68 mon_hw_68; ubx_mon_hw_68 mon_hw_68;
ubx_mon_hw2 mon_hw2; ubx_mon_hw2 mon_hw2;
ubx_cfg_sbas sbas; ubx_cfg_sbas sbas;
ubx_nav_svinfo_header svinfo_header;
uint8_t bytes[]; uint8_t bytes[];
} _buffer; } _buffer;
@ -227,7 +234,8 @@ private:
MSG_CFG_NAV_SETTINGS = 0x24, MSG_CFG_NAV_SETTINGS = 0x24,
MSG_CFG_SBAS = 0x16, MSG_CFG_SBAS = 0x16,
MSG_MON_HW = 0x09, MSG_MON_HW = 0x09,
MSG_MON_HW2 = 0x0B MSG_MON_HW2 = 0x0B,
MSG_NAV_SVINFO = 0x30
}; };
enum ubs_nav_fix_type { enum ubs_nav_fix_type {
FIX_NONE = 0, FIX_NONE = 0,
@ -240,6 +248,13 @@ private:
enum ubx_nav_status_bits { enum ubx_nav_status_bits {
NAV_STATUS_FIX_VALID = 1 NAV_STATUS_FIX_VALID = 1
}; };
enum ubx_hardware_version {
ANTARIS = 0,
UBLOX_5,
UBLOX_6,
UBLOX_7,
UBLOX_M8
};
// Packet checksum accumulators // Packet checksum accumulators
uint8_t _ck_a; uint8_t _ck_a;
@ -284,6 +299,7 @@ private:
void _update_checksum(uint8_t *data, uint8_t len, uint8_t &ck_a, uint8_t &ck_b); void _update_checksum(uint8_t *data, uint8_t len, uint8_t &ck_a, uint8_t &ck_b);
void _send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size); void _send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size);
void send_next_rate_update(void); void send_next_rate_update(void);
void _request_version(void);
void unexpected_message(void); void unexpected_message(void);
void write_logging_headers(void); void write_logging_headers(void);