bool
AP_GPS_SBP::_attempt_state_update()
{
// If we currently have heartbeats
// - NO FIX
//
// If we have a full update available, save it
//
uint32_t now = AP_HAL::millis();
bool ret = false;
if (now - last_heatbeat_received_ms > SBP_TIMEOUT_HEATBEAT) {
state.status = AP_GPS::NO_FIX;
Debug("No Heartbeats from Piksi! Driver Ready to Die!");
} else if (last_pos_llh_rtk.tow == last_vel_ned.tow
&& abs((int32_t) (last_gps_time.tow - last_vel_ned.tow)) < 10000
&& abs((int32_t) (last_dops.tow - last_vel_ned.tow)) < 60000
&& last_vel_ned.tow > last_full_update_tow) {
// Use the RTK position
sbp_pos_llh_t *pos_llh = &last_pos_llh_rtk;
// Update time state
state.time_week = last_gps_time.wn;
state.time_week_ms = last_vel_ned.tow;
state.hdop = last_dops.hdop;
// Update velocity state
state.velocity[0] = (float)(last_vel_ned.n * 1.0e-3);
state.velocity[1] = (float)(last_vel_ned.e * 1.0e-3);
state.velocity[2] = (float)(last_vel_ned.d * 1.0e-3);
state.have_vertical_velocity = true;
float ground_vector_sq = state.velocity[0]*state.velocity[0] + state.velocity[1]*state.velocity[1];
state.ground_speed = safe_sqrt(ground_vector_sq);
state.ground_course = wrap_360(degrees(atan2f(state.velocity[1], state.velocity[0])));
// Update position state
state.location.lat = (int32_t) (pos_llh->lat * (double)1e7);
state.location.lng = (int32_t) (pos_llh->lon * (double)1e7);
state.location.alt = (int32_t) (pos_llh->height * 100);
state.num_sats = pos_llh->n_sats;
if (pos_llh->flags == 0) {
state.status = AP_GPS::GPS_OK_FIX_3D;
} else if (pos_llh->flags == 2) {
state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FLOAT;
} else if (pos_llh->flags == 1) {
state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FIXED;
}
last_full_update_tow = last_vel_ned.tow;
last_full_update_cpu_ms = now;
logging_log_full_update();
ret = true;
} else if (now - last_full_update_cpu_ms > SBP_TIMEOUT_PVT) {
//INVARIANT: If we currently have a fix, ONLY return true after a full update.
state.status = AP_GPS::NO_FIX;
ret = true;
} else {
//No timeouts yet, no data yet, nothing has happened.
}
return ret;
}
bool
AP_GPS_SBF::process_message(void)
{
uint16_t blockid = (sbf_msg.blockid & 4095u);
Debug("BlockID %d", blockid);
// ExtEventPVTGeodetic
if (blockid == 4038) {
log_ExtEventPVTGeodetic(sbf_msg.data.msg4007u);
}
// PVTGeodetic
if (blockid == 4007) {
const msg4007 &temp = sbf_msg.data.msg4007u;
// Update time state
if (temp.WNc != 65535) {
state.time_week = temp.WNc;
state.time_week_ms = (uint32_t)(temp.TOW);
}
state.last_gps_time_ms = AP_HAL::millis();
state.hdop = last_hdop;
// Update velocity state (don't use −2·10^10)
if (temp.Vn > -200000) {
state.velocity.x = (float)(temp.Vn);
state.velocity.y = (float)(temp.Ve);
state.velocity.z = (float)(-temp.Vu);
state.have_vertical_velocity = true;
float ground_vector_sq = state.velocity[0] * state.velocity[0] + state.velocity[1] * state.velocity[1];
state.ground_speed = (float)safe_sqrt(ground_vector_sq);
state.ground_course = wrap_360(degrees(atan2f(state.velocity[1], state.velocity[0])));
state.horizontal_accuracy = (float)temp.HAccuracy * 0.01f;
state.vertical_accuracy = (float)temp.VAccuracy * 0.01f;
state.have_horizontal_accuracy = true;
state.have_vertical_accuracy = true;
}
// Update position state (don't use −2·10^10)
if (temp.Latitude > -200000) {
state.location.lat = (int32_t)(temp.Latitude * RAD_TO_DEG_DOUBLE * 1e7);
state.location.lng = (int32_t)(temp.Longitude * RAD_TO_DEG_DOUBLE * 1e7);
state.location.alt = (int32_t)((float)temp.Height * 1e2f);
}
if (temp.NrSV != 255) {
state.num_sats = temp.NrSV;
}
Debug("temp.Mode=0x%02x\n", (unsigned)temp.Mode);
switch (temp.Mode & 15) {
case 0: // no pvt
state.status = AP_GPS::NO_FIX;
break;
case 1: // standalone
state.status = AP_GPS::GPS_OK_FIX_3D;
break;
case 2: // dgps
state.status = AP_GPS::GPS_OK_FIX_3D_DGPS;
break;
case 3: // fixed location
state.status = AP_GPS::GPS_OK_FIX_3D;
break;
case 4: // rtk fixed
state.status = AP_GPS::GPS_OK_FIX_3D_RTK;
break;
case 5: // rtk float
state.status = AP_GPS::GPS_OK_FIX_3D_DGPS;
break;
case 6: // sbas
state.status = AP_GPS::GPS_OK_FIX_3D;
break;
case 7: // moving rtk fixed
state.status = AP_GPS::GPS_OK_FIX_3D_RTK;
break;
case 8: // moving rtk float
state.status = AP_GPS::GPS_OK_FIX_3D_DGPS;
break;
}
if ((temp.Mode & 64) > 0) // gps is in base mode
state.status = AP_GPS::NO_FIX;
if ((temp.Mode & 128) > 0) // gps only has 2d fix
state.status = AP_GPS::GPS_OK_FIX_2D;
return true;
}
// DOP
if (blockid == 4001) {
const msg4001 &temp = sbf_msg.data.msg4001u;
last_hdop = temp.HDOP;
state.hdop = last_hdop;
}
return false;
}
bool
AP_GPS_SBF::process_message(void)
{
uint16_t blockid = (sbf_msg.blockid & 8191u);
Debug("BlockID %d", blockid);
switch (blockid) {
case ExtEventPVTGeodetic:
log_ExtEventPVTGeodetic(sbf_msg.data.msg4007u);
break;
case PVTGeodetic:
{
const msg4007 &temp = sbf_msg.data.msg4007u;
// Update time state
if (temp.WNc != 65535) {
state.time_week = temp.WNc;
state.time_week_ms = (uint32_t)(temp.TOW);
}
state.last_gps_time_ms = AP_HAL::millis();
// Update velocity state (don't use −2·10^10)
if (temp.Vn > -200000) {
state.velocity.x = (float)(temp.Vn);
state.velocity.y = (float)(temp.Ve);
state.velocity.z = (float)(-temp.Vu);
state.have_vertical_velocity = true;
float ground_vector_sq = state.velocity[0] * state.velocity[0] + state.velocity[1] * state.velocity[1];
state.ground_speed = (float)safe_sqrt(ground_vector_sq);
state.ground_course = wrap_360(degrees(atan2f(state.velocity[1], state.velocity[0])));
state.rtk_age_ms = temp.MeanCorrAge * 10;
// value is expressed as twice the rms error = int16 * 0.01/2
state.horizontal_accuracy = (float)temp.HAccuracy * 0.005f;
state.vertical_accuracy = (float)temp.VAccuracy * 0.005f;
state.have_horizontal_accuracy = true;
state.have_vertical_accuracy = true;
}
// Update position state (don't use −2·10^10)
if (temp.Latitude > -200000) {
state.location.lat = (int32_t)(temp.Latitude * RAD_TO_DEG_DOUBLE * (double)1e7);
state.location.lng = (int32_t)(temp.Longitude * RAD_TO_DEG_DOUBLE * (double)1e7);
state.location.alt = (int32_t)(((float)temp.Height - temp.Undulation) * 1e2f);
}
if (temp.NrSV != 255) {
state.num_sats = temp.NrSV;
}
Debug("temp.Mode=0x%02x\n", (unsigned)temp.Mode);
switch (temp.Mode & 15) {
case 0: // no pvt
state.status = AP_GPS::NO_FIX;
break;
case 1: // standalone
state.status = AP_GPS::GPS_OK_FIX_3D;
break;
case 2: // dgps
state.status = AP_GPS::GPS_OK_FIX_3D_DGPS;
break;
case 3: // fixed location
state.status = AP_GPS::GPS_OK_FIX_3D;
break;
case 4: // rtk fixed
state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FIXED;
break;
case 5: // rtk float
state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FLOAT;
break;
case 6: // sbas
state.status = AP_GPS::GPS_OK_FIX_3D_DGPS;
break;
case 7: // moving rtk fixed
state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FIXED;
break;
case 8: // moving rtk float
state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FLOAT;
break;
}
if ((temp.Mode & 64) > 0) { // gps is in base mode
state.status = AP_GPS::NO_FIX;
} else if ((temp.Mode & 128) > 0) { // gps only has 2d fix
state.status = AP_GPS::GPS_OK_FIX_2D;
}
return true;
}
case DOP:
{
const msg4001 &temp = sbf_msg.data.msg4001u;
state.hdop = temp.HDOP;
state.vdop = temp.VDOP;
break;
}
case ReceiverStatus:
{
const msg4014 &temp = sbf_msg.data.msg4014u;
RxState = temp.RxState;
RxError = temp.RxError;
break;
}
case VelCovGeodetic:
{
const msg5908 &temp = sbf_msg.data.msg5908u;
// select the maximum variance, as the EKF will apply it to all the columnds in it's estimate
// FIXME: Support returning the covariance matric to the EKF
float max_variance_squared = MAX(temp.Cov_VnVn, MAX(temp.Cov_VeVe, temp.Cov_VuVu));
if (is_positive(max_variance_squared)) {
state.have_speed_accuracy = true;
state.speed_accuracy = sqrt(max_variance_squared);
} else {
state.have_speed_accuracy = false;
}
break;
}
}
return false;
}
// handles an incoming mavlink message (HIL_GPS) and sets
// corresponding gps data appropriately;
void AP_GPS_MAV::handle_msg(mavlink_message_t *msg)
{
mavlink_gps_input_t packet;
mavlink_msg_gps_input_decode(msg, &packet);
bool have_alt = ((packet.ignore_flags & GPS_INPUT_IGNORE_FLAG_ALT) == 0);
bool have_hdop = ((packet.ignore_flags & GPS_INPUT_IGNORE_FLAG_HDOP) == 0);
bool have_vdop = ((packet.ignore_flags & GPS_INPUT_IGNORE_FLAG_VDOP) == 0);
bool have_vel_h = ((packet.ignore_flags & GPS_INPUT_IGNORE_FLAG_VEL_HORIZ) == 0);
bool have_vel_v = ((packet.ignore_flags & GPS_INPUT_IGNORE_FLAG_VEL_VERT) == 0);
bool have_sa = ((packet.ignore_flags & GPS_INPUT_IGNORE_FLAG_SPEED_ACCURACY) == 0);
bool have_ha = ((packet.ignore_flags & GPS_INPUT_IGNORE_FLAG_HORIZONTAL_ACCURACY) == 0);
bool have_va = ((packet.ignore_flags & GPS_INPUT_IGNORE_FLAG_VERTICAL_ACCURACY) == 0);
state.time_week = packet.time_week;
state.time_week_ms = packet.time_week_ms;
state.status = (AP_GPS::GPS_Status)packet.fix_type;
Location loc = {};
loc.lat = packet.lat;
loc.lng = packet.lon;
if (have_alt) {
loc.alt = packet.alt;
}
state.location = loc;
state.location.options = 0;
if (have_hdop) {
state.hdop = packet.hdop * 10; //In centimeters
}
if (have_vdop) {
state.vdop = packet.vdop * 10; //In centimeters
}
if (have_vel_h) {
Vector3f vel(packet.vn, packet.ve, 0);
if (have_vel_v)
vel.z = packet.vd;
state.velocity = vel;
state.ground_course = wrap_360(degrees(atan2f(vel.y, vel.x)));
state.ground_speed = norm(vel.x, vel.y);
}
if (have_sa) {
state.speed_accuracy = packet.speed_accuracy;
state.have_speed_accuracy = 1;
}
if (have_ha) {
state.horizontal_accuracy = packet.horiz_accuracy;
state.have_horizontal_accuracy = 1;
}
if (have_va) {
state.vertical_accuracy = packet.vert_accuracy;
state.have_vertical_accuracy = 1;
}
state.num_sats = packet.satellites_visible;
state.last_gps_time_ms = AP_HAL::millis();
_new_data = true;
}
static void gnss_fix_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix>& msg, uint8_t mgr)
{
if (hal.can_mgr[mgr] != nullptr) {
AP_UAVCAN *ap_uavcan = hal.can_mgr[mgr]->get_UAVCAN();
if (ap_uavcan != nullptr) {
AP_GPS::GPS_State *state = ap_uavcan->find_gps_node(msg.getSrcNodeID().get());
if (state != nullptr) {
bool process = false;
if (msg.status == uavcan::equipment::gnss::Fix::STATUS_NO_FIX) {
state->status = AP_GPS::GPS_Status::NO_FIX;
} else {
if (msg.status == uavcan::equipment::gnss::Fix::STATUS_TIME_ONLY) {
state->status = AP_GPS::GPS_Status::NO_FIX;
} else if (msg.status == uavcan::equipment::gnss::Fix::STATUS_2D_FIX) {
state->status = AP_GPS::GPS_Status::GPS_OK_FIX_2D;
process = true;
} else if (msg.status == uavcan::equipment::gnss::Fix::STATUS_3D_FIX) {
state->status = AP_GPS::GPS_Status::GPS_OK_FIX_3D;
process = true;
}
if (msg.gnss_time_standard == uavcan::equipment::gnss::Fix::GNSS_TIME_STANDARD_UTC) {
uint64_t epoch_ms = uavcan::UtcTime(msg.gnss_timestamp).toUSec();
epoch_ms /= 1000;
uint64_t gps_ms = epoch_ms - UNIX_OFFSET_MSEC;
state->time_week = (uint16_t)(gps_ms / AP_MSEC_PER_WEEK);
state->time_week_ms = (uint32_t)(gps_ms - (state->time_week) * AP_MSEC_PER_WEEK);
}
}
if (process) {
Location loc = { };
loc.lat = msg.latitude_deg_1e8 / 10;
loc.lng = msg.longitude_deg_1e8 / 10;
loc.alt = msg.height_msl_mm / 10;
state->location = loc;
state->location.options = 0;
if (!uavcan::isNaN(msg.ned_velocity[0])) {
Vector3f vel(msg.ned_velocity[0], msg.ned_velocity[1], msg.ned_velocity[2]);
state->velocity = vel;
state->ground_speed = norm(vel.x, vel.y);
state->ground_course = wrap_360(degrees(atan2f(vel.y, vel.x)));
state->have_vertical_velocity = true;
} else {
state->have_vertical_velocity = false;
}
float pos_cov[9];
msg.position_covariance.unpackSquareMatrix(pos_cov);
if (!uavcan::isNaN(pos_cov[8])) {
if (pos_cov[8] > 0) {
state->vertical_accuracy = sqrtf(pos_cov[8]);
state->have_vertical_accuracy = true;
} else {
state->have_vertical_accuracy = false;
}
} else {
state->have_vertical_accuracy = false;
}
const float horizontal_pos_variance = MAX(pos_cov[0], pos_cov[4]);
if (!uavcan::isNaN(horizontal_pos_variance)) {
if (horizontal_pos_variance > 0) {
state->horizontal_accuracy = sqrtf(horizontal_pos_variance);
state->have_horizontal_accuracy = true;
} else {
state->have_horizontal_accuracy = false;
}
} else {
state->have_horizontal_accuracy = false;
}
float vel_cov[9];
msg.velocity_covariance.unpackSquareMatrix(vel_cov);
if (!uavcan::isNaN(vel_cov[0])) {
state->speed_accuracy = sqrtf((vel_cov[0] + vel_cov[4] + vel_cov[8]) / 3.0);
state->have_speed_accuracy = true;
} else {
state->have_speed_accuracy = false;
}
state->num_sats = msg.sats_used;
} else {
state->have_vertical_velocity = false;
state->have_vertical_accuracy = false;
state->have_horizontal_accuracy = false;
state->have_speed_accuracy = false;
state->num_sats = 0;
}
state->last_gps_time_ms = AP_HAL::millis();
// after all is filled, update all listeners with new data
ap_uavcan->update_gps_state(msg.getSrcNodeID().get());
}
}
}
}
bool
AP_GPS_UBLOX::_parse_gps(void)
{
if (_class == CLASS_ACK) {
Debug("ACK %u", (unsigned)_msg_id);
if(_msg_id == MSG_ACK_ACK) {
switch(_buffer.ack.clsID) {
case CLASS_CFG:
switch(_buffer.ack.msgID) {
case MSG_CFG_CFG:
_cfg_saved = true;
_cfg_needs_save = false;
break;
case MSG_CFG_GNSS:
_unconfigured_messages &= ~CONFIG_GNSS;
break;
case MSG_CFG_MSG:
// There is no way to know what MSG config was ack'ed, assume it was the last
// one requested. To verify it rerequest the last config we sent. If we miss
// the actual ack we will catch it next time through the poll loop, but that
// will be a good chunk of time later.
break;
case MSG_CFG_NAV_SETTINGS:
_unconfigured_messages &= ~CONFIG_NAV_SETTINGS;
break;
case MSG_CFG_RATE:
// The GPS will ACK a update rate that is invalid. in order to detect this
// only accept the rate as configured by reading the settings back and
// validating that they all match the target values
break;
case MSG_CFG_SBAS:
_unconfigured_messages &= ~CONFIG_SBAS;
break;
}
break;
case CLASS_MON:
switch(_buffer.ack.msgID) {
case MSG_MON_HW:
_unconfigured_messages &= ~CONFIG_RATE_MON_HW;
break;
case MSG_MON_HW2:
_unconfigured_messages &= ~CONFIG_RATE_MON_HW2;
break;
}
}
}
return false;
}
if (_class == CLASS_CFG) {
switch(_msg_id) {
case MSG_CFG_NAV_SETTINGS:
Debug("Got settings %u min_elev %d drLimit %u\n",
(unsigned)_buffer.nav_settings.dynModel,
(int)_buffer.nav_settings.minElev,
(unsigned)_buffer.nav_settings.drLimit);
_buffer.nav_settings.mask = 0;
if (gps._navfilter != AP_GPS::GPS_ENGINE_NONE &&
_buffer.nav_settings.dynModel != gps._navfilter) {
// we've received the current nav settings, change the engine
// settings and send them back
Debug("Changing engine setting from %u to %u\n",
(unsigned)_buffer.nav_settings.dynModel, (unsigned)gps._navfilter);
_buffer.nav_settings.dynModel = gps._navfilter;
_buffer.nav_settings.mask |= 1;
}
if (gps._min_elevation != -100 &&
_buffer.nav_settings.minElev != gps._min_elevation) {
Debug("Changing min elevation to %d\n", (int)gps._min_elevation);
_buffer.nav_settings.minElev = gps._min_elevation;
_buffer.nav_settings.mask |= 2;
}
if (_buffer.nav_settings.mask != 0) {
_send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS,
&_buffer.nav_settings,
sizeof(_buffer.nav_settings));
_unconfigured_messages |= CONFIG_NAV_SETTINGS;
_cfg_needs_save = true;
} else {
_unconfigured_messages &= ~CONFIG_NAV_SETTINGS;
}
return false;
#if UBLOX_GNSS_SETTINGS
case MSG_CFG_GNSS:
if (gps._gnss_mode[state.instance] != 0) {
struct ubx_cfg_gnss start_gnss = _buffer.gnss;
uint8_t gnssCount = 0;
Debug("Got GNSS Settings %u %u %u %u:\n",
(unsigned)_buffer.gnss.msgVer,
(unsigned)_buffer.gnss.numTrkChHw,
(unsigned)_buffer.gnss.numTrkChUse,
(unsigned)_buffer.gnss.numConfigBlocks);
#if UBLOX_DEBUGGING
for(int i = 0; i < _buffer.gnss.numConfigBlocks; i++) {
Debug(" %u %u %u 0x%08x\n",
(unsigned)_buffer.gnss.configBlock[i].gnssId,
(unsigned)_buffer.gnss.configBlock[i].resTrkCh,
(unsigned)_buffer.gnss.configBlock[i].maxTrkCh,
(unsigned)_buffer.gnss.configBlock[i].flags);
}
#endif
for(int i = 0; i < UBLOX_MAX_GNSS_CONFIG_BLOCKS; i++) {
if((gps._gnss_mode[state.instance] & (1 << i)) && i != GNSS_SBAS) {
gnssCount++;
}
}
for(int i = 0; i < _buffer.gnss.numConfigBlocks; i++) {
// Reserve an equal portion of channels for all enabled systems
if(gps._gnss_mode[state.instance] & (1 << _buffer.gnss.configBlock[i].gnssId)) {
if(GNSS_SBAS !=_buffer.gnss.configBlock[i].gnssId) {
_buffer.gnss.configBlock[i].resTrkCh = (_buffer.gnss.numTrkChHw - 3) / (gnssCount * 2);
_buffer.gnss.configBlock[i].maxTrkCh = _buffer.gnss.numTrkChHw;
} else {
_buffer.gnss.configBlock[i].resTrkCh = 1;
_buffer.gnss.configBlock[i].maxTrkCh = 3;
}
_buffer.gnss.configBlock[i].flags = _buffer.gnss.configBlock[i].flags | 0x00000001;
} else {
_buffer.gnss.configBlock[i].resTrkCh = 0;
_buffer.gnss.configBlock[i].maxTrkCh = 0;
_buffer.gnss.configBlock[i].flags = _buffer.gnss.configBlock[i].flags & 0xFFFFFFFE;
}
}
if (!memcmp(&start_gnss, &_buffer.gnss, sizeof(start_gnss))) {
_send_message(CLASS_CFG, MSG_CFG_GNSS, &_buffer.gnss, 4 + (8 * _buffer.gnss.numConfigBlocks));
_unconfigured_messages |= CONFIG_GNSS;
_cfg_needs_save = true;
} else {
_unconfigured_messages &= ~CONFIG_GNSS;
}
} else {
_unconfigured_messages &= ~CONFIG_GNSS;
}
return false;
#endif
case MSG_CFG_SBAS:
if (gps._sbas_mode != 2) {
Debug("Got SBAS settings %u %u %u 0x%x 0x%x\n",
(unsigned)_buffer.sbas.mode,
(unsigned)_buffer.sbas.usage,
(unsigned)_buffer.sbas.maxSBAS,
(unsigned)_buffer.sbas.scanmode2,
(unsigned)_buffer.sbas.scanmode1);
if (_buffer.sbas.mode != gps._sbas_mode) {
_buffer.sbas.mode = gps._sbas_mode;
_send_message(CLASS_CFG, MSG_CFG_SBAS,
&_buffer.sbas,
sizeof(_buffer.sbas));
_unconfigured_messages |= CONFIG_SBAS;
_cfg_needs_save = true;
} else {
_unconfigured_messages &= ~CONFIG_SBAS;
}
} else {
_unconfigured_messages &= ~CONFIG_SBAS;
}
return false;
case MSG_CFG_MSG:
if(_payload_length == sizeof(ubx_cfg_msg_rate_6)) {
// can't verify the setting without knowing the port
// request the port again
if(_ublox_port >= UBLOX_MAX_PORTS) {
_request_port();
return false;
}
_verify_rate(_buffer.msg_rate_6.msg_class, _buffer.msg_rate_6.msg_id,
_buffer.msg_rate_6.rates[_ublox_port]);
} else {
_verify_rate(_buffer.msg_rate.msg_class, _buffer.msg_rate.msg_id,
_buffer.msg_rate.rate);
}
return false;
case MSG_CFG_PRT:
_ublox_port = _buffer.prt.portID;
return false;
case MSG_CFG_RATE:
if(_buffer.nav_rate.measure_rate_ms != gps._rate_ms[state.instance] ||
_buffer.nav_rate.nav_rate != 1 ||
_buffer.nav_rate.timeref != 0) {
_configure_rate();
_unconfigured_messages |= CONFIG_RATE_NAV;
_cfg_needs_save = true;
} else {
_unconfigured_messages &= ~CONFIG_RATE_NAV;
}
return false;
}
}
if (_class == CLASS_MON) {
switch(_msg_id) {
case MSG_MON_HW:
if (_payload_length == 60 || _payload_length == 68) {
log_mon_hw();
}
break;
case MSG_MON_HW2:
if (_payload_length == 28) {
log_mon_hw2();
}
break;
case MSG_MON_VER:
_have_version = true;
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO,
"u-blox %d HW: %s SW: %s",
state.instance,
_buffer.mon_ver.hwVersion,
_buffer.mon_ver.swVersion);
break;
default:
unexpected_message();
}
return false;
}
#if UBLOX_RXM_RAW_LOGGING
if (_class == CLASS_RXM && _msg_id == MSG_RXM_RAW && gps._raw_data != 0) {
log_rxm_raw(_buffer.rxm_raw);
return false;
} else if (_class == CLASS_RXM && _msg_id == MSG_RXM_RAWX && gps._raw_data != 0) {
log_rxm_rawx(_buffer.rxm_rawx);
return false;
}
#endif // UBLOX_RXM_RAW_LOGGING
if (_class != CLASS_NAV) {
unexpected_message();
return false;
}
switch (_msg_id) {
case MSG_POSLLH:
Debug("MSG_POSLLH next_fix=%u", next_fix);
_last_pos_time = _buffer.posllh.time;
state.location.lng = _buffer.posllh.longitude;
state.location.lat = _buffer.posllh.latitude;
state.location.alt = _buffer.posllh.altitude_msl / 10;
state.status = next_fix;
_new_position = true;
state.horizontal_accuracy = _buffer.posllh.horizontal_accuracy*1.0e-3f;
state.vertical_accuracy = _buffer.posllh.vertical_accuracy*1.0e-3f;
state.have_horizontal_accuracy = true;
state.have_vertical_accuracy = true;
#if UBLOX_FAKE_3DLOCK
state.location.lng = 1491652300L;
state.location.lat = -353632610L;
state.location.alt = 58400;
state.vertical_accuracy = 0;
state.horizontal_accuracy = 0;
#endif
break;
case MSG_STATUS:
Debug("MSG_STATUS fix_status=%u fix_type=%u",
_buffer.status.fix_status,
_buffer.status.fix_type);
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)) {
next_fix = AP_GPS::GPS_OK_FIX_3D_DGPS;
}else if( _buffer.status.fix_type == AP_GPS_UBLOX::FIX_3D) {
next_fix = AP_GPS::GPS_OK_FIX_3D;
}else if (_buffer.status.fix_type == AP_GPS_UBLOX::FIX_2D) {
next_fix = AP_GPS::GPS_OK_FIX_2D;
}else{
next_fix = AP_GPS::NO_FIX;
state.status = AP_GPS::NO_FIX;
}
}else{
next_fix = AP_GPS::NO_FIX;
state.status = AP_GPS::NO_FIX;
}
#if UBLOX_FAKE_3DLOCK
state.status = AP_GPS::GPS_OK_FIX_3D;
next_fix = state.status;
#endif
break;
case MSG_DOP:
Debug("MSG_DOP");
noReceivedHdop = false;
state.hdop = _buffer.dop.hDOP;
state.vdop = _buffer.dop.vDOP;
#if UBLOX_FAKE_3DLOCK
state.hdop = 130;
state.hdop = 170;
#endif
break;
case MSG_SOL:
Debug("MSG_SOL fix_status=%u fix_type=%u",
_buffer.solution.fix_status,
_buffer.solution.fix_type);
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)) {
next_fix = AP_GPS::GPS_OK_FIX_3D_DGPS;
}else if( _buffer.solution.fix_type == AP_GPS_UBLOX::FIX_3D) {
next_fix = AP_GPS::GPS_OK_FIX_3D;
}else if (_buffer.solution.fix_type == AP_GPS_UBLOX::FIX_2D) {
next_fix = AP_GPS::GPS_OK_FIX_2D;
}else{
next_fix = AP_GPS::NO_FIX;
state.status = AP_GPS::NO_FIX;
}
}else{
next_fix = AP_GPS::NO_FIX;
state.status = AP_GPS::NO_FIX;
}
if(noReceivedHdop) {
state.hdop = _buffer.solution.position_DOP;
}
state.num_sats = _buffer.solution.satellites;
if (next_fix >= AP_GPS::GPS_OK_FIX_2D) {
state.last_gps_time_ms = AP_HAL::millis();
state.time_week_ms = _buffer.solution.time;
state.time_week = _buffer.solution.week;
}
#if UBLOX_FAKE_3DLOCK
next_fix = state.status;
state.num_sats = 10;
state.time_week = 1721;
state.time_week_ms = AP_HAL::millis() + 3*60*60*1000 + 37000;
state.last_gps_time_ms = AP_HAL::millis();
state.hdop = 130;
#endif
break;
case MSG_VELNED:
Debug("MSG_VELNED");
_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
state.have_vertical_velocity = true;
state.velocity.x = _buffer.velned.ned_north * 0.01f;
state.velocity.y = _buffer.velned.ned_east * 0.01f;
state.velocity.z = _buffer.velned.ned_down * 0.01f;
state.ground_course = wrap_360(degrees(atan2f(state.velocity.y, state.velocity.x)));
state.ground_speed = norm(state.velocity.y, state.velocity.x);
state.have_speed_accuracy = true;
state.speed_accuracy = _buffer.velned.speed_accuracy*0.01f;
#if UBLOX_FAKE_3DLOCK
state.speed_accuracy = 0;
#endif
_new_speed = true;
break;
case MSG_NAV_SVINFO:
{
Debug("MSG_NAV_SVINFO\n");
static const uint8_t HardwareGenerationMask = 0x07;
_hardware_generation = _buffer.svinfo_header.globalFlags & HardwareGenerationMask;
switch (_hardware_generation) {
case UBLOX_5:
case UBLOX_6:
// only 7 and newer support CONFIG_GNSS
_unconfigured_messages &= ~CONFIG_GNSS;
break;
case UBLOX_7:
case UBLOX_M8:
#if UBLOX_SPEED_CHANGE
port->begin(4000000U);
Debug("Changed speed to 4Mhz for SPI-driven UBlox\n");
#endif
break;
default:
hal.console->printf("Wrong Ublox Hardware Version%u\n", _hardware_generation);
break;
};
_unconfigured_messages &= ~CONFIG_VERSION;
/* We don't need that anymore */
_configure_message_rate(CLASS_NAV, MSG_NAV_SVINFO, 0);
break;
}
default:
Debug("Unexpected NAV message 0x%02x", (unsigned)_msg_id);
if (++_disable_counter == 0) {
Debug("Disabling NAV message 0x%02x", (unsigned)_msg_id);
_configure_message_rate(CLASS_NAV, _msg_id, 0);
}
return false;
}
// we only return true when we get new position and speed data
// this ensures we don't use stale data
if (_new_position && _new_speed && _last_vel_time == _last_pos_time) {
_new_speed = _new_position = false;
return true;
}
return false;
}
void DecoderBinaural::setSampleRate(unsigned int sampleRate)
{
int index;
float value_left;
float value_right;
if(sampleRate != m_sample_rate)
{
m_impulses_loaded = 0;
m_sample_rate = 0;
for(unsigned int i = 0; i < hoa_number_binaural_samplerate; i++)
{
if(sampleRate == hoa_binaural_samplerate[i])
{
index = i;
m_sample_rate = hoa_binaural_samplerate[i];
m_impulses_size = hoa_binaural_impulse_sizes[i];
}
}
if(!m_sample_rate)
{
index = 0;
m_sample_rate = hoa_binaural_samplerate[0];
m_impulses_size = hoa_binaural_impulse_sizes[0];
}
for(int i = 0; i < m_number_of_virtual_channels; i++)
{
m_impulses_vector[i] = get_mit_hrtf_2D(m_sample_rate, wrap_360(-i * 360 / m_number_of_virtual_channels), m_pinna_size) +hoa_binaural_crop[index];
}
if(m_impulses_matrix)
delete [] m_impulses_matrix;
// Sample by sample
for(int i = 0; i < m_number_of_virtual_channels; i++)
{
if(m_channels_inputs_left[i])
delete [] m_channels_inputs_left[i];
if(m_channels_inputs_right[i])
delete [] m_channels_inputs_right[i];
m_channels_inputs_left[i] = new float[m_impulses_size * 2 - 1];
m_channels_inputs_right[i] = new float[m_impulses_size * 2 - 1];
}
m_index = m_impulses_size;
// Other
m_impulses_matrix = new float[m_impulses_size * 2 * m_number_of_harmonics];
for(unsigned int i = 0; i < m_number_of_harmonics; i++)
m_harmonics_vector[i] = 0.;
for(unsigned int i = 0; i < m_number_of_virtual_channels; i++)
m_channels_vector[i] = 0.;
m_harmonics_vector[0] = 1.;
for(unsigned int i = 1; i < m_number_of_harmonics; i++)
{
m_harmonics_vector[i] = 0.;
}
for(unsigned int i = 0; i < m_number_of_harmonics; i++)
{
if(i != 0)
{
m_harmonics_vector[i-1] = 0.;
m_harmonics_vector[i] = 1.;
}
m_decoder->process(m_harmonics_vector, m_channels_vector);
for(unsigned int j = 0; j < m_impulses_size; j++)
{
value_left = 0;
value_right = 0;
for(unsigned int k = 0; k < m_number_of_virtual_channels; k++)
{
value_right += m_channels_vector[k] * m_impulses_vector[k][j];
if(k == 0)
value_left += m_channels_vector[k] * m_impulses_vector[k][j];
else
value_left += m_channels_vector[k] * m_impulses_vector[m_number_of_virtual_channels - k][j];
}
m_impulses_matrix[j * m_number_of_harmonics + i] = value_left;
m_impulses_matrix[(j + m_impulses_size) * m_number_of_harmonics + i] = value_right;
}
}
m_impulses_loaded = 1;
}
}
// Process bytes available from the stream
//
// The stream is assumed to contain only our custom message. If it
// contains other messages, and those messages contain the preamble bytes,
// it is possible for this code to become de-synchronised. Without
// buffering the entire message and re-processing it from the top,
// this is unavoidable.
//
// The lack of a standard header length field makes it impossible to skip
// unrecognised messages.
//
bool
AP_GPS_MTK::read(void)
{
uint8_t data;
int16_t numc;
bool parsed = false;
numc = port->available();
for (int16_t i = 0; i < numc; i++) { // Process bytes received
// read the next byte
data = port->read();
restart:
switch(_step) {
// Message preamble, class, ID detection
//
// If we fail to match any of the expected bytes, we
// reset the state machine and re-consider the failed
// byte as the first byte of the preamble. This
// improves our chances of recovering from a mismatch
// and makes it less likely that we will be fooled by
// the preamble appearing as data in some other message.
//
case 0:
if(PREAMBLE1 == data)
_step++;
break;
case 1:
if (PREAMBLE2 == data) {
_step++;
break;
}
_step = 0;
goto restart;
case 2:
if (MESSAGE_CLASS == data) {
_step++;
_ck_b = _ck_a = data; // reset the checksum accumulators
} else {
_step = 0; // reset and wait for a message of the right class
goto restart;
}
break;
case 3:
if (MESSAGE_ID == data) {
_step++;
_ck_b += (_ck_a += data);
_payload_counter = 0;
} else {
_step = 0;
goto restart;
}
break;
// Receive message data
//
case 4:
_buffer.bytes[_payload_counter++] = data;
_ck_b += (_ck_a += data);
if (_payload_counter == sizeof(_buffer))
_step++;
break;
// Checksum and message processing
//
case 5:
_step++;
if (_ck_a != data) {
_step = 0;
}
break;
case 6:
_step = 0;
if (_ck_b != data) {
break;
}
// set fix type
if (_buffer.msg.fix_type == FIX_3D) {
state.status = AP_GPS::GPS_OK_FIX_3D;
}else if (_buffer.msg.fix_type == FIX_2D) {
state.status = AP_GPS::GPS_OK_FIX_2D;
}else{
state.status = AP_GPS::NO_FIX;
}
state.location.lat = swap_int32(_buffer.msg.latitude) * 10;
state.location.lng = swap_int32(_buffer.msg.longitude) * 10;
state.location.alt = swap_int32(_buffer.msg.altitude);
state.ground_speed = swap_int32(_buffer.msg.ground_speed) * 0.01f;
state.ground_course = wrap_360(swap_int32(_buffer.msg.ground_course) * 1.0e-6f);
state.num_sats = _buffer.msg.satellites;
if (state.status >= AP_GPS::GPS_OK_FIX_2D) {
make_gps_time(0, swap_int32(_buffer.msg.utc_time)*10);
}
// we don't change _last_gps_time as we don't know the
// full date
fill_3d_velocity();
parsed = true;
}
}
return parsed;
}
