Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_GPS_UBLOX: add pvt message support

This commit is contained in:
Michael Oborne 2016-07-31 07:38:43 +08:00 committed by Francisco Ferreira
parent fc6ab3e29e
commit d05b0d5885
3 changed files with 209 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

122
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -65,7 +65,8 @@ AP_GPS_UBLOX::AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UART
_disable_counter(0),
next_fix(AP_GPS::NO_FIX),
_cfg_needs_save(false),
noReceivedHdop(true)
noReceivedHdop(true),
havePvtMsg(false)
{
// stop any config strings that are pending
gps.send_blob_start(state.instance, nullptr, 0);
@ -95,12 +96,12 @@ AP_GPS_UBLOX::_request_next_config(void)
_configure_rate();
break;
case STEP_RATE_POSLLH:
if(!_configure_message_rate(CLASS_NAV, MSG_POSLLH, RATE_POSLLH)) {
if(!havePvtMsg && !_configure_message_rate(CLASS_NAV, MSG_POSLLH, RATE_POSLLH)) {
_next_message--;
}
break;
case STEP_RATE_VELNED:
if(!_configure_message_rate(CLASS_NAV, MSG_VELNED, RATE_VELNED)) {
if(!havePvtMsg && !_configure_message_rate(CLASS_NAV, MSG_VELNED, RATE_VELNED)) {
_next_message--;
}
break;
@ -130,12 +131,12 @@ AP_GPS_UBLOX::_request_next_config(void)
_send_message(CLASS_CFG, MSG_CFG_RATE, nullptr, 0);
break;
case STEP_POSLLH:
if(!_request_message_rate(CLASS_NAV, MSG_POSLLH)) {
if(!havePvtMsg && !_request_message_rate(CLASS_NAV, MSG_POSLLH)) {
_next_message--;
}
break;
case STEP_STATUS:
if(!_request_message_rate(CLASS_NAV, MSG_STATUS)) {
if(!havePvtMsg && !_request_message_rate(CLASS_NAV, MSG_STATUS)) {
_next_message--;
}
break;
@ -144,8 +145,13 @@ AP_GPS_UBLOX::_request_next_config(void)
_next_message--;
}
break;
case STEP_PVT:
if(!_request_message_rate(CLASS_NAV, MSG_PVT)) {
_next_message--;
}
break;
case STEP_VELNED:
if(!_request_message_rate(CLASS_NAV, MSG_VELNED)) {
if(!havePvtMsg && !_request_message_rate(CLASS_NAV, MSG_VELNED)) {
_next_message--;
}
break;
@ -204,6 +210,7 @@ AP_GPS_UBLOX::_request_next_config(void)
void
AP_GPS_UBLOX::_verify_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate) {
Debug("class=%u id=%u rate=%u",msg_class,msg_id,rate);
switch(msg_class) {
case CLASS_NAV:
switch(msg_id) {
@ -234,6 +241,15 @@ AP_GPS_UBLOX::_verify_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate) {
_cfg_needs_save = true;
}
break;
case MSG_PVT:
if(rate == RATE_PVT) {
_unconfigured_messages &= ~CONFIG_RATE_PVT;
} else {
_configure_message_rate(msg_class, msg_id, RATE_PVT);
_unconfigured_messages |= CONFIG_RATE_PVT;
_cfg_needs_save = true;
}
break;
case MSG_VELNED:
if(rate == RATE_VELNED) {
_unconfigured_messages &= ~CONFIG_RATE_VELNED;
@ -342,7 +358,7 @@ AP_GPS_UBLOX::read(void)
_delay_time = 750;
}
} else {
_delay_time = 2000;
_delay_time = 4000;
}
}
@ -823,6 +839,10 @@ AP_GPS_UBLOX::_parse_gps(void)
switch (_msg_id) {
case MSG_POSLLH:
Debug("MSG_POSLLH next_fix=%u", next_fix);
if (havePvtMsg) {
_configure_message_rate(_class, _msg_id, 0);
break;
}
_last_pos_time = _buffer.posllh.time;
state.location.lng = _buffer.posllh.longitude;
state.location.lat = _buffer.posllh.latitude;
@ -845,6 +865,10 @@ AP_GPS_UBLOX::_parse_gps(void)
Debug("MSG_STATUS fix_status=%u fix_type=%u",
_buffer.status.fix_status,
_buffer.status.fix_type);
if (havePvtMsg) {
_configure_message_rate(_class, _msg_id, 0);
break;
}
if (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) {
if( (_buffer.status.fix_type == AP_GPS_UBLOX::FIX_3D) &&
(_buffer.status.fix_status & AP_GPS_UBLOX::NAV_STATUS_DGPS_USED)) {
@ -880,6 +904,10 @@ AP_GPS_UBLOX::_parse_gps(void)
Debug("MSG_SOL fix_status=%u fix_type=%u",
_buffer.solution.fix_status,
_buffer.solution.fix_type);
if (havePvtMsg) {
state.time_week = _buffer.solution.week;
break;
}
if (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) {
if( (_buffer.solution.fix_type == AP_GPS_UBLOX::FIX_3D) &&
(_buffer.solution.fix_status & AP_GPS_UBLOX::NAV_STATUS_DGPS_USED)) {
@ -914,8 +942,88 @@ AP_GPS_UBLOX::_parse_gps(void)
state.hdop = 130;
#endif
break;
case MSG_PVT:
Debug("MSG_PVT");
havePvtMsg = true;
//unsubscribe to these messages
_unconfigured_messages &= ~CONFIG_RATE_POSLLH;
_unconfigured_messages &= ~CONFIG_RATE_VELNED;
_unconfigured_messages &= ~CONFIG_RATE_STATUS;
// position
_last_pos_time = _buffer.pvt.itow;
state.location.lng = _buffer.pvt.lon;
state.location.lat = _buffer.pvt.lat;
state.location.alt = _buffer.pvt.h_msl / 10;
switch (_buffer.pvt.fix_type)
{
case 0:
state.status = AP_GPS::NO_FIX;
break;
case 1:
state.status = AP_GPS::NO_FIX;
break;
case 2:
state.status = AP_GPS::GPS_OK_FIX_2D;
break;
case 3:
state.status = AP_GPS::GPS_OK_FIX_3D;
if (_buffer.pvt.flags & 0b00000010) // diffsoln
state.status = AP_GPS::GPS_OK_FIX_3D_DGPS;
if (_buffer.pvt.flags & 0b01000000) // carrsoln - float
state.status = AP_GPS::GPS_OK_FIX_3D_RTK;
if (_buffer.pvt.flags & 0b10000000) // carrsoln - fixed
state.status = AP_GPS::GPS_OK_FIX_3D_RTK;
break;
case 4:
state.status = AP_GPS::GPS_OK_FIX_3D;
break;
case 5:
state.status = AP_GPS::NO_FIX;
break;
default:
state.status = AP_GPS::NO_FIX;
break;
}
next_fix = state.status;
_new_position = true;
state.horizontal_accuracy = _buffer.pvt.h_acc*1.0e-3f;
state.vertical_accuracy = _buffer.pvt.v_acc*1.0e-3f;
state.have_horizontal_accuracy = true;
state.have_vertical_accuracy = true;
// SVs
state.num_sats = _buffer.pvt.num_sv;
// velocity
_last_vel_time = _buffer.pvt.itow;
state.ground_speed = _buffer.pvt.gspeed*0.001f; // m/s
state.ground_course = wrap_360(_buffer.pvt.head_mot * 1.0e-5f); // Heading 2D deg * 100000
state.have_vertical_velocity = true;
state.velocity.x = _buffer.pvt.velN * 0.001f;
state.velocity.y = _buffer.pvt.velE * 0.001f;
state.velocity.z = _buffer.pvt.velD * 0.001f;
state.have_speed_accuracy = true;
state.speed_accuracy = _buffer.pvt.s_acc*0.001f;
_new_speed = true;
// dop
if(noReceivedHdop) {
state.hdop = _buffer.pvt.p_dop;
state.vdop = _buffer.pvt.p_dop;
}
state.last_gps_time_ms = AP_HAL::millis();
if (state.time_week_ms + 220 > _buffer.pvt.itow) {
// we got a 5Hz update.
_last_5hz_time = state.last_gps_time_ms;
}
// time
state.time_week_ms = _buffer.pvt.itow;
break;
case MSG_VELNED:
Debug("MSG_VELNED");
if (havePvtMsg) {
_configure_message_rate(_class, _msg_id, 0);
break;
}
_last_vel_time = _buffer.velned.time;
state.ground_speed = _buffer.velned.speed_2d*0.01f; // m/s
state.ground_course = wrap_360(_buffer.velned.heading_2d * 1.0e-5f); // Heading 2D deg * 100000

50
libraries/AP_GPS/AP_GPS_UBLOX.h
View File

@ -53,6 +53,7 @@
#define RATE_POSLLH 1
#define RATE_STATUS 1
#define RATE_SOL 1
#define RATE_PVT 1
#define RATE_VELNED 1
#define RATE_DOP 1
#define RATE_HW 5
@ -71,10 +72,11 @@
#define CONFIG_NAV_SETTINGS (1<<10)
#define CONFIG_GNSS (1<<11)
#define CONFIG_SBAS (1<<12)
#define CONFIG_RATE_PVT (1<<13)
#define CONFIG_ALL (CONFIG_RATE_NAV | CONFIG_RATE_POSLLH | CONFIG_RATE_STATUS | CONFIG_RATE_SOL | CONFIG_RATE_VELNED \
| CONFIG_RATE_DOP | CONFIG_RATE_MON_HW | CONFIG_RATE_MON_HW2 | CONFIG_RATE_RAW | CONFIG_VERSION \
| CONFIG_NAV_SETTINGS | CONFIG_GNSS | CONFIG_SBAS)
| CONFIG_NAV_SETTINGS | CONFIG_GNSS | CONFIG_SBAS | CONFIG_RATE_PVT)
//Configuration Sub-Sections
#define SAVE_CFG_IO (1<<0)
@ -94,7 +96,7 @@ public:
// Methods
bool read();
AP_GPS::GPS_Status highest_supported_status(void) { return AP_GPS::GPS_OK_FIX_3D_DGPS; }
AP_GPS::GPS_Status highest_supported_status(void) { return AP_GPS::GPS_OK_FIX_3D_RTK; }
static bool _detect(struct UBLOX_detect_state &state, uint8_t data);
@ -227,6 +229,29 @@ private:
uint8_t satellites;
uint32_t res2;
};
struct PACKED ubx_nav_pvt {
uint32_t itow;
uint16_t year;
uint8_t month, day, hour, min, sec;
uint8_t valid;
uint32_t t_acc;
int32_t nano;
uint8_t fix_type;
uint8_t flags;
uint8_t flags2;
uint8_t num_sv;
int32_t lon, lat;
int32_t height, h_msl;
uint32_t h_acc, v_acc;
int32_t velN, velE, velD, gspeed;
int32_t head_mot;
uint32_t s_acc;
uint32_t head_acc;
uint16_t p_dop;
uint8_t reserved1[6];
uint32_t headVeh;
uint8_t reserved2[4];
};
struct PACKED ubx_nav_velned {
uint32_t time; // GPS msToW
int32_t ned_north;
@ -363,6 +388,7 @@ private:
ubx_nav_status status;
ubx_nav_dop dop;
ubx_nav_solution solution;
ubx_nav_pvt pvt;
ubx_nav_velned velned;
ubx_cfg_msg_rate msg_rate;
ubx_cfg_msg_rate_6 msg_rate_6;
@ -399,6 +425,7 @@ private:
MSG_STATUS = 0x3,
MSG_DOP = 0x4,
MSG_SOL = 0x6,
MSG_PVT = 0x7,
MSG_VELNED = 0x12,
MSG_CFG_CFG = 0x09,
MSG_CFG_RATE = 0x08,
@ -445,17 +472,18 @@ private:
enum config_step {
STEP_RATE_NAV = 0,
STEP_RATE_POSLLH,
STEP_RATE_VELNED,
STEP_PORT,
STEP_POLL_SVINFO,
STEP_POLL_SBAS,
STEP_POLL_NAV,
STEP_POLL_GNSS,
STEP_NAV_RATE,
STEP_SOL,
STEP_PVT,
STEP_POLL_SVINFO, // poll svinfo
STEP_POLL_SBAS, // poll SBAS
STEP_POLL_NAV, // poll NAV settings
STEP_POLL_GNSS, // poll GNSS
STEP_NAV_RATE, // poll NAV rate
STEP_RATE_POSLLH,
STEP_RATE_VELNED,
STEP_POSLLH,
STEP_STATUS,
STEP_SOL,
STEP_VELNED,
STEP_DOP,
STEP_MON_HW,
@ -508,6 +536,8 @@ private:
bool _cfg_needs_save;
bool noReceivedHdop;
bool havePvtMsg;
bool _configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate);
void _configure_rate(void);

54
libraries/AP_HAL_SITL/sitl_gps.cpp
View File

@ -248,11 +248,35 @@ void SITL_State::_update_gps_ubx(const struct gps_data *d)
uint16_t nDOP;
uint16_t eDOP;
} dop;
struct PACKED ubx_nav_pvt {
uint32_t itow;
uint16_t year;
uint8_t month, day, hour, min, sec;
uint8_t valid;
uint32_t t_acc;
int32_t nano;
uint8_t fix_type;
uint8_t flags;
uint8_t flags2;
uint8_t num_sv;
int32_t lon, lat;
int32_t height, h_msl;
uint32_t h_acc, v_acc;
int32_t velN, velE, velD, gspeed;
int32_t head_mot;
uint32_t s_acc;
uint32_t head_acc;
uint16_t p_dop;
uint8_t reserved1[6];
uint32_t headVeh;
uint8_t reserved2[4];
} pvt;
const uint8_t MSG_POSLLH = 0x2;
const uint8_t MSG_STATUS = 0x3;
const uint8_t MSG_DOP = 0x4;
const uint8_t MSG_VELNED = 0x12;
const uint8_t MSG_SOL = 0x6;
const uint8_t MSG_PVT = 0x7;
uint16_t time_week;
uint32_t time_week_ms;
@ -302,12 +326,42 @@ void SITL_State::_update_gps_ubx(const struct gps_data *d)
dop.hDOP = 121;
dop.nDOP = 65535;
dop.eDOP = 65535;
pvt.itow = time_week_ms;
pvt.year = 0;
pvt.month = 0;
pvt.day = 0;
pvt.hour = 0;
pvt.min = 0;
pvt.sec = 0;
pvt.valid = 0; // invalid utc date
pvt.t_acc = 0;
pvt.nano = 0;
pvt.fix_type = 0x3;
pvt.flags = 0b10000011; // carrsoln=fixed, psm = na, diffsoln and fixok
pvt.flags2 =0;
pvt.num_sv = d->have_lock?_sitl->gps_numsats:3;
pvt.lon = d->longitude * 1.0e7;
pvt.lat = d->latitude * 1.0e7;
pvt.height = d->altitude*1000.0f;
pvt.h_msl = d->altitude*1000.0f;
pvt.h_acc = 200;
pvt.v_acc = 200;
pvt.velN = 1000.0f * d->speedN;
pvt.velE = 1000.0f * d->speedE;
pvt.velD = 1000.0f * d->speedD;
pvt.gspeed = norm(d->speedN, d->speedE) * 1000;
pvt.head_mot = ToDeg(atan2f(d->speedE, d->speedN)) * 1.0e5;
pvt.s_acc = 40;
pvt.head_acc = 38 * 1.0e5;
pvt.p_dop = 65535;
_gps_send_ubx(MSG_POSLLH, (uint8_t*)&pos, sizeof(pos));
_gps_send_ubx(MSG_STATUS, (uint8_t*)&status, sizeof(status));
_gps_send_ubx(MSG_VELNED, (uint8_t*)&velned, sizeof(velned));
_gps_send_ubx(MSG_SOL, (uint8_t*)&sol, sizeof(sol));
_gps_send_ubx(MSG_DOP, (uint8_t*)&dop, sizeof(dop));
_gps_send_ubx(MSG_PVT, (uint8_t*)&pvt, sizeof(pvt));
}
static void swap_uint32(uint32_t *v, uint8_t n)