diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 910fd81684..3f2f59ff12 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -79,6 +79,25 @@ const AP_Param::GroupInfo AP_GPS::var_info[] PROGMEM = { // @User: Advanced AP_GROUPINFO("MIN_ELEV", 6, AP_GPS, _min_elevation, -100), +#if GPS_MAX_INSTANCES > 1 + + // @Param: INJECT_TO + // @DisplayName: Destination for GPS_INJECT_DATA MAVLink packets + // @Description: The GGS can send raw serial packets to inject data to multiple GPSes. + // @Values: 0,1: send to specified instance. 127: broadcast + AP_GROUPINFO("INJECT_TO", 7, AP_GPS, _inject_to, 127), + +#endif + +#if GPS_RTK_AVAILABLE + // @Param: SBP_LOGMASK + // @DisplayName: Swift Binary Protocol Logging Mask + // @Description: Masked with the SBP msg_type field to determine whether SBR1/SBR2 data is logged + // @Values: 0x0000 for none, 0xFFFF for all, 0xFF00 for external only. + // @User: Advanced + AP_GROUPINFO("SBP_LOGMASK", 8, AP_GPS, _sbp_logmask, 0xFF00), +#endif + AP_GROUPEND }; @@ -251,34 +270,6 @@ found_gps: } } -bool -AP_GPS::can_calculate_base_pos(void) -{ -#if GPS_RTK_AVAILABLE - for (uint8_t i=0; ican_calculate_base_pos()) { - return true; - } - } -#endif - return false; -} - -/* - Tells the underlying GPS drivers to capture its current position as home. - */ -void -AP_GPS::calculate_base_pos(void) -{ -#if GPS_RTK_AVAILABLE - for (uint8_t i = 0; ican_calculate_base_pos()) { - drivers[i]->calculate_base_pos(); - } - } -#endif -} - AP_GPS::GPS_Status AP_GPS::highest_supported_status(uint8_t instance) const { @@ -441,6 +432,7 @@ AP_GPS::setHIL(uint8_t instance, GPS_Status _status, uint64_t time_epoch_ms, void AP_GPS::lock_port(uint8_t instance, bool lock) { + if (instance >= GPS_MAX_INSTANCES) { return; } @@ -451,6 +443,35 @@ AP_GPS::lock_port(uint8_t instance, bool lock) } } + //Inject a packet of raw binary to a GPS +void +AP_GPS::inject_data(uint8_t *data, uint8_t len) +{ + +#if GPS_MAX_INSTANCES > 1 + + //Support broadcasting to all GPSes. + if (_inject_to == 127) { + for (uint8_t i=0; iinject_data(data, len); +} + void AP_GPS::send_mavlink_gps_raw(mavlink_channel_t chan) { diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 73402058c3..c54d08cd48 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -74,14 +74,6 @@ public: /// more) to process incoming data. void update(void); - //True if any of the underlying GPS Drivers are ready to enter - //a dgps-based fix beyond 3D lock, such as RTK mode. - bool can_calculate_base_pos(void); - - //Allows the underlying GPS Drivers to enter a differential lock - //Might cause a position jump, thus only do this on the ground. - void calculate_base_pos(void); - // GPS driver types enum GPS_Type { GPS_TYPE_NONE = 0, @@ -336,6 +328,8 @@ public: #if GPS_MAX_INSTANCES > 1 AP_Int8 _auto_switch; AP_Int8 _min_dgps; + AP_Int16 _sbp_logmask; + AP_Int8 _inject_to; #endif AP_Int8 _sbas_mode; AP_Int8 _min_elevation; @@ -347,6 +341,10 @@ public: // lock out a GPS port, allowing another application to use the port void lock_port(uint8_t instance, bool locked); + //Inject a packet of raw binary to a GPS + void inject_data(uint8_t *data, uint8_t len); + void inject_data(uint8_t instance, uint8_t *data, uint8_t len); + //MAVLink Status Sending void send_mavlink_gps_raw(mavlink_channel_t chan); #if GPS_MAX_INSTANCES > 1 diff --git a/libraries/AP_GPS/AP_GPS_SBP.cpp b/libraries/AP_GPS/AP_GPS_SBP.cpp index 9b28adce27..c99a251df4 100644 --- a/libraries/AP_GPS/AP_GPS_SBP.cpp +++ b/libraries/AP_GPS/AP_GPS_SBP.cpp @@ -15,10 +15,11 @@ */ // -// Swift Navigation GPS driver for ArduPilot -// Origin code by Niels Joubert njoubert.com +// Swift Navigation SBP GPS driver for ArduPilot. +// Code by Niels Joubert +// +// Swift Binary Protocol format: http://docs.swift-nav.com/ // - #include #include "AP_GPS_SBP.h" @@ -26,336 +27,87 @@ #if GPS_RTK_AVAILABLE -#define SBP_DEBUGGING 0 -#define SBP_FAKE_3DLOCK 0 - extern const AP_HAL::HAL& hal; -#define SBP_MILLIS_BETWEEN_HEALTHCHECKS 2000U -#define SBP_BASELINE_TIMEOUT_MS 1000U -#define SBP_FIX_TIMEOUT_MS 1000U -#define SBP_HEARTBEAT_TIMEOUT_MS 5000U +#define SBP_DEBUGGING 1 +#define SBP_HW_LOGGING 1 -#define SBP_MILLIS_BETWEEN_TRACKING_LOG 1800U +#define SBP_TIMEOUT_HEATBEAT 4000 +#define SBP_TIMEOUT_PVT 500 -#define SBP_DEBUGGING 0 #if SBP_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 # define Debug(fmt, args ...) - #endif - -/* - only do detailed hardware logging on boards likely to have more log - storage space - */ -#if GPS_RTK_AVAILABLE -#define SBP_HW_LOGGING 1 -#else -#define SBP_HW_LOGGING 0 #endif bool AP_GPS_SBP::logging_started = false; -AP_GPS_SBP::AP_GPS_SBP(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) : + +AP_GPS_SBP::AP_GPS_SBP(AP_GPS &_gps, AP_GPS::GPS_State &_state, + AP_HAL::UARTDriver *_port) : AP_GPS_Backend(_gps, _state, _port), - last_baseline_received_ms(0), - last_heatbeat_received_ms(0), - last_tracking_state_ms(0), - iar_num_hypotheses(-1), - baseline_recv_rate(0), - - dgps_corrections_incoming(false), - rtk_corrections_incoming(false), - - has_new_pos_llh(false), - has_new_vel_ned(false), - has_new_baseline_ecef(false), - - has_rtk_base_pos(false), - - pos_msg_counter(0), - vel_msg_counter(0), - baseline_msg_counter(0), - full_update_counter(0), crc_error_counter(0), - last_healthcheck_millis(0) + last_full_update_tow(0), + last_full_update_cpu_ms(0), + last_injected_data_ms(0), + last_iar_num_hypotheses(0) { + Debug("SBP Driver Initialized"); + parser_state.state = sbp_parser_state_t::WAITING; + //Externally visible state state.status = AP_GPS::NO_FIX; state.have_vertical_velocity = true; - - state.last_gps_time_ms = last_heatbeat_received_ms = last_healthcheck_millis = hal.scheduler->millis(); + state.last_gps_time_ms = last_heatbeat_received_ms = hal.scheduler->millis(); } -bool -AP_GPS_SBP::can_calculate_base_pos(void) -{ - return (rtk_corrections_incoming && !has_rtk_base_pos); -}; - - -void -AP_GPS_SBP::calculate_base_pos(void) -{ - //INVARIANT: - // Only ever capture home with motors not armed! - // External driver checks whether can_raise_fix_level becomes true - // and only if it can, AND motors are not armed, will be capture home! - if (state.status < AP_GPS::GPS_OK_FIX_3D) { - Debug("Attempting to capture home without GPS Fix available. Can't do RTK without home lat-lon."); - return; - } - - if (!rtk_corrections_incoming) { - Debug("Attempting to capture home baseline without rtk corrections being received."); - return; - } - - Vector3d current_llh; - Vector3d current_ecef; - Vector3d current_baseline_ecef; - - current_llh[0] = last_sbp_pos_llh_msg.lat * DEG_TO_RAD_DOUBLE; - current_llh[1] = last_sbp_pos_llh_msg.lon * DEG_TO_RAD_DOUBLE; - current_llh[2] = last_sbp_pos_llh_msg.height; - - wgsllh2ecef(current_llh, current_ecef); - - current_baseline_ecef[0] = ((double)last_sbp_baseline_ecef_msg.x) / 1000.0; - current_baseline_ecef[1] = ((double)last_sbp_baseline_ecef_msg.y) / 1000.0; - current_baseline_ecef[2] = ((double)last_sbp_baseline_ecef_msg.z) / 1000.0; - - base_pos_ecef = current_ecef - current_baseline_ecef; - has_rtk_base_pos = true; - - Debug("SBP Got Base Position! has_rtk_base_pos=%d, (%.2f, %.2f, %.2f)", has_rtk_base_pos, - base_pos_ecef[0], - base_pos_ecef[1], - base_pos_ecef[2]); -} -void -AP_GPS_SBP::invalidate_base_pos() -{ - has_rtk_base_pos = false; -} - -bool +// Process all bytes available from the stream +// +bool AP_GPS_SBP::read(void) { + //Invariant: Calling this function processes *all* data current in the UART buffer. // //IMPORTANT NOTICE: This function is NOT CALLED for several seconds // during arming. That should not cause the driver to die. Process *all* waiting messages - bool full_update = false; - do { - //Attempt to process one message at a time - bool new_message = sbp_process(); + _sbp_process(); - //Attempt to update our internal state with this new message. - if (update_state(new_message)) { - full_update = true; - full_update_counter += 1; - } - - } while (port->available() > 0); - - uint32_t now = hal.scheduler->millis(); - uint32_t elapsed = now - last_healthcheck_millis; - if (elapsed > SBP_MILLIS_BETWEEN_HEALTHCHECKS) { - last_healthcheck_millis = now; - - float pos_msg_hz = pos_msg_counter / (float) elapsed * 1000; - float vel_msg_hz = vel_msg_counter / (float) elapsed * 1000; - float baseline_msg_hz = baseline_msg_counter / (float) elapsed * 1000; - float full_update_hz = full_update_counter / (float) elapsed * 1000; - - baseline_recv_rate = uint8_t (baseline_msg_hz * 10); - - pos_msg_counter = 0; - vel_msg_counter = 0; - baseline_msg_counter = 0; - full_update_counter = 0; - - Debug("SBP GPS perf: Fix=(%d) CRC=(%d) Pos=(%.2fHz) Vel=(%.2fHz) Baseline=(%.2fHz) Update=(%.2fHz) DGPS=(%d) RTK=(%d) RTK_HOME=(%d) IAR=(%d)", - state.status, - crc_error_counter, - pos_msg_hz, - vel_msg_hz, - baseline_msg_hz, - full_update_hz, - dgps_corrections_incoming, - rtk_corrections_incoming, - has_rtk_base_pos, - iar_num_hypotheses); - -#if SBP_HW_LOGGING - logging_log_health(pos_msg_hz, - vel_msg_hz, - baseline_msg_hz, - full_update_hz); -#endif - - } - - return full_update; -} - -//This consolidates all the latest messages, -//and the current mode the driver is in -// -// INVARIANT: -// If in a fix mode >= 3, -// returns true only if a full position and velocity update happened. -// If in fix mode 0 or 1, -// returns true if messages are being received or we haven't timed out -bool -AP_GPS_SBP::update_state(bool has_new_message) -{ - - uint32_t now = hal.scheduler->millis(); - - //Determine the current mode the GPS is in: DGPS or plain - //Notice that this is sticky. - if (has_new_baseline_ecef && (now - last_baseline_received_ms < SBP_BASELINE_TIMEOUT_MS)) { - dgps_corrections_incoming = true; - if (gps._min_dgps >= 100) { - //Allow only IntegerRTK baselines - rtk_corrections_incoming = dgps_corrections_incoming && (last_sbp_baseline_ecef_msg.flags & 0x1); - } else { - //Allow floatRTK baselines - rtk_corrections_incoming = dgps_corrections_incoming; - } - } - - //Currently we only use relative positioning if we have RTK-level fixes, - //we ignore float-level fixes - bool using_relative_positioning = rtk_corrections_incoming && has_rtk_base_pos; - - //Drop out of RTK mode if we haven't seen a baseline for a while... - if (using_relative_positioning && (now - last_baseline_received_ms > SBP_BASELINE_TIMEOUT_MS)) { - dgps_corrections_incoming = false; - rtk_corrections_incoming = false; - using_relative_positioning = false; - } - - //UPDATE POSITION AND VELOCITY - if (!using_relative_positioning && - (has_new_pos_llh && has_new_vel_ned) && - (last_sbp_pos_llh_msg.tow == last_sbp_vel_ned_msg.tow)) { - - state.last_gps_time_ms = hal.scheduler->millis(); - - state.time_week_ms = last_sbp_pos_llh_msg.tow; - state.location.lat = (int32_t) (last_sbp_pos_llh_msg.lat*1e7); - state.location.lng = (int32_t) (last_sbp_pos_llh_msg.lon*1e7); - state.location.alt = (int32_t) (last_sbp_pos_llh_msg.height*1e2); - state.num_sats = last_sbp_pos_llh_msg.n_sats; - - update_state_velocity(); - - has_new_pos_llh = false; - - state.status = AP_GPS::GPS_OK_FIX_3D; - - return true; - - } else if (using_relative_positioning && - (has_new_baseline_ecef && has_new_vel_ned) && - (last_sbp_baseline_ecef_msg.tow == last_sbp_vel_ned_msg.tow)) { - - state.last_gps_time_ms = hal.scheduler->millis(); - - //Generate a new lat-lon from baseline - - //Grab the current baseline - Vector3d current_baseline_ecef; //units are currently in mm - current_baseline_ecef[0] = ((double)last_sbp_baseline_ecef_msg.x) / 1000.0; - current_baseline_ecef[1] = ((double)last_sbp_baseline_ecef_msg.y) / 1000.0; - current_baseline_ecef[2] = ((double)last_sbp_baseline_ecef_msg.z) / 1000.0; - - //Offset the reference point from that - Vector3d current_pos_ecef; - current_pos_ecef = base_pos_ecef + current_baseline_ecef; - - Vector3d current_pos_llh; - wgsecef2llh(current_pos_ecef, current_pos_llh); - - current_pos_llh[0] *= RAD_TO_DEG_DOUBLE; - current_pos_llh[1] *= RAD_TO_DEG_DOUBLE; - - state.time_week_ms = last_sbp_baseline_ecef_msg.tow; - state.location.lat = (int32_t) (current_pos_llh[0] * 1e7); - state.location.lng = (int32_t) (current_pos_llh[1] * 1e7); - state.location.alt = (int32_t) (current_pos_llh[2] * 1e3); - state.num_sats = last_sbp_baseline_ecef_msg.n_sats; - - update_state_velocity(); - - has_new_baseline_ecef = false; - - state.status = AP_GPS::GPS_OK_FIX_3D_RTK; - - return true; - } - - - //If we get here, - //We have not been able to update the GPS state yet for this process call. - //Check whether the GPS is still alive and processing messages! - - if (!using_relative_positioning && (now - state.last_gps_time_ms > SBP_FIX_TIMEOUT_MS)) { - state.status = AP_GPS::NO_FIX; - return (now - last_heatbeat_received_ms < SBP_HEARTBEAT_TIMEOUT_MS); - } - - if (now - last_heatbeat_received_ms > SBP_HEARTBEAT_TIMEOUT_MS) { - state.status = AP_GPS::NO_GPS; - return false; - } - - if (state.status < AP_GPS::GPS_OK_FIX_3D) { - //If we are receiving messages, but dont have a fix yet, thats okay. - return has_new_message; - } else { - //If we have a fix and we got here, then we're in between message synchronizations - return false; - } - return true; + return _attempt_state_update(); } void -AP_GPS_SBP::update_state_velocity(void) +AP_GPS_SBP::inject_data(uint8_t *data, uint8_t len) { - state.time_week_ms = last_sbp_vel_ned_msg.tow; - state.velocity[0] = (float)(last_sbp_vel_ned_msg.n / 1000.0); - state.velocity[1] = (float)(last_sbp_vel_ned_msg.e / 1000.0); - state.velocity[2] = (float)(last_sbp_vel_ned_msg.d / 1000.0); - - 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_cd = (int32_t) 100*ToDeg(atan2f(state.velocity[1], state.velocity[0])); - if (state.ground_course_cd < 0) { - state.ground_course_cd += 36000; + if (port->txspace() > len) { + last_injected_data_ms = hal.scheduler->millis(); + port->write(data, len); + } else { + Debug("PIKSI: Not enough TXSPACE"); } - has_new_vel_ned = false; - } -//This attempts to read a SINGLE SBP messages from the incoming port. +//This attempts to reads all SBP messages from the incoming port. //Returns true if a new message was read, false if we failed to read a message. -bool -AP_GPS_SBP::sbp_process() +void +AP_GPS_SBP::_sbp_process() { + while (port->available() > 0) { uint8_t temp = port->read(); uint16_t crc; @@ -415,52 +167,13 @@ AP_GPS_SBP::sbp_process() crc = crc16_ccitt(&(parser_state.msg_len), 1, crc); crc = crc16_ccitt(parser_state.msg_buff, parser_state.msg_len, crc); if (parser_state.crc == crc) { - //OK, we have a valid message. Dispatch the appropriate function: - switch(parser_state.msg_type) { - case SBP_POS_ECEF_MSGTYPE: - sbp_process_pos_ecef(parser_state.msg_buff); - break; - case SBP_POS_LLH_MSGTYPE: - sbp_process_pos_llh(parser_state.msg_buff); - break; - case SBP_BASELINE_ECEF_MSGTYPE: - sbp_process_baseline_ecef(parser_state.msg_buff); - break; - case SBP_BASELINE_NED_MSGTYPE: - sbp_process_baseline_ned(parser_state.msg_buff); - break; - case SBP_VEL_ECEF_MSGTYPE: - sbp_process_vel_ecef(parser_state.msg_buff); - break; - case SBP_VEL_NED_MSGTYPE: - sbp_process_vel_ned(parser_state.msg_buff); - break; - case SBP_GPS_TIME_MSGTYPE: - sbp_process_gpstime(parser_state.msg_buff); - break; - case SBP_DOPS_MSGTYPE: - sbp_process_dops(parser_state.msg_buff); - break; - case SBP_TRACKING_STATE_MSGTYPE: - sbp_process_tracking_state(parser_state.msg_buff, parser_state.msg_len); - break; - case SBP_IAR_STATE_MSGTYPE: - sbp_process_iar_state(parser_state.msg_buff); - break; - case SBP_HEARTBEAT_MSGTYPE: - sbp_process_heartbeat(parser_state.msg_buff); - break; - case SBP_STARTUP_MSGTYPE: - sbp_process_startup(parser_state.msg_buff); - break; - } - return true; - + _sbp_process_message(); } else { Debug("CRC Error Occurred!"); crc_error_counter += 1; } + parser_state.state = sbp_parser_state_t::WAITING; } break; @@ -469,145 +182,154 @@ AP_GPS_SBP::sbp_process() break; } } - //We have parsed all the waiting messages - return false; } + +//INVARIANT: A fully received message with correct CRC is currently in parser_state void -AP_GPS_SBP::sbp_process_heartbeat(uint8_t* msg) -{ - last_heatbeat_received_ms = hal.scheduler->millis(); -} +AP_GPS_SBP::_sbp_process_message() { + switch(parser_state.msg_type) { + case SBP_HEARTBEAT_MSGTYPE: + last_heatbeat_received_ms = hal.scheduler->millis(); + break; -void -AP_GPS_SBP::sbp_process_gpstime(uint8_t* msg) -{ - struct sbp_gps_time_t* t = (struct sbp_gps_time_t*)msg; - state.time_week = t->wn; - state.time_week_ms = t->tow; -} + case SBP_GPS_TIME_MSGTYPE: + last_gps_time = *(struct sbp_gps_time_t*)parser_state.msg_buff; + break; -void -AP_GPS_SBP::sbp_process_dops(uint8_t* msg) -{ - struct sbp_dops_t* d = (struct sbp_dops_t*) msg; - state.time_week_ms = d->tow; - state.hdop = d->hdop; -} + case SBP_VEL_NED_MSGTYPE: + last_vel_ned = *(struct sbp_vel_ned_t*)parser_state.msg_buff; + break; -void -AP_GPS_SBP::sbp_process_pos_ecef(uint8_t* msg) -{ - //Using LLH, not ECEF -} + case SBP_POS_LLH_MSGTYPE: { + struct sbp_pos_llh_t *pos_llh = (struct sbp_pos_llh_t*)parser_state.msg_buff; + // Check if this is a single point or RTK solution + // flags = 0 -> single point + if (pos_llh->flags == 0) { + last_pos_llh_spp = *pos_llh; + } else if (pos_llh->flags == 1 || pos_llh->flags == 2) { + last_pos_llh_rtk = *pos_llh; + } + break; + } -void -AP_GPS_SBP::sbp_process_pos_llh(uint8_t* msg) -{ - struct sbp_pos_llh_t* pos = (struct sbp_pos_llh_t*)msg; - last_sbp_pos_llh_msg = *pos; + case SBP_DOPS_MSGTYPE: + last_dops = *(struct sbp_dops_t*)parser_state.msg_buff; + break; - has_new_pos_llh = true; + case SBP_TRACKING_STATE_MSGTYPE: + //INTENTIONALLY BLANK + //Currenly unhandled, but logged after switch statement. + break; -#if SBP_DEBUGGING || SBP_HW_LOGGING - pos_msg_counter += 1; -#endif - -#if SBP_HW_LOGGING - logging_log_llh(pos); -#endif -} - -void -AP_GPS_SBP::sbp_process_baseline_ecef(uint8_t* msg) -{ - struct sbp_baseline_ecef_t* b = (struct sbp_baseline_ecef_t*)msg; - last_sbp_baseline_ecef_msg = *b; - - last_baseline_received_ms = hal.scheduler->millis(); - has_new_baseline_ecef = true; - -#if SBP_DEBUGGING || SBP_HW_LOGGING - baseline_msg_counter += 1; -#endif - -#if SBP_HW_LOGGING - logging_log_baseline_ecef(b); -#endif -} - -void -AP_GPS_SBP::sbp_process_baseline_ned(uint8_t* msg) -{ - //Currently we use ECEF baselines. - //This is just for logging purposes. - struct sbp_baseline_ned_t* b = (struct sbp_baseline_ned_t*)msg; - last_sbp_baseline_ned_msg = *b; - -} - -void -AP_GPS_SBP::sbp_process_vel_ecef(uint8_t* msg) -{ - //Currently we use NED velocity. -} - -void -AP_GPS_SBP::sbp_process_vel_ned(uint8_t* msg) -{ - struct sbp_vel_ned_t* vel = (struct sbp_vel_ned_t*)msg; - last_sbp_vel_ned_msg = *vel; - - has_new_vel_ned = true; - -#if SBP_DEBUGGING || SBP_HW_LOGGING - vel_msg_counter += 1; -#endif -} - -void -AP_GPS_SBP::sbp_process_tracking_state(uint8_t* msg, uint8_t len) -{ - uint32_t now = hal.scheduler->millis(); - - struct sbp_tracking_state_t* tracking_state = (struct sbp_tracking_state_t*)msg; - last_sbp_tracking_state_msg = *tracking_state; - - uint8_t num = len / sizeof(sbp_tracking_state_t); - last_sbp_tracking_state_msg_num = num; - - //Rate-limit the tracking state messages to no more than 1.8 seconds - if (now - last_tracking_state_ms > SBP_MILLIS_BETWEEN_TRACKING_LOG) { - last_tracking_state_ms = now; - -#ifdef SBP_HW_LOGGING - logging_log_tracking_state(tracking_state, num); -#endif + case SBP_IAR_STATE_MSGTYPE: { + sbp_iar_state_t *iar = (struct sbp_iar_state_t*)parser_state.msg_buff; + last_iar_num_hypotheses = iar->num_hypotheses; + break; + } + default: + // Break out of any logging if it's an unsupported message + return; } -} - -void -AP_GPS_SBP::sbp_process_iar_state(uint8_t* msg) -{ - struct sbp_iar_state_t* iar_state = (struct sbp_iar_state_t*)msg; - iar_num_hypotheses = (int32_t) iar_state->num_hypotheses; -} - -void -AP_GPS_SBP::sbp_process_startup(uint8_t* msg) -{ - invalidate_base_pos(); + logging_log_raw_sbp(parser_state.msg_type, parser_state.sender_id, parser_state.msg_len, parser_state.msg_buff); } bool -AP_GPS_SBP::_detect(struct SBP_detect_state &state, uint8_t data) +AP_GPS_SBP::_attempt_state_update() { - //This switch reads one character at a time, - //if we find something that looks like our preamble - //we'll try to read the full message length, calculating the CRC. - //If the CRC matches, we have a SBP GPS! + + // If we currently have heartbeats + // - NO FIX + // + // If we have a full update available, save it + // + uint32_t now = hal.scheduler->millis(); + bool ret = false; + + if (now - last_heatbeat_received_ms > SBP_TIMEOUT_HEATBEAT) { + + state.status = AP_GPS::NO_GPS; + Debug("No Heartbeats from Piksi! Driver Ready to Die!"); + ret = false; + + } 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 / 1000.0); + state.velocity[1] = (float)(last_vel_ned.e / 1000.0); + state.velocity[2] = (float)(last_vel_ned.d / 1000.0); + + 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_cd = (int32_t) 100*ToDeg(atan2f(state.velocity[1], state.velocity[0])); + if (state.ground_course_cd < 0) { + state.ground_course_cd += 36000; + } + + // Update position state + + state.location.lat = (int32_t) (pos_llh->lat*1e7); + state.location.lng = (int32_t) (pos_llh->lon*1e7); + state.location.alt = (int32_t) (pos_llh->height*1e2); + 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_DGPS; + else if (pos_llh->flags == 1) + state.status = AP_GPS::GPS_OK_FIX_3D_RTK; + + + 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. + ret = false; + + } + + + return ret; + +} + + + +bool +AP_GPS_SBP::_detect(struct SBP_detect_state &state, uint8_t data) +{ + // This switch reads one character at a time, if we find something that + // looks like our preamble we'll try to read the full message length, + // calculating the CRC. If the CRC matches, we have an SBP GPS! + switch(state.state) { case SBP_detect_state::WAITING: if (data == SBP_PREAMBLE) { @@ -667,61 +389,6 @@ AP_GPS_SBP::_detect(struct SBP_detect_state &state, uint8_t data) return false; } -void -AP_GPS_SBP::send_mavlink_gps_rtk(mavlink_channel_t chan) -{ - - uint8_t health = dgps_corrections_incoming | - (rtk_corrections_incoming << 1) | - (has_rtk_base_pos << 2); - - mavlink_msg_gps_rtk_send( - chan, - last_baseline_received_ms, // Time since boot of last baseline message received in ms. - AP_GPS::GPS_TYPE_SBP, // Identification of connected RTK receiver. - state.time_week, // GPS Week Number of last baseline - last_sbp_baseline_ned_msg.tow, // GPS Time of Week of last baseline - health, // GPS-specific health report for RTK data. - baseline_recv_rate, // Rate of baseline messages being received by GPS, in HZ*10 - last_sbp_baseline_ned_msg.n_sats, // Current number of sats used for RTK calculation. - 1, // Coordinate system of baseline. 0 == ECEF, 1 == NED - last_sbp_baseline_ned_msg.n, // Current baseline in ECEF x or NED north component in mm - last_sbp_baseline_ned_msg.e, // Current baseline in ECEF y or NED east component in mm - last_sbp_baseline_ned_msg.d, // Current baseline in ECEF z or NED down component in mm - last_sbp_baseline_ned_msg.h_accuracy, // Current estimate of baseline accuracy. - iar_num_hypotheses // Current number of integer ambiguity hypotheses. - ); - -} - -#if GPS_MAX_INSTANCES > 1 -void -AP_GPS_SBP::send_mavlink_gps2_rtk(mavlink_channel_t chan) -{ - - uint8_t health = dgps_corrections_incoming | - (rtk_corrections_incoming << 1) | - (has_rtk_base_pos << 2); - - mavlink_msg_gps2_rtk_send( - chan, - last_baseline_received_ms, // Time since boot of last baseline message received in ms. - AP_GPS::GPS_TYPE_SBP, // Identification of connected RTK receiver. - state.time_week, // GPS Week Number of last baseline - last_sbp_baseline_ned_msg.tow, // GPS Time of Week of last baseline - health, // GPS-specific health report for RTK data. - baseline_recv_rate, // Rate of baseline messages being received by GPS, in HZ*10 - last_sbp_baseline_ned_msg.n_sats, // Current number of sats used for RTK calculation. - 1, // Coordinate system of baseline. 0 == ECEF, 1 == NED - last_sbp_baseline_ned_msg.n, // Current baseline in ECEF x or NED north component in mm - last_sbp_baseline_ned_msg.e, // Current baseline in ECEF y or NED east component in mm - last_sbp_baseline_ned_msg.d, // Current baseline in ECEF z or NED down component in mm - last_sbp_baseline_ned_msg.h_accuracy, // Current estimate of baseline accuracy. - iar_num_hypotheses // Current number of integer ambiguity hypotheses. - ); -} -#endif - #if SBP_HW_LOGGING #define LOG_MSG_SBPHEALTH 202 @@ -730,19 +397,8 @@ AP_GPS_SBP::send_mavlink_gps2_rtk(mavlink_channel_t chan) #define LOG_MSG_SBPTRACKING1 205 #define LOG_MSG_SBPTRACKING2 206 -struct PACKED log_SbpHealth { - LOG_PACKET_HEADER; - uint32_t timestamp; - float pos_msg_hz; - float vel_msg_hz; - float baseline_msg_hz; - float full_update_hz; - uint32_t crc_error_counter; - uint8_t dgps_corrections_incoming; - uint8_t rtk_corrections_incoming; - uint8_t has_rtk_base_pos; - int32_t iar_num_hypotheses; -}; +#define LOG_MSG_SBPRAW1 207 +#define LOG_MSG_SBPRAW2 208 struct PACKED log_SbpLLH { LOG_PACKET_HEADER; @@ -752,74 +408,44 @@ struct PACKED log_SbpLLH { int32_t lon; int32_t alt; uint8_t n_sats; + uint8_t flags; }; -struct PACKED log_SbpBaseline { +struct PACKED log_SbpHealth { LOG_PACKET_HEADER; uint32_t timestamp; - uint32_t tow; //< GPS Time of Week of ECEF Baseline (unit: ms) - int32_t x; //< Baseline ECEF X coordinate - int32_t y; //< Baseline ECEF Y coordinate - int32_t z; //< Baseline ECEF Z coordinate - int32_t length; //< Baseline length - uint16_t accuracy; //< Horizontal position accuracy estimate (unit: mm) - uint8_t n_sats; //< Number of satellites used in solution - uint8_t flags; //< Status flags (reserved) + uint32_t crc_error_counter; + uint32_t last_injected_data_ms; + uint32_t last_iar_num_hypotheses; }; -struct PACKED log_SbpTracking1 { +struct PACKED log_SbpRAW1 { LOG_PACKET_HEADER; uint32_t timestamp; - uint8_t ch1_prn; - float ch1_cn0; - uint8_t ch2_prn; - float ch2_cn0; - uint8_t ch3_prn; - float ch3_cn0; - uint8_t ch4_prn; - float ch4_cn0; - uint8_t ch5_prn; - float ch5_cn0; - uint8_t ch6_prn; - float ch6_cn0; - uint8_t ch7_prn; - float ch7_cn0; + uint16_t msg_type; + uint16_t sender_id; + uint8_t msg_len; + uint8_t data1[64]; }; -struct PACKED log_SbpTracking2 { +struct PACKED log_SbpRAW2 { LOG_PACKET_HEADER; uint32_t timestamp; - uint8_t ch8_prn; - float ch8_cn0; - uint8_t ch9_prn; - float ch9_cn0; - uint8_t ch10_prn; - float ch10_cn0; - uint8_t ch11_prn; - float ch11_cn0; - uint8_t ch12_prn; - float ch12_cn0; - uint8_t ch13_prn; - float ch13_cn0; - uint8_t ch14_prn; - float ch14_cn0; + uint16_t msg_type; + uint8_t data2[192]; }; + static const struct LogStructure sbp_log_structures[] PROGMEM = { { LOG_MSG_SBPHEALTH, sizeof(log_SbpHealth), - "SBPH", "IffffIBBBi", "TimeMS,PHz,VHz,BHz,UpHz,CrcError,dgpsOn,rtkOn,hasRtkBase,IAR" }, - { LOG_MSG_SBPLLH, sizeof(log_SbpLLH), - "SBPL", "IIiiiB", "TimeMS,tow,lat,lon,alt,num_sats" }, - { LOG_MSG_SBPBASELINE, sizeof(log_SbpBaseline), - "SBPB", "IIiiiiHBB", "TimeMS,tow,x,y,z,len,acc,num_sats,flags" }, - { LOG_MSG_SBPTRACKING1, sizeof(log_SbpTracking1), - "SBT1", "IBfBfBfBfBfBfBf", "TimeMS,s1,c1,s2,c2,s3,c3,s4,c4,s5,c5,s6,c6,s7,c7" }, - { LOG_MSG_SBPTRACKING2, sizeof(log_SbpTracking2), - "SBT2", "IBfBfBfBfBfBfBf", "TimeMS,s8,c8,s9,c9,s10,c10,s11,c11,s12,c12,s13,c13,s14,c14" } - + "SBPH", "IIII", "TimeMS,CrcError,LastInject,IARhyp" }, + { LOG_MSG_SBPRAW1, sizeof(log_SbpRAW1), + "SBR1", "IHHBZ", "TimeMS,msg_type,sender_id,msg_len,d1" }, + { LOG_MSG_SBPRAW2, sizeof(log_SbpRAW2), + "SBR2", "IHZZZ", "TimeMS,msg_type,d2,d3,d4" } }; -void +void AP_GPS_SBP::logging_write_headers(void) { if (!logging_started) { @@ -829,7 +455,7 @@ AP_GPS_SBP::logging_write_headers(void) } void -AP_GPS_SBP::logging_log_health(float pos_msg_hz, float vel_msg_hz, float baseline_msg_hz, float full_update_hz) +AP_GPS_SBP::logging_log_full_update() { if (gps._DataFlash == NULL || !gps._DataFlash->logging_started()) { @@ -840,133 +466,59 @@ AP_GPS_SBP::logging_log_health(float pos_msg_hz, float vel_msg_hz, float baselin struct log_SbpHealth pkt = { LOG_PACKET_HEADER_INIT(LOG_MSG_SBPHEALTH), - timestamp : hal.scheduler->millis(), - pos_msg_hz : pos_msg_hz, - vel_msg_hz : vel_msg_hz, - baseline_msg_hz : baseline_msg_hz, - full_update_hz : full_update_hz, + timestamp : hal.scheduler->millis(), crc_error_counter : crc_error_counter, - dgps_corrections_incoming : dgps_corrections_incoming, - rtk_corrections_incoming : rtk_corrections_incoming, - has_rtk_base_pos : has_rtk_base_pos, - iar_num_hypotheses : iar_num_hypotheses + last_injected_data_ms : last_injected_data_ms, + last_iar_num_hypotheses : last_iar_num_hypotheses, }; gps._DataFlash->WriteBlock(&pkt, sizeof(pkt)); }; void -AP_GPS_SBP::logging_log_llh(struct sbp_pos_llh_t* p) -{ +AP_GPS_SBP::logging_log_raw_sbp(uint16_t msg_type, + uint16_t sender_id, + uint8_t msg_len, + uint8_t *msg_buff) { if (gps._DataFlash == NULL || !gps._DataFlash->logging_started()) { return; } - logging_write_headers(); - - struct log_SbpLLH pkt = { - LOG_PACKET_HEADER_INIT(LOG_MSG_SBPLLH), - timestamp : hal.scheduler->millis(), - tow : p->tow, - lat : (int32_t) (p->lat*1e7), - lon : (int32_t) (p->lon*1e7), - alt : (int32_t) (p->height*1e2), - n_sats : p->n_sats, - }; - gps._DataFlash->WriteBlock(&pkt, sizeof(pkt)); - -}; - -void -AP_GPS_SBP::logging_log_baseline_ecef(struct sbp_baseline_ecef_t* b) -{ - - if (gps._DataFlash == NULL || !gps._DataFlash->logging_started()) { - return; + //MASK OUT MESSAGES WE DON'T WANT TO LOG + if (( ((uint16_t) gps._sbp_logmask) & msg_type) == 0) { + return; } logging_write_headers(); - float x = b->x / 1000.0; - float y = b->y / 1000.0; - float z = b->z / 1000.0; - int32_t len = (int32_t) (safe_sqrt(x*x+y*y+z*z) * 1000.0f); + uint32_t now = hal.scheduler->millis(); - struct log_SbpBaseline pkt = { - LOG_PACKET_HEADER_INIT(LOG_MSG_SBPBASELINE), - timestamp : hal.scheduler->millis(), - tow : b->tow, - x : b->x, - y : b->y, - z : b->z, - length : len, - accuracy : b->accuracy, - n_sats : b->n_sats, - flags : b->flags + struct log_SbpRAW1 pkt = { + LOG_PACKET_HEADER_INIT(LOG_MSG_SBPRAW1), + timestamp : now, + msg_type : msg_type, + sender_id : sender_id, + msg_len : msg_len, }; - + memcpy(pkt.data1, msg_buff, max(msg_len,64)); gps._DataFlash->WriteBlock(&pkt, sizeof(pkt)); -}; + if (msg_len > 64) { - -void -AP_GPS_SBP::logging_log_tracking_state(struct sbp_tracking_state_t* tstate, uint8_t num) -{ - if (gps._DataFlash == NULL || !gps._DataFlash->logging_started()) { - return; - } - - logging_write_headers(); - - struct log_SbpTracking1 pkt = { - LOG_PACKET_HEADER_INIT(LOG_MSG_SBPTRACKING1), - timestamp : hal.scheduler->millis(), - ch1_prn : tstate[0].prn, - ch1_cn0 : tstate[0].cn0, - ch2_prn : (uint8_t)(num < 1 ? 0 : tstate[1].prn), - ch2_cn0 : num < 1 ? 0 : tstate[1].cn0, - ch3_prn : (uint8_t)(num < 2 ? 0 : tstate[2].prn), - ch3_cn0 : num < 2 ? 0 : tstate[2].cn0, - ch4_prn : (uint8_t)(num < 3 ? 0 : tstate[3].prn), - ch4_cn0 : num < 3 ? 0 : tstate[3].cn0, - ch5_prn : (uint8_t)(num < 4 ? 0 : tstate[4].prn), - ch5_cn0 : num < 4 ? 0 : tstate[4].cn0, - ch6_prn : (uint8_t)(num < 5 ? 0 : tstate[5].prn), - ch6_cn0 : num < 5 ? 0 : tstate[5].cn0, - ch7_prn : (uint8_t)(num < 6 ? 0 : tstate[6].prn), - ch7_cn0 : num < 6 ? 0 : tstate[6].cn0, - }; - gps._DataFlash->WriteBlock(&pkt, sizeof(pkt)); - - if (num > 6) { - - struct log_SbpTracking2 pkt2 = { - LOG_PACKET_HEADER_INIT(LOG_MSG_SBPTRACKING2), - timestamp : hal.scheduler->millis(), - ch8_prn : (uint8_t)(num < 7 ? 0 : tstate[7].prn), - ch8_cn0 : num < 7 ? 0 : tstate[7].cn0, - ch9_prn : (uint8_t)(num < 8 ? 0 : tstate[8].prn), - ch9_cn0 : num < 8 ? 0 : tstate[8].cn0, - ch10_prn : (uint8_t)(num < 9 ? 0 : tstate[9].prn), - ch10_cn0 : num < 9 ? 0 : tstate[9].cn0, - ch11_prn : (uint8_t)(num < 10 ? 0 : tstate[10].prn), - ch11_cn0 : num < 10 ? 0 : tstate[10].cn0, - ch12_prn : (uint8_t)(num < 11 ? 0 : tstate[11].prn), - ch12_cn0 : num < 11 ? 0 : tstate[11].cn0, - ch13_prn : (uint8_t)(num < 12 ? 0 : tstate[12].prn), - ch13_cn0 : num < 12 ? 0 : tstate[12].cn0, - ch14_prn : (uint8_t)(num < 13 ? 0 : tstate[13].prn), - ch14_cn0 : num < 13 ? 0 : tstate[13].cn0, + struct log_SbpRAW2 pkt2 = { + LOG_PACKET_HEADER_INIT(LOG_MSG_SBPRAW2), + timestamp : now, + msg_type : msg_type, }; - gps._DataFlash->WriteBlock(&pkt2, sizeof(pkt)); - - }; + memcpy(pkt2.data2, &msg_buff[64], msg_len - 64); + gps._DataFlash->WriteBlock(&pkt2, sizeof(pkt2)); + } }; + #endif // SBP_HW_LOGGING -#endif // GPS_RTK_AVAILABLE +#endif // GPS_RTK_AVAILABLE \ No newline at end of file diff --git a/libraries/AP_GPS/AP_GPS_SBP.h b/libraries/AP_GPS/AP_GPS_SBP.h index 4370e5c5c3..d2d445bcc7 100644 --- a/libraries/AP_GPS/AP_GPS_SBP.h +++ b/libraries/AP_GPS/AP_GPS_SBP.h @@ -18,59 +18,31 @@ // Swift Navigation SBP GPS driver for ArduPilot. // Code by Niels Joubert // -// Swift Binary Protocol format: http://docs.swift-nav.com/libswiftnav +// Swift Binary Protocol format: http://docs.swift-nav.com/ +// #ifndef __AP_GPS_SBP_H__ #define __AP_GPS_SBP_H__ -#if GPS_RTK_AVAILABLE - #include - -//OVERALL DESIGN NOTES. -// Niels Joubert, April 2014 -// -// -// REQUIREMENTS: -// 1) We need to update the entire state structure "atomically", -// which is indicated by returning "true" from read(). -// -// - We use sticky bits to track when each part is updated -// and return true when all the sticky bits are set -// -// 2) We want to minimize memory usage in the detection routine -// -// - We use a stripped-down version of the sbp_parser_state_t struct -// that does not bother to decode the message, it just tracks the CRC. -// - class AP_GPS_SBP : public AP_GPS_Backend { public: - AP_GPS_SBP(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port); - - bool can_calculate_base_pos(void); - - void calculate_base_pos(void); - - void invalidate_base_pos(void); + AP_GPS_SBP(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port); AP_GPS::GPS_Status highest_supported_status(void) { return AP_GPS::GPS_OK_FIX_3D_RTK; } + // Methods bool read(); + void inject_data(uint8_t *data, uint8_t len); + static bool _detect(struct SBP_detect_state &state, uint8_t data); - virtual void send_mavlink_gps_rtk(mavlink_channel_t chan); - -#if GPS_MAX_INSTANCES > 1 - virtual void send_mavlink_gps2_rtk(mavlink_channel_t chan); -#endif - private: - // ************************************************************************ + // ************************************************************************ // Swift Navigation SBP protocol types and definitions // ************************************************************************ @@ -107,13 +79,14 @@ private: static const uint16_t SBP_TRACKING_STATE_MSGTYPE = 0x0016; static const uint16_t SBP_IAR_STATE_MSGTYPE = 0x0019; + // GPS Time struct PACKED sbp_gps_time_t { uint16_t wn; //< GPS week number (unit: weeks) uint32_t tow; //< GPS Time of Week rounded to the nearest ms (unit: ms) int32_t ns; //< Nanosecond remainder of rounded tow (unit: ns) uint8_t flags; //< Status flags (reserved) - }; + }; // 11 bytes // Dilution of Precision struct PACKED sbp_dops_t { @@ -123,18 +96,7 @@ private: uint16_t tdop; //< Time Dilution of Precision (unit: 0.01) uint16_t hdop; //< Horizontal Dilution of Precision (unit: 0.01) uint16_t vdop; //< Vertical Dilution of Precision (unit: 0.01) - }; - - // Position solution in absolute Earth Centered Earth Fixed (ECEF) coordinates. - struct PACKED sbp_pos_ecef_t { - uint32_t tow; //< GPS Time of Week (unit: ms) - double x; //< ECEF X coordinate (unit: meters) - double y; //< ECEF Y coordinate (unit: meters) - double z; //< ECEF Z coordinate (unit: meters) - uint16_t accuracy; //< Position accuracy estimate (unit: mm) - uint8_t n_sats; //< Number of satellites used in solution - uint8_t flags; //< Status flags - }; + }; // 14 bytes // Geodetic position solution. struct PACKED sbp_pos_llh_t { @@ -146,41 +108,7 @@ private: uint16_t v_accuracy; //< Vertical position accuracy estimate (unit: mm) uint8_t n_sats; //< Number of satellites used in solution uint8_t flags; //< Status flags - }; - - // Baseline in Earth Centered Earth Fixed (ECEF) coordinates. - struct PACKED sbp_baseline_ecef_t { - uint32_t tow; //< GPS Time of Week (unit: ms) - int32_t x; //< Baseline ECEF X coordinate (unit: mm) - int32_t y; //< Baseline ECEF Y coordinate (unit: mm) - int32_t z; //< Baseline ECEF Z coordinate (unit: mm) - uint16_t accuracy; //< Position accuracy estimate (unit: mm) - uint8_t n_sats; //< Number of satellites used in solution - uint8_t flags; //< Status flags (reserved) - }; - - // Baseline in local North East Down (NED) coordinates. - struct PACKED sbp_baseline_ned_t { - uint32_t tow; //< GPS Time of Week (unit: ms) - int32_t n; //< Baseline North coordinate (unit: mm) - int32_t e; //< Baseline East coordinate (unit: mm) - int32_t d; //< Baseline Down coordinate (unit: mm) - uint16_t h_accuracy; //< Horizontal position accuracy estimate (unit: mm) - uint16_t v_accuracy; //< Vertical position accuracy estimate (unit: mm) - uint8_t n_sats; //< Number of satellites used in solution - uint8_t flags; //< Status flags (reserved) - }; - - //Velocity in Earth Centered Earth Fixed (ECEF) coordinates. - struct PACKED sbp_vel_ecef_t { - uint32_t tow; //< GPS Time of Week (unit: ms) - int32_t x; //< Velocity ECEF X coordinate (unit: mm/s) - int32_t y; //< Velocity ECEF Y coordinate (unit: mm/s) - int32_t z; //< Velocity ECEF Z coordinate (unit: mm/s) - uint16_t accuracy; //< Velocity accuracy estimate (unit: mm/s) - uint8_t n_sats; //< Number of satellites used in solution - uint8_t flags; //< Status flags (reserved) - }; + }; // 34 bytes // Velocity in NED Velocity in local North East Down (NED) coordinates. struct PACKED sbp_vel_ned_t { @@ -192,7 +120,7 @@ private: uint16_t v_accuracy; //< Vertical velocity accuracy estimate (unit: mm/s) uint8_t n_sats; //< Number of satellites used in solution uint8_t flags; //< Status flags (reserved) - }; + }; // 22 bytes // Activity and Signal-to-Noise data of a tracking channel on the GPS. struct PACKED sbp_tracking_state_t { @@ -206,74 +134,31 @@ private: uint32_t num_hypotheses; }; - + void _sbp_process(); + void _sbp_process_message(); + bool _attempt_state_update(); // ************************************************************************ - // Swift Navigation SBP protocol parsing and processing + // Internal Received Messages State // ************************************************************************ - - //Pulls data from the port, dispatches messages to processing functions - //Returns true if a new message was successfully decoded. - bool sbp_process(); - - bool update_state(bool has_new_message); - - void update_state_velocity(void); - - //Processes individual messages - //When a message is received, it sets a sticky bit that it has updated - //itself. This is used to track when a full update of GPS_State has occurred - void sbp_process_heartbeat(uint8_t* msg); - void sbp_process_gpstime(uint8_t* msg); - void sbp_process_dops(uint8_t* msg); - void sbp_process_pos_ecef(uint8_t* msg); - void sbp_process_pos_llh(uint8_t* msg); - void sbp_process_baseline_ecef(uint8_t* msg); - void sbp_process_baseline_ned(uint8_t* msg); - void sbp_process_vel_ecef(uint8_t* msg); - void sbp_process_vel_ned(uint8_t* msg); - void sbp_process_tracking_state(uint8_t* msg, uint8_t len); - void sbp_process_iar_state(uint8_t* msg); - void sbp_process_startup(uint8_t* msg); - - - //Internal last-received-messages - sbp_pos_llh_t last_sbp_pos_llh_msg; - sbp_vel_ned_t last_sbp_vel_ned_msg; - sbp_baseline_ecef_t last_sbp_baseline_ecef_msg; - sbp_baseline_ned_t last_sbp_baseline_ned_msg; - sbp_tracking_state_t last_sbp_tracking_state_msg; - uint8_t last_sbp_tracking_state_msg_num; - - //Tracking GPS health and received time-of-week - uint32_t last_baseline_received_ms; uint32_t last_heatbeat_received_ms; - uint32_t last_tracking_state_ms; - int32_t iar_num_hypotheses; - uint8_t baseline_recv_rate; //in hertz * 10 + uint32_t last_injected_data_ms; - //Sticky bits to track updating of state - bool dgps_corrections_incoming:1; - bool rtk_corrections_incoming:1; + struct sbp_gps_time_t last_gps_time; + struct sbp_dops_t last_dops; + struct sbp_pos_llh_t last_pos_llh_spp; + struct sbp_pos_llh_t last_pos_llh_rtk; + struct sbp_vel_ned_t last_vel_ned; + uint32_t last_iar_num_hypotheses; - bool has_new_pos_llh:1; - bool has_new_vel_ned:1; - bool has_new_baseline_ecef:1; + uint32_t last_full_update_tow; + uint32_t last_full_update_cpu_ms; - //RTK-specific relative-to-absolute positioning - bool has_rtk_base_pos:1; - Vector3d base_pos_ecef; - // ************************************************************************ // Monitoring and Performance Counting // ************************************************************************ - uint8_t pos_msg_counter; - uint8_t vel_msg_counter; - uint8_t baseline_msg_counter; - uint8_t full_update_counter; uint32_t crc_error_counter; - uint32_t last_healthcheck_millis; // ************************************************************************ // Logging to DataFlash @@ -283,14 +168,10 @@ private: static bool logging_started; void logging_write_headers(); - - void logging_log_health(float pos_msg_hz, float vel_msg_hz, float baseline_msg_hz, float full_update_hz); - void logging_log_llh(struct sbp_pos_llh_t* p); - void logging_log_baseline_ecef(struct sbp_baseline_ecef_t*); - void logging_log_tracking_state(struct sbp_tracking_state_t*, uint8_t num); + void logging_log_full_update(); + void logging_log_raw_sbp(uint16_t msg_type, uint16_t sender_id, uint8_t msg_len, uint8_t *msg_buff); + }; -#endif // GPS_RTK_AVAILABLE - -#endif // __AP_GPS_SBP_H__ +#endif // __AP_GPS_SBP_H__ diff --git a/libraries/AP_GPS/GPS_Backend.h b/libraries/AP_GPS/GPS_Backend.h index 092c7a440e..b88bbdb67e 100644 --- a/libraries/AP_GPS/GPS_Backend.h +++ b/libraries/AP_GPS/GPS_Backend.h @@ -37,16 +37,9 @@ public: // valid packet from the GPS. virtual bool read() = 0; + virtual void inject_data(uint8_t *data, uint8_t len) { return; } + #if GPS_RTK_AVAILABLE - - // true once an RTK GPS Driver has a converged baseline vector and - // absolute single point solution to enter into an RTK Fix. - virtual bool can_calculate_base_pos(void) { return false; }; - - // tells a RTK GPS Driver to capture the current single-point solution - // and baseline solution as the current home data. - virtual void calculate_base_pos(void) {}; - // Highest status supported by this GPS. // Allows external system to identify type of receiver connected. virtual AP_GPS::GPS_Status highest_supported_status(void) { return AP_GPS::GPS_OK_FIX_3D; }