/* This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see . */ #include "AP_GPS.h" #include "GPS_Backend.h" #include #include #include #include #define GPS_BACKEND_DEBUGGING 0 #if GPS_BACKEND_DEBUGGING # 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 #include #if AP_GPS_DEBUG_LOGGING_ENABLED #include #endif extern const AP_HAL::HAL& hal; AP_GPS_Backend::AP_GPS_Backend(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) : port(_port), gps(_gps), state(_state) { state.have_speed_accuracy = false; state.have_horizontal_accuracy = false; state.have_vertical_accuracy = false; } int32_t AP_GPS_Backend::swap_int32(int32_t v) const { const uint8_t *b = (const uint8_t *)&v; union { int32_t v; uint8_t b[4]; } u; u.b[0] = b[3]; u.b[1] = b[2]; u.b[2] = b[1]; u.b[3] = b[0]; return u.v; } int16_t AP_GPS_Backend::swap_int16(int16_t v) const { const uint8_t *b = (const uint8_t *)&v; union { int16_t v; uint8_t b[2]; } u; u.b[0] = b[1]; u.b[1] = b[0]; return u.v; } /** fill in time_week_ms and time_week from BCD date and time components assumes MTK19 millisecond form of bcd_time */ void AP_GPS_Backend::make_gps_time(uint32_t bcd_date, uint32_t bcd_milliseconds) { struct tm tm {}; tm.tm_year = 100U + bcd_date % 100U; tm.tm_mon = ((bcd_date / 100U) % 100U)-1; tm.tm_mday = bcd_date / 10000U; uint32_t v = bcd_milliseconds; uint16_t msec = v % 1000U; v /= 1000U; tm.tm_sec = v % 100U; v /= 100U; tm.tm_min = v % 100U; v /= 100U; tm.tm_hour = v % 100U; // convert from time structure to unix time time_t unix_time = AP::rtc().mktime(&tm); // convert to time since GPS epoch const uint32_t unix_to_GPS_secs = 315964800UL; const uint16_t leap_seconds_unix = GPS_LEAPSECONDS_MILLIS/1000U; uint32_t ret = unix_time + leap_seconds_unix - unix_to_GPS_secs; // get GPS week and time state.time_week = ret / AP_SEC_PER_WEEK; state.time_week_ms = (ret % AP_SEC_PER_WEEK) * AP_MSEC_PER_SEC; state.time_week_ms += msec; } /* get the last time of week in ms */ uint32_t AP_GPS_Backend::get_last_itow_ms(void) const { if (!_have_itow) { return state.time_week_ms; } return (_pseudo_itow_delta_ms == 0)?(_last_itow_ms):((_pseudo_itow/1000ULL) + _pseudo_itow_delta_ms); } /* fill in 3D velocity for a GPS that doesn't give vertical velocity numbers */ void AP_GPS_Backend::fill_3d_velocity(void) { float gps_heading = radians(state.ground_course); state.velocity.x = state.ground_speed * cosf(gps_heading); state.velocity.y = state.ground_speed * sinf(gps_heading); state.velocity.z = 0; state.have_vertical_velocity = false; } /* fill in 3D velocity for a GPS that doesn't give vertical velocity numbers */ void AP_GPS_Backend::velocity_to_speed_course(AP_GPS::GPS_State &s) { s.ground_course = wrap_360(degrees(atan2f(s.velocity.y, s.velocity.x))); s.ground_speed = s.velocity.xy().length(); } void AP_GPS_Backend::inject_data(const uint8_t *data, uint16_t len) { // not all backends have valid ports if (port != nullptr) { if (port->txspace() > len) { port->write(data, len); } else { Debug("GPS %d: Not enough TXSPACE", state.instance + 1); } } } void AP_GPS_Backend::_detection_message(char *buffer, const uint8_t buflen) const { const uint8_t instance = state.instance; const struct AP_GPS::detect_state dstate = gps.detect_state[instance]; if (dstate.auto_detected_baud) { hal.util->snprintf(buffer, buflen, "GPS %d: detected as %s at %d baud", instance + 1, name(), int(gps._baudrates[dstate.current_baud])); } else { hal.util->snprintf(buffer, buflen, "GPS %d: specified as %s", instance + 1, name()); } } void AP_GPS_Backend::broadcast_gps_type() const { char buffer[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1]; _detection_message(buffer, sizeof(buffer)); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s", buffer); } void AP_GPS_Backend::Write_AP_Logger_Log_Startup_messages() const { #if HAL_LOGGING_ENABLED char buffer[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1]; _detection_message(buffer, sizeof(buffer)); AP::logger().Write_Message(buffer); #endif } bool AP_GPS_Backend::should_log() const { return gps.should_log(); } void AP_GPS_Backend::send_mavlink_gps_rtk(mavlink_channel_t chan) { #if HAL_GCS_ENABLED const uint8_t instance = state.instance; // send status switch (instance) { case 0: mavlink_msg_gps_rtk_send(chan, 0, // Not implemented yet 0, // Not implemented yet state.rtk_week_number, state.rtk_time_week_ms, 0, // Not implemented yet 0, // Not implemented yet state.rtk_num_sats, state.rtk_baseline_coords_type, state.rtk_baseline_x_mm, state.rtk_baseline_y_mm, state.rtk_baseline_z_mm, state.rtk_accuracy, state.rtk_iar_num_hypotheses); break; case 1: mavlink_msg_gps2_rtk_send(chan, 0, // Not implemented yet 0, // Not implemented yet state.rtk_week_number, state.rtk_time_week_ms, 0, // Not implemented yet 0, // Not implemented yet state.rtk_num_sats, state.rtk_baseline_coords_type, state.rtk_baseline_x_mm, state.rtk_baseline_y_mm, state.rtk_baseline_z_mm, state.rtk_accuracy, state.rtk_iar_num_hypotheses); break; } #endif } /* set a timestamp based on arrival time on uart at current byte, assuming the message started nbytes ago */ void AP_GPS_Backend::set_uart_timestamp(uint16_t nbytes) { if (port) { state.last_corrected_gps_time_us = port->receive_time_constraint_us(nbytes); state.corrected_timestamp_updated = true; } } void AP_GPS_Backend::check_new_itow(uint32_t itow, uint32_t msg_length) { if (itow != _last_itow_ms) { _last_itow_ms = itow; _have_itow = true; /* we need to calculate a pseudo-itow, which copes with the iTow from the GPS changing in unexpected ways. We assume that timestamps from the GPS are always in multiples of 50ms. That means we can't handle a GPS with an update rate of more than 20Hz. We could do more, but we'd need the GPS poll time to be higher */ const uint32_t gps_min_period_ms = 50; // get the time the packet arrived on the UART uint64_t uart_us; if (_last_pps_time_us != 0 && (state.status >= AP_GPS::GPS_OK_FIX_2D)) { // pps is only reliable when we have some sort of GPS fix uart_us = _last_pps_time_us; _last_pps_time_us = 0; } else if (port) { uart_us = port->receive_time_constraint_us(msg_length); } else { uart_us = AP_HAL::micros64(); } uint32_t now = AP_HAL::millis(); uint32_t dt_ms = now - _last_ms; _last_ms = now; // round to nearest 50ms period dt_ms = ((dt_ms + (gps_min_period_ms/2)) / gps_min_period_ms) * gps_min_period_ms; // work out an actual message rate. If we get 5 messages in a // row with a new rate we switch rate if (_last_rate_ms == dt_ms) { if (_rate_counter < 5) { _rate_counter++; } else if (_rate_ms != dt_ms) { _rate_ms = dt_ms; } } else { _rate_counter = 0; _last_rate_ms = dt_ms; if (_rate_ms != 0) { set_pps_desired_freq(1000/_rate_ms); } } if (_rate_ms == 0) { // only allow 5Hz to 20Hz in user config _rate_ms = constrain_int16(gps.get_rate_ms(state.instance), 50, 200); } // round to calculated message rate dt_ms = ((dt_ms + (_rate_ms/2)) / _rate_ms) * _rate_ms; // calculate pseudo-itow _pseudo_itow += dt_ms * 1000U; // use msg arrival time, and correct for jitter uint64_t local_us = jitter_correction.correct_offboard_timestamp_usec(_pseudo_itow, uart_us); state.last_corrected_gps_time_us = local_us; state.corrected_timestamp_updated = true; #ifndef HAL_BUILD_AP_PERIPH // look for lagged data from the GPS. This is meant to detect // the case that the GPS is trying to push more data into the // UART than can fit (eg. with GPS_RAW_DATA at 115200). // This is disabled on AP_Periph as it is better to catch missed packet rate at the flight // controller level float expected_lag; if (gps.get_lag(state.instance, expected_lag)) { float lag_s = (now - (state.last_corrected_gps_time_us/1000U)) * 0.001; if (lag_s > expected_lag+0.05) { // more than 50ms over expected lag, increment lag counter state.lagged_sample_count++; } else { state.lagged_sample_count = 0; } } #endif // HAL_BUILD_AP_PERIPH if (state.status >= AP_GPS::GPS_OK_FIX_2D) { // we must have a decent fix to calculate difference between itow and pseudo-itow _pseudo_itow_delta_ms = itow - (_pseudo_itow/1000ULL); } } } #if GPS_MOVING_BASELINE bool AP_GPS_Backend::calculate_moving_base_yaw(float reported_heading_deg, const float reported_distance, const float reported_D) { return calculate_moving_base_yaw(state, reported_heading_deg, reported_distance, reported_D); } bool AP_GPS_Backend::calculate_moving_base_yaw(AP_GPS::GPS_State &interim_state, const float reported_heading_deg, const float reported_distance, const float reported_D) { constexpr float minimum_antenna_seperation = 0.05; // meters constexpr float permitted_error_length_pct = 0.2; // percentage bool selectedOffset = false; Vector3f offset; switch (MovingBase::Type(gps.mb_params[interim_state.instance].type.get())) { case MovingBase::Type::RelativeToAlternateInstance: offset = gps._antenna_offset[interim_state.instance^1].get() - gps._antenna_offset[interim_state.instance].get(); selectedOffset = true; break; case MovingBase::Type::RelativeToCustomBase: offset = gps.mb_params[interim_state.instance].base_offset.get(); selectedOffset = true; break; } if (!selectedOffset) { // invalid type, let's throw up a flag INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); goto bad_yaw; } { const float offset_dist = offset.length(); const float min_dist = MIN(offset_dist, reported_distance); if (offset_dist < minimum_antenna_seperation) { // offsets have to be sufficently large to get a meaningful angle off of them Debug("Insufficent antenna offset (%f, %f, %f)", (double)offset.x, (double)offset.y, (double)offset.z); goto bad_yaw; } if (reported_distance < minimum_antenna_seperation) { // if the reported distance is less then the minimum seperation it's not sufficently robust Debug("Reported baseline distance (%f) was less then the minimum antenna seperation (%f)", (double)reported_distance, (double)minimum_antenna_seperation); goto bad_yaw; } if ((offset_dist - reported_distance) > (min_dist * permitted_error_length_pct)) { // the magnitude of the vector is much further then we were expecting Debug("Exceeded the permitted error margin %f > %f", (double)(offset_dist - reported_distance), (double)(min_dist * permitted_error_length_pct)); goto bad_yaw; } #ifndef HAL_BUILD_AP_PERIPH { // get lag float lag = 0.1; get_lag(lag); // get vehicle rotation, projected back in time using the gyro // this is not 100% accurate, but it is good enough for // this test. To do it completely accurately we'd need an // interface into DCM, EKF2 and EKF3 to ask for a // historical attitude. That is far too complex to justify // for this use case const auto &ahrs = AP::ahrs(); const Vector3f &gyro = ahrs.get_gyro(); Matrix3f rot_body_to_ned = ahrs.get_rotation_body_to_ned(); rot_body_to_ned.rotate(gyro * (-lag)); // apply rotation to the offset to get the Z offset in NED const Vector3f antenna_tilt = rot_body_to_ned * offset; const float alt_error = reported_D + antenna_tilt.z; if (fabsf(alt_error) > permitted_error_length_pct * min_dist) { // the vertical component is out of range, reject it Debug("bad alt_err %.1f > %.1f\n", alt_error, permitted_error_length_pct * min_dist); goto bad_yaw; } } #endif // HAL_BUILD_AP_PERIPH { // at this point the offsets are looking okay, go ahead and actually calculate a useful heading const float rotation_offset_rad = Vector2f(-offset.x, -offset.y).angle(); interim_state.gps_yaw = wrap_360(reported_heading_deg - degrees(rotation_offset_rad)); interim_state.have_gps_yaw = true; interim_state.gps_yaw_time_ms = AP_HAL::millis(); } goto good_yaw; } bad_yaw: interim_state.have_gps_yaw = false; good_yaw: #if HAL_LOGGING_ENABLED // this log message helps diagnose GPS yaw issues // @LoggerMessage: GPYW // @Description: GPS Yaw // @Field: TimeUS: Time since system startup // @Field: Id: instance // @Field: RHD: reported heading,deg // @Field: RDist: antenna separation,m // @Field: RDown: vertical antenna separation,m // @Field: OK: 1 if have yaw AP::logger().WriteStreaming("GPYW", "TimeUS,Id,RHD,RDist,RDown,OK", "s#dmm-", "F-----", "QBfffB", AP_HAL::micros64(), state.instance, reported_heading_deg, reported_distance, reported_D, interim_state.have_gps_yaw); #endif return interim_state.have_gps_yaw; } #endif // GPS_MOVING_BASELINE /* set altitude in location structure, honouring the driver option for MSL vs ellipsoid height */ void AP_GPS_Backend::set_alt_amsl_cm(AP_GPS::GPS_State &_state, int32_t alt_amsl_cm) { if (option_set(AP_GPS::HeightEllipsoid) && _state.have_undulation) { // user has asked ArduPilot to use ellipsoid height in the // canonical height for mission and navigation _state.location.alt = alt_amsl_cm - _state.undulation*100; } else { _state.location.alt = alt_amsl_cm; } } #if AP_GPS_DEBUG_LOGGING_ENABLED /* log some data for debugging the logging format matches that used by SITL with SIM_GPS_TYPE=7, allowing for development of GPS drivers based on logged data */ void AP_GPS_Backend::log_data(const uint8_t *data, uint16_t length) { if (state.instance < 2) { logging[state.instance].buf.write(data, length); } if (!log_thread_created) { log_thread_created = true; hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_GPS_Backend::logging_start, void), "gps_log", 4096, AP_HAL::Scheduler::PRIORITY_IO, 0); } } AP_GPS_Backend::loginfo AP_GPS_Backend::logging[2]; bool AP_GPS_Backend::log_thread_created; // logging loop, needs to be static to allow for re-alloc of GPS backends void AP_GPS_Backend::logging_loop(void) { while (true) { hal.scheduler->delay(10); static uint16_t lognum; for (uint8_t instance=0; instance<2; instance++) { if (logging[instance].fd == -1 && logging[instance].buf.available()) { char fname[] = "gpsN_XXX.log"; fname[3] = '1' + instance; if (lognum == 0) { for (lognum=1; lognum<1000; lognum++) { struct stat st; hal.util->snprintf(&fname[5], 8, "%03u.log", lognum); if (AP::FS().stat(fname, &st) != 0) { break; } } } hal.util->snprintf(&fname[5], 8, "%03u.log", lognum); logging[instance].fd = AP::FS().open(fname, O_WRONLY|O_CREAT|O_APPEND); } if (logging[instance].fd != -1) { uint32_t n = 0; const uint8_t *p; while ((p = logging[instance].buf.readptr(n)) != nullptr && n != 0) { struct { uint32_t magic = 0x7fe53b04U; uint32_t time_ms; uint32_t n; } header; header.n = n; header.time_ms = AP_HAL::millis(); // short writes are unlikely and are ignored (only FS full errors) AP::FS().write(logging[instance].fd, (const uint8_t *)&header, sizeof(header)); AP::FS().write(logging[instance].fd, p, n); logging[instance].buf.advance(n); AP::FS().fsync(logging[instance].fd); } } } } } // logging thread start, needs to be non-static for thread_create void AP_GPS_Backend::logging_start(void) { logging_loop(); } #endif // AP_GPS_DEBUG_LOGGING_ENABLED