2014-03-28 16:52:27 -03:00
/*
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 < http : //www.gnu.org/licenses/>.
*/
2015-12-23 09:35:01 -04:00
# include "AP_GPS.h"
2014-03-28 16:52:27 -03:00
2015-08-11 03:28:43 -03:00
# include <AP_Common/AP_Common.h>
# include <AP_HAL/AP_HAL.h>
2015-10-26 09:01:52 -03:00
# include <AP_Math/AP_Math.h>
2015-08-11 03:28:43 -03:00
# include <AP_Notify/AP_Notify.h>
2016-02-09 16:59:12 -04:00
# include <GCS_MAVLink/GCS.h>
2017-04-02 11:56:26 -03:00
# include <AP_BoardConfig/AP_BoardConfig.h>
2019-04-04 07:47:33 -03:00
# include <AP_RTC/AP_RTC.h>
2017-05-29 19:02:25 -03:00
# include <climits>
2014-03-28 16:52:27 -03:00
2016-05-16 10:09:06 -03:00
# include "AP_GPS_NOVA.h"
2016-04-13 14:20:05 -03:00
# include "AP_GPS_ERB.h"
# include "AP_GPS_GSOF.h"
# include "AP_GPS_MTK.h"
# include "AP_GPS_MTK19.h"
# include "AP_GPS_NMEA.h"
# include "AP_GPS_SBF.h"
# include "AP_GPS_SBP.h"
2017-03-27 22:13:31 -03:00
# include "AP_GPS_SBP2.h"
2016-04-13 14:20:05 -03:00
# include "AP_GPS_SIRF.h"
# include "AP_GPS_UBLOX.h"
2016-05-19 20:24:08 -03:00
# include "AP_GPS_MAV.h"
2016-04-13 14:20:05 -03:00
# include "GPS_Backend.h"
2017-04-02 11:56:26 -03:00
# if HAL_WITH_UAVCAN
2017-05-06 06:13:31 -03:00
# include <AP_BoardConfig/AP_BoardConfig_CAN.h>
2017-04-02 11:56:26 -03:00
# include <AP_UAVCAN/AP_UAVCAN.h>
# include "AP_GPS_UAVCAN.h"
# endif
2019-08-27 17:06:38 -03:00
# include <AP_AHRS/AP_AHRS.h>
2019-04-04 07:47:33 -03:00
# include <AP_Logger/AP_Logger.h>
# define GPS_RTK_INJECT_TO_ALL 127
# define GPS_MAX_RATE_MS 200 // maximum value of rate_ms (i.e. slowest update rate) is 5hz or 200ms
2017-01-29 19:02:57 -04:00
# define GPS_BAUD_TIME_MS 1200
2017-07-08 20:06:19 -03:00
# define GPS_TIMEOUT_MS 4000u
2017-01-29 19:02:57 -04:00
// defines used to specify the mask position for use of different accuracy metrics in the blending algorithm
# define BLEND_MASK_USE_HPOS_ACC 1
# define BLEND_MASK_USE_VPOS_ACC 2
# define BLEND_MASK_USE_SPD_ACC 4
2017-03-08 20:47:40 -04:00
# define BLEND_COUNTER_FAILURE_INCREMENT 10
2017-01-29 19:02:57 -04:00
2015-12-23 09:35:01 -04:00
extern const AP_HAL : : HAL & hal ;
2014-03-28 16:52:27 -03:00
2017-03-08 06:11:38 -04:00
// baudrates to try to detect GPSes with
2017-08-04 20:01:17 -03:00
const uint32_t AP_GPS : : _baudrates [ ] = { 9600U , 115200U , 4800U , 19200U , 38400U , 57600U , 230400U } ;
2017-03-08 06:11:38 -04:00
// initialisation blobs to send to the GPS to try to get it into the
// right mode
const char AP_GPS : : _initialisation_blob [ ] = UBLOX_SET_BINARY MTK_SET_BINARY SIRF_SET_BINARY ;
2017-10-24 22:30:10 -03:00
AP_GPS * AP_GPS : : _singleton ;
2014-03-28 16:52:27 -03:00
// table of user settable parameters
2015-10-25 14:03:46 -03:00
const AP_Param : : GroupInfo AP_GPS : : var_info [ ] = {
2014-03-28 16:52:27 -03:00
// @Param: TYPE
// @DisplayName: GPS type
// @Description: GPS type
2017-05-28 01:11:31 -03:00
// @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:UAVCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA,16:HemisphereNMEA
2015-07-24 02:32:21 -03:00
// @RebootRequired: True
2016-07-26 02:35:15 -03:00
// @User: Advanced
2018-02-12 23:14:25 -04:00
AP_GROUPINFO ( " TYPE " , 0 , AP_GPS , _type [ 0 ] , HAL_GPS_TYPE_DEFAULT ) ,
2014-03-28 16:52:27 -03:00
2019-05-26 22:34:13 -03:00
# if GPS_MAX_RECEIVERS > 1
2014-03-28 16:52:27 -03:00
// @Param: TYPE2
// @DisplayName: 2nd GPS type
// @Description: GPS type of 2nd GPS
2017-05-28 01:11:31 -03:00
// @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:UAVCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA,16:HemisphereNMEA
2015-07-24 02:32:21 -03:00
// @RebootRequired: True
2016-07-26 02:35:15 -03:00
// @User: Advanced
2014-03-28 16:52:27 -03:00
AP_GROUPINFO ( " TYPE2 " , 1 , AP_GPS , _type [ 1 ] , 0 ) ,
2019-05-26 22:34:13 -03:00
# endif
2014-06-06 18:58:11 -03:00
2014-03-28 16:52:27 -03:00
// @Param: NAVFILTER
// @DisplayName: Navigation filter setting
// @Description: Navigation filter engine setting
2014-05-20 10:08:40 -03:00
// @Values: 0:Portable,2:Stationary,3:Pedestrian,4:Automotive,5:Sea,6:Airborne1G,7:Airborne2G,8:Airborne4G
2016-07-26 02:35:15 -03:00
// @User: Advanced
2014-03-28 16:52:27 -03:00
AP_GROUPINFO ( " NAVFILTER " , 2 , AP_GPS , _navfilter , GPS_ENGINE_AIRBORNE_4G ) ,
2019-05-26 22:34:13 -03:00
# if GPS_MAX_RECEIVERS > 1
2014-06-06 18:58:11 -03:00
// @Param: AUTO_SWITCH
// @DisplayName: Automatic Switchover Setting
// @Description: Automatic switchover to GPS reporting best lock
2018-06-15 09:26:51 -03:00
// @Values: 0:Disabled,1:UseBest,2:Blend,3:UseSecond
2014-06-06 18:58:11 -03:00
// @User: Advanced
AP_GROUPINFO ( " AUTO_SWITCH " , 3 , AP_GPS , _auto_switch , 1 ) ,
2019-05-26 22:34:13 -03:00
# endif
2014-06-06 18:58:11 -03:00
2015-07-21 07:50:12 -03:00
// @Param: MIN_DGPS
2014-06-06 18:58:11 -03:00
// @DisplayName: Minimum Lock Type Accepted for DGPS
// @Description: Sets the minimum type of differential GPS corrections required before allowing to switch into DGPS mode.
// @Values: 0:Any,50:FloatRTK,100:IntegerRTK
// @User: Advanced
2015-07-24 02:32:21 -03:00
// @RebootRequired: True
2014-06-06 18:58:11 -03:00
AP_GROUPINFO ( " MIN_DGPS " , 4 , AP_GPS , _min_dgps , 100 ) ,
2014-09-04 01:27:05 -03:00
// @Param: SBAS_MODE
// @DisplayName: SBAS Mode
// @Description: This sets the SBAS (satellite based augmentation system) mode if available on this GPS. If set to 2 then the SBAS mode is not changed in the GPS. Otherwise the GPS will be reconfigured to enable/disable SBAS. Disabling SBAS may be worthwhile in some parts of the world where an SBAS signal is available but the baseline is too long to be useful.
// @Values: 0:Disabled,1:Enabled,2:NoChange
// @User: Advanced
AP_GROUPINFO ( " SBAS_MODE " , 5 , AP_GPS , _sbas_mode , 2 ) ,
2014-09-04 01:43:29 -03:00
// @Param: MIN_ELEV
// @DisplayName: Minimum elevation
// @Description: This sets the minimum elevation of satellites above the horizon for them to be used for navigation. Setting this to -100 leaves the minimum elevation set to the GPS modules default.
// @Range: -100 90
2017-05-02 10:43:10 -03:00
// @Units: deg
2014-09-04 01:43:29 -03:00
// @User: Advanced
AP_GROUPINFO ( " MIN_ELEV " , 6 , AP_GPS , _min_elevation , - 100 ) ,
2015-04-22 20:10:46 -03:00
// @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.
2016-01-07 00:43:23 -04:00
// @Values: 0:send to first GPS,1:send to 2nd GPS,127:send to all
2016-07-26 02:35:15 -03:00
// @User: Advanced
2015-06-27 04:26:25 -03:00
AP_GROUPINFO ( " INJECT_TO " , 7 , AP_GPS , _inject_to , GPS_RTK_INJECT_TO_ALL ) ,
2015-04-22 20:10:46 -03:00
// @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
2017-02-10 20:26:41 -04:00
// @Values: 0:None (0x0000),-1:All (0xFFFF),-256:External only (0xFF00)
2015-04-22 20:10:46 -03:00
// @User: Advanced
2019-05-09 19:06:22 -03:00
AP_GROUPINFO ( " SBP_LOGMASK " , 8 , AP_GPS , _sbp_logmask , - 256 ) ,
2015-04-22 20:10:46 -03:00
2015-07-21 07:50:12 -03:00
// @Param: RAW_DATA
2015-05-04 05:18:34 -03:00
// @DisplayName: Raw data logging
2019-02-11 04:19:08 -04:00
// @Description: Handles logging raw data; on uBlox chips that support raw data this will log RXM messages into logger; on Septentrio this will log on the equipment's SD card and when set to 2, the autopilot will try to stop logging after disarming and restart after arming
2017-10-19 22:15:11 -03:00
// @Values: 0:Ignore,1:Always log,2:Stop logging when disarmed (SBF only),5:Only log every five samples (uBlox only)
2015-07-24 02:32:21 -03:00
// @RebootRequired: True
2016-07-26 02:35:15 -03:00
// @User: Advanced
2015-05-04 05:18:34 -03:00
AP_GROUPINFO ( " RAW_DATA " , 9 , AP_GPS , _raw_data , 0 ) ,
2015-07-14 18:12:47 -03:00
// @Param: GNSS_MODE
// @DisplayName: GNSS system configuration
2016-02-02 03:58:33 -04:00
// @Description: Bitmask for what GNSS system to use on the first GPS (all unchecked or zero to leave GPS as configured)
2015-09-09 01:38:51 -03:00
// @Values: 0:Leave as currently configured, 1:GPS-NoSBAS, 3:GPS+SBAS, 4:Galileo-NoSBAS, 6:Galileo+SBAS, 8:Beidou, 51:GPS+IMES+QZSS+SBAS (Japan Only), 64:GLONASS, 66:GLONASS+SBAS, 67:GPS+GLONASS+SBAS
2016-01-07 00:43:23 -04:00
// @Bitmask: 0:GPS,1:SBAS,2:Galileo,3:Beidou,4:IMES,5:QZSS,6:GLOSNASS
2015-07-14 18:12:47 -03:00
// @User: Advanced
2016-02-02 03:58:33 -04:00
AP_GROUPINFO ( " GNSS_MODE " , 10 , AP_GPS , _gnss_mode [ 0 ] , 0 ) ,
2015-07-14 18:12:47 -03:00
2015-10-28 21:40:50 -03:00
// @Param: SAVE_CFG
// @DisplayName: Save GPS configuration
2016-02-12 16:32:23 -04:00
// @Description: Determines whether the configuration for this GPS should be written to non-volatile memory on the GPS. Currently working for UBlox 6 series and above.
// @Values: 0:Do not save config,1:Save config,2:Save only when needed
2015-10-28 21:40:50 -03:00
// @User: Advanced
2017-11-27 17:26:11 -04:00
AP_GROUPINFO ( " SAVE_CFG " , 11 , AP_GPS , _save_config , 2 ) ,
2015-10-28 21:40:50 -03:00
2019-05-26 22:34:13 -03:00
# if GPS_MAX_RECEIVERS > 1
2016-02-02 03:58:33 -04:00
// @Param: GNSS_MODE2
// @DisplayName: GNSS system configuration
// @Description: Bitmask for what GNSS system to use on the second GPS (all unchecked or zero to leave GPS as configured)
// @Values: 0:Leave as currently configured, 1:GPS-NoSBAS, 3:GPS+SBAS, 4:Galileo-NoSBAS, 6:Galileo+SBAS, 8:Beidou, 51:GPS+IMES+QZSS+SBAS (Japan Only), 64:GLONASS, 66:GLONASS+SBAS, 67:GPS+GLONASS+SBAS
// @Bitmask: 0:GPS,1:SBAS,2:Galileo,3:Beidou,4:IMES,5:QZSS,6:GLOSNASS
// @User: Advanced
AP_GROUPINFO ( " GNSS_MODE2 " , 12 , AP_GPS , _gnss_mode [ 1 ] , 0 ) ,
2019-05-26 22:34:13 -03:00
# endif
2016-02-02 03:58:33 -04:00
// @Param: AUTO_CONFIG
// @DisplayName: Automatic GPS configuration
// @Description: Controls if the autopilot should automatically configure the GPS based on the parameters and default settings
// @Values: 0:Disables automatic configuration,1:Enable automatic configuration
// @User: Advanced
AP_GROUPINFO ( " AUTO_CONFIG " , 13 , AP_GPS , _auto_config , 1 ) ,
2016-10-01 04:42:47 -03:00
// @Param: RATE_MS
// @DisplayName: GPS update rate in milliseconds
// @Description: Controls how often the GPS should provide a position update. Lowering below 5Hz is not allowed
2017-05-02 10:43:10 -03:00
// @Units: ms
2016-10-01 04:42:47 -03:00
// @Values: 100:10Hz,125:8Hz,200:5Hz
2017-03-10 02:46:16 -04:00
// @Range: 50 200
2016-10-01 04:42:47 -03:00
// @User: Advanced
AP_GROUPINFO ( " RATE_MS " , 14 , AP_GPS , _rate_ms [ 0 ] , 200 ) ,
2019-05-26 22:34:13 -03:00
# if GPS_MAX_RECEIVERS > 1
2016-10-01 04:42:47 -03:00
// @Param: RATE_MS2
// @DisplayName: GPS 2 update rate in milliseconds
// @Description: Controls how often the GPS should provide a position update. Lowering below 5Hz is not allowed
2017-05-02 10:43:10 -03:00
// @Units: ms
2016-10-01 04:42:47 -03:00
// @Values: 100:10Hz,125:8Hz,200:5Hz
2017-03-10 02:46:16 -04:00
// @Range: 50 200
2016-10-01 04:42:47 -03:00
// @User: Advanced
AP_GROUPINFO ( " RATE_MS2 " , 15 , AP_GPS , _rate_ms [ 1 ] , 200 ) ,
2019-05-26 22:34:13 -03:00
# endif
2016-10-01 04:42:47 -03:00
2016-10-07 19:04:14 -03:00
// @Param: POS1_X
// @DisplayName: Antenna X position offset
2016-10-18 19:33:07 -03:00
// @Description: X position of the first GPS antenna in body frame. Positive X is forward of the origin. Use antenna phase centroid location if provided by the manufacturer.
2016-10-07 19:04:14 -03:00
// @Units: m
2018-12-13 06:43:51 -04:00
// @Range: -10 10
2016-10-07 19:04:14 -03:00
// @User: Advanced
// @Param: POS1_Y
// @DisplayName: Antenna Y position offset
2016-10-18 19:33:07 -03:00
// @Description: Y position of the first GPS antenna in body frame. Positive Y is to the right of the origin. Use antenna phase centroid location if provided by the manufacturer.
2016-10-07 19:04:14 -03:00
// @Units: m
2018-12-13 06:43:51 -04:00
// @Range: -10 10
2016-10-07 19:04:14 -03:00
// @User: Advanced
// @Param: POS1_Z
// @DisplayName: Antenna Z position offset
2016-10-18 19:33:07 -03:00
// @Description: Z position of the first GPS antenna in body frame. Positive Z is down from the origin. Use antenna phase centroid location if provided by the manufacturer.
2016-10-07 19:04:14 -03:00
// @Units: m
2018-12-13 06:43:51 -04:00
// @Range: -10 10
2016-10-07 19:04:14 -03:00
// @User: Advanced
AP_GROUPINFO ( " POS1 " , 16 , AP_GPS , _antenna_offset [ 0 ] , 0.0f ) ,
// @Param: POS2_X
// @DisplayName: Antenna X position offset
2016-10-18 19:33:07 -03:00
// @Description: X position of the second GPS antenna in body frame. Positive X is forward of the origin. Use antenna phase centroid location if provided by the manufacturer.
2016-10-07 19:04:14 -03:00
// @Units: m
2018-12-13 06:43:51 -04:00
// @Range: -10 10
2016-10-07 19:04:14 -03:00
// @User: Advanced
// @Param: POS2_Y
// @DisplayName: Antenna Y position offset
2016-10-18 19:33:07 -03:00
// @Description: Y position of the second GPS antenna in body frame. Positive Y is to the right of the origin. Use antenna phase centroid location if provided by the manufacturer.
2016-10-07 19:04:14 -03:00
// @Units: m
2018-12-13 06:43:51 -04:00
// @Range: -10 10
2016-10-07 19:04:14 -03:00
// @User: Advanced
2019-05-26 22:34:13 -03:00
# if GPS_MAX_RECEIVERS > 1
2016-10-07 19:04:14 -03:00
// @Param: POS2_Z
// @DisplayName: Antenna Z position offset
2016-10-18 19:33:07 -03:00
// @Description: Z position of the second GPS antenna in body frame. Positive Z is down from the origin. Use antenna phase centroid location if provided by the manufacturer.
2016-10-07 19:04:14 -03:00
// @Units: m
2018-12-13 06:43:51 -04:00
// @Range: -10 10
2016-10-07 19:04:14 -03:00
// @User: Advanced
AP_GROUPINFO ( " POS2 " , 17 , AP_GPS , _antenna_offset [ 1 ] , 0.0f ) ,
2019-05-26 22:34:13 -03:00
# endif
2016-10-07 19:04:14 -03:00
2016-12-28 18:20:06 -04:00
// @Param: DELAY_MS
// @DisplayName: GPS delay in milliseconds
// @Description: Controls the amount of GPS measurement delay that the autopilot compensates for. Set to zero to use the default delay for the detected GPS type.
2017-05-02 10:43:10 -03:00
// @Units: ms
2016-12-28 18:20:06 -04:00
// @Range: 0 250
// @User: Advanced
2017-05-24 14:42:03 -03:00
// @RebootRequired: True
2016-12-28 18:20:06 -04:00
AP_GROUPINFO ( " DELAY_MS " , 18 , AP_GPS , _delay_ms [ 0 ] , 0 ) ,
2019-05-26 22:34:13 -03:00
# if GPS_MAX_RECEIVERS > 1
2016-12-28 18:20:06 -04:00
// @Param: DELAY_MS2
// @DisplayName: GPS 2 delay in milliseconds
// @Description: Controls the amount of GPS measurement delay that the autopilot compensates for. Set to zero to use the default delay for the detected GPS type.
2017-05-02 10:43:10 -03:00
// @Units: ms
2016-12-28 18:20:06 -04:00
// @Range: 0 250
// @User: Advanced
2017-05-24 14:42:03 -03:00
// @RebootRequired: True
2016-12-28 18:20:06 -04:00
AP_GROUPINFO ( " DELAY_MS2 " , 19 , AP_GPS , _delay_ms [ 1 ] , 0 ) ,
2019-05-26 22:34:13 -03:00
# endif
2017-01-29 19:02:57 -04:00
2019-05-26 22:34:13 -03:00
# if defined(GPS_BLENDED_INSTANCE)
2017-01-29 19:02:57 -04:00
// @Param: BLEND_MASK
// @DisplayName: Multi GPS Blending Mask
// @Description: Determines which of the accuracy measures Horizontal position, Vertical Position and Speed are used to calculate the weighting on each GPS receiver when soft switching has been selected by setting GPS_AUTO_SWITCH to 2
// @Bitmask: 0:Horiz Pos,1:Vert Pos,2:Speed
// @User: Advanced
AP_GROUPINFO ( " BLEND_MASK " , 20 , AP_GPS , _blend_mask , 5 ) ,
// @Param: BLEND_TC
// @DisplayName: Blending time constant
// @Description: Controls the slowest time constant applied to the calculation of GPS position and height offsets used to adjust different GPS receivers for steady state position differences.
2017-05-02 10:43:10 -03:00
// @Units: s
2017-01-29 19:02:57 -04:00
// @Range: 5.0 30.0
// @User: Advanced
AP_GROUPINFO ( " BLEND_TC " , 21 , AP_GPS , _blend_tc , 10.0f ) ,
2019-05-26 22:34:13 -03:00
# endif
2017-01-29 19:02:57 -04:00
2014-03-28 16:52:27 -03:00
AP_GROUPEND
} ;
2017-03-08 05:56:52 -04:00
// constructor
AP_GPS : : AP_GPS ( )
{
2017-05-29 19:02:25 -03:00
static_assert ( ( sizeof ( _initialisation_blob ) * ( CHAR_BIT + 2 ) ) < ( 4800 * GPS_BAUD_TIME_MS * 1e-3 ) ,
2019-01-14 16:20:18 -04:00
" GPS initilisation blob is too large to be completely sent before the baud rate changes " ) ;
2017-05-29 19:02:25 -03:00
2017-03-08 05:56:52 -04:00
AP_Param : : setup_object_defaults ( this , var_info ) ;
2017-10-24 22:30:10 -03:00
if ( _singleton ! = nullptr ) {
AP_HAL : : panic ( " AP_GPS must be singleton " ) ;
}
_singleton = this ;
2017-03-08 05:56:52 -04:00
}
2019-10-23 22:46:26 -03:00
// return true if a specific type of GPS uses a UART
bool AP_GPS : : needs_uart ( GPS_Type type ) const
{
switch ( type ) {
case GPS_TYPE_NONE :
case GPS_TYPE_HIL :
case GPS_TYPE_UAVCAN :
case GPS_TYPE_MAV :
return false ;
default :
break ;
}
return true ;
}
2014-03-28 16:52:27 -03:00
/// Startup initialisation.
2017-06-27 05:12:45 -03:00
void AP_GPS : : init ( const AP_SerialManager & serial_manager )
2014-03-28 16:52:27 -03:00
{
2014-06-06 18:58:11 -03:00
primary_instance = 0 ;
2015-01-19 09:36:35 -04:00
// search for serial ports with gps protocol
2019-10-23 22:46:26 -03:00
uint8_t uart_idx = 0 ;
2019-05-26 22:34:13 -03:00
for ( uint8_t i = 0 ; i < GPS_MAX_RECEIVERS ; i + + ) {
2019-10-23 22:46:26 -03:00
if ( needs_uart ( ( GPS_Type ) _type [ i ] . get ( ) ) ) {
_port [ i ] = serial_manager . find_serial ( AP_SerialManager : : SerialProtocol_GPS , uart_idx ) ;
uart_idx + + ;
}
2019-05-26 22:34:13 -03:00
}
2015-05-21 18:07:59 -03:00
_last_instance_swap_ms = 0 ;
2017-01-29 19:02:57 -04:00
// Initialise class variables used to do GPS blending
_omega_lpf = 1.0f / constrain_float ( _blend_tc , 5.0f , 30.0f ) ;
2017-07-18 15:07:41 -03:00
// prep the state instance fields
for ( uint8_t i = 0 ; i < GPS_MAX_INSTANCES ; i + + ) {
state [ i ] . instance = i ;
}
2017-03-07 23:33:31 -04:00
// sanity check update rate
for ( uint8_t i = 0 ; i < GPS_MAX_RECEIVERS ; i + + ) {
if ( _rate_ms [ i ] < = 0 | | _rate_ms [ i ] > GPS_MAX_RATE_MS ) {
_rate_ms [ i ] = GPS_MAX_RATE_MS ;
}
}
2014-03-28 16:52:27 -03:00
}
2017-03-08 05:56:52 -04:00
// return number of active GPS sensors. Note that if the first GPS
// is not present but the 2nd is then we return 2. Note that a blended
// GPS solution is treated as an additional sensor.
uint8_t AP_GPS : : num_sensors ( void ) const
{
if ( ! _output_is_blended ) {
return num_instances ;
} else {
return num_instances + 1 ;
}
}
2014-03-28 16:52:27 -03:00
2017-03-08 05:56:52 -04:00
bool AP_GPS : : speed_accuracy ( uint8_t instance , float & sacc ) const
{
if ( state [ instance ] . have_speed_accuracy ) {
sacc = state [ instance ] . speed_accuracy ;
return true ;
}
return false ;
}
bool AP_GPS : : horizontal_accuracy ( uint8_t instance , float & hacc ) const
{
if ( state [ instance ] . have_horizontal_accuracy ) {
hacc = state [ instance ] . horizontal_accuracy ;
return true ;
}
return false ;
}
bool AP_GPS : : vertical_accuracy ( uint8_t instance , float & vacc ) const
{
if ( state [ instance ] . have_vertical_accuracy ) {
vacc = state [ instance ] . vertical_accuracy ;
return true ;
}
return false ;
}
2014-03-28 16:52:27 -03:00
2017-03-08 05:59:27 -04:00
/**
convert GPS week and milliseconds to unix epoch in milliseconds
*/
uint64_t AP_GPS : : time_epoch_convert ( uint16_t gps_week , uint32_t gps_ms )
{
2017-05-05 07:09:29 -03:00
uint64_t fix_time_ms = UNIX_OFFSET_MSEC + gps_week * AP_MSEC_PER_WEEK + gps_ms ;
2017-03-08 05:59:27 -04:00
return fix_time_ms ;
}
/**
calculate current time since the unix epoch in microseconds
*/
2017-07-25 23:47:00 -03:00
uint64_t AP_GPS : : time_epoch_usec ( uint8_t instance ) const
2017-03-08 05:59:27 -04:00
{
const GPS_State & istate = state [ instance ] ;
2019-07-31 00:17:06 -03:00
if ( istate . last_gps_time_ms = = 0 | | istate . time_week = = 0 ) {
2017-03-08 05:59:27 -04:00
return 0 ;
}
uint64_t fix_time_ms = time_epoch_convert ( istate . time_week , istate . time_week_ms ) ;
// add in the milliseconds since the last fix
return ( fix_time_ms + ( AP_HAL : : millis ( ) - istate . last_gps_time_ms ) ) * 1000ULL ;
}
2014-03-31 06:48:22 -03:00
/*
send some more initialisation string bytes if there is room in the
UART transmit buffer
*/
2015-10-26 08:25:44 -03:00
void AP_GPS : : send_blob_start ( uint8_t instance , const char * _blob , uint16_t size )
2014-03-31 06:48:22 -03:00
{
initblob_state [ instance ] . blob = _blob ;
initblob_state [ instance ] . remaining = size ;
}
/*
send some more initialisation string bytes if there is room in the
UART transmit buffer
*/
void AP_GPS : : send_blob_update ( uint8_t instance )
{
2015-01-19 09:36:35 -04:00
// exit immediately if no uart for this instance
2016-10-30 02:24:21 -03:00
if ( _port [ instance ] = = nullptr ) {
2015-01-19 09:36:35 -04:00
return ;
}
2014-03-31 06:48:22 -03:00
// see if we can write some more of the initialisation blob
if ( initblob_state [ instance ] . remaining > 0 ) {
2015-01-19 09:36:35 -04:00
int16_t space = _port [ instance ] - > txspace ( ) ;
2014-03-31 06:48:22 -03:00
if ( space > ( int16_t ) initblob_state [ instance ] . remaining ) {
space = initblob_state [ instance ] . remaining ;
}
while ( space > 0 ) {
2015-12-23 09:35:01 -04:00
_port [ instance ] - > write ( * initblob_state [ instance ] . blob ) ;
2014-03-31 06:48:22 -03:00
initblob_state [ instance ] . blob + + ;
space - - ;
initblob_state [ instance ] . remaining - - ;
}
}
}
2014-03-28 16:52:27 -03:00
/*
run detection step for one GPS instance . If this finds a GPS then it
will fill in drivers [ instance ] and change state [ instance ] . status
from NO_GPS to NO_FIX .
*/
2017-04-02 11:56:26 -03:00
void AP_GPS : : detect_instance ( uint8_t instance )
2014-03-28 16:52:27 -03:00
{
2016-10-30 02:24:21 -03:00
AP_GPS_Backend * new_gps = nullptr ;
2014-03-28 16:52:27 -03:00
struct detect_state * dstate = & detect_state [ instance ] ;
2017-09-23 20:53:40 -03:00
const uint32_t now = AP_HAL : : millis ( ) ;
2014-03-28 16:52:27 -03:00
2017-07-17 21:57:36 -03:00
state [ instance ] . status = NO_GPS ;
state [ instance ] . hdop = GPS_UNKNOWN_DOP ;
state [ instance ] . vdop = GPS_UNKNOWN_DOP ;
2018-07-18 06:27:15 -03:00
2016-12-15 09:12:54 -04:00
switch ( _type [ instance ] ) {
2016-10-29 04:45:00 -03:00
// user has to explicitly set the MAV type, do not use AUTO
// do not try to detect the MAV type, assume it's there
2018-07-18 06:27:15 -03:00
case GPS_TYPE_MAV :
2019-05-26 22:34:13 -03:00
# ifndef HAL_BUILD_AP_PERIPH
2017-05-30 03:47:50 -03:00
dstate - > auto_detected_baud = false ; // specified, not detected
2016-10-30 02:24:21 -03:00
new_gps = new AP_GPS_MAV ( * this , state [ instance ] , nullptr ) ;
2016-10-29 04:45:00 -03:00
goto found_gps ;
2019-05-26 22:34:13 -03:00
# endif
2016-12-15 09:12:54 -04:00
break ;
2017-04-02 11:56:26 -03:00
// user has to explicitly set the UAVCAN type, do not use AUTO
2018-07-18 06:27:15 -03:00
case GPS_TYPE_UAVCAN :
2018-07-18 03:27:37 -03:00
# if HAL_WITH_UAVCAN
2017-05-30 03:47:50 -03:00
dstate - > auto_detected_baud = false ; // specified, not detected
2018-07-18 06:27:15 -03:00
new_gps = AP_GPS_UAVCAN : : probe ( * this , state [ instance ] ) ;
goto found_gps ;
2017-04-02 11:56:26 -03:00
# endif
2018-07-18 06:27:15 -03:00
return ; // We don't do anything here if UAVCAN is not supported
2016-12-15 09:12:54 -04:00
default :
break ;
2016-10-29 04:45:00 -03:00
}
2016-10-30 02:24:21 -03:00
if ( _port [ instance ] = = nullptr ) {
2014-03-28 16:52:27 -03:00
// UART not available
return ;
}
2017-05-30 03:47:50 -03:00
// all remaining drivers automatically cycle through baud rates to detect
// the correct baud rate, and should have the selected baud broadcast
dstate - > auto_detected_baud = true ;
2019-05-26 22:34:13 -03:00
# ifndef HAL_BUILD_AP_PERIPH
2016-12-15 09:12:54 -04:00
switch ( _type [ instance ] ) {
// by default the sbf/trimble gps outputs no data on its port, until configured.
case GPS_TYPE_SBF :
new_gps = new AP_GPS_SBF ( * this , state [ instance ] , _port [ instance ] ) ;
break ;
case GPS_TYPE_GSOF :
new_gps = new AP_GPS_GSOF ( * this , state [ instance ] , _port [ instance ] ) ;
break ;
case GPS_TYPE_NOVA :
new_gps = new AP_GPS_NOVA ( * this , state [ instance ] , _port [ instance ] ) ;
break ;
default :
break ;
}
2019-05-26 22:34:13 -03:00
# endif // HAL_BUILD_AP_PERIPH
2015-11-03 09:46:46 -04:00
2015-07-14 04:07:29 -03:00
if ( now - dstate - > last_baud_change_ms > GPS_BAUD_TIME_MS ) {
2014-03-28 16:52:27 -03:00
// try the next baud rate
2016-09-24 03:16:40 -03:00
// incrementing like this will skip the first element in array of bauds
// this is okay, and relied upon
dstate - > current_baud + + ;
if ( dstate - > current_baud = = ARRAY_SIZE ( _baudrates ) ) {
dstate - > current_baud = 0 ;
}
uint32_t baudrate = _baudrates [ dstate - > current_baud ] ;
_port [ instance ] - > begin ( baudrate ) ;
_port [ instance ] - > set_flow_control ( AP_HAL : : UARTDriver : : FLOW_CONTROL_DISABLE ) ;
dstate - > last_baud_change_ms = now ;
2016-09-06 04:16:01 -03:00
2017-09-21 23:49:41 -03:00
if ( _auto_config = = GPS_AUTO_CONFIG_ENABLE & & new_gps = = nullptr ) {
2017-05-28 01:11:31 -03:00
if ( _type [ instance ] = = GPS_TYPE_HEMI ) {
send_blob_start ( instance , AP_GPS_NMEA_HEMISPHERE_INIT_STRING , strlen ( AP_GPS_NMEA_HEMISPHERE_INIT_STRING ) ) ;
} else {
send_blob_start ( instance , _initialisation_blob , sizeof ( _initialisation_blob ) ) ;
}
2016-08-08 16:23:17 -03:00
}
2014-03-28 16:52:27 -03:00
}
2017-09-21 23:49:41 -03:00
if ( _auto_config = = GPS_AUTO_CONFIG_ENABLE & & new_gps = = nullptr ) {
2016-08-08 16:23:17 -03:00
send_blob_update ( instance ) ;
}
2014-03-28 16:52:27 -03:00
2015-07-14 04:07:29 -03:00
while ( initblob_state [ instance ] . remaining = = 0 & & _port [ instance ] - > available ( ) > 0
2017-04-02 11:56:26 -03:00
& & new_gps = = nullptr ) {
2015-01-19 09:36:35 -04:00
uint8_t data = _port [ instance ] - > read ( ) ;
2014-03-28 16:52:27 -03:00
/*
running a uBlox at less than 38400 will lead to packet
corruption , as we can ' t receive the packets in the 200 ms
window for 5 Hz fixes . The NMEA startup message should force
2017-01-25 15:26:55 -04:00
the uBlox into 115200 no matter what rate it is configured
2014-03-28 16:52:27 -03:00
for .
*/
if ( ( _type [ instance ] = = GPS_TYPE_AUTO | | _type [ instance ] = = GPS_TYPE_UBLOX ) & &
2017-01-25 15:26:55 -04:00
( ( ! _auto_config & & _baudrates [ dstate - > current_baud ] > = 38400 ) | |
_baudrates [ dstate - > current_baud ] = = 115200 ) & &
2014-03-28 16:52:27 -03:00
AP_GPS_UBLOX : : _detect ( dstate - > ublox_detect_state , data ) ) {
2015-01-19 09:36:35 -04:00
new_gps = new AP_GPS_UBLOX ( * this , state [ instance ] , _port [ instance ] ) ;
2017-04-02 11:56:26 -03:00
}
2019-05-26 22:34:13 -03:00
# ifndef HAL_BUILD_AP_PERIPH
2017-01-27 00:57:34 -04:00
# if !HAL_MINIMIZE_FEATURES
// we drop the MTK drivers when building a small build as they are so rarely used
// and are surprisingly large
2016-12-15 09:12:54 -04:00
else if ( ( _type [ instance ] = = GPS_TYPE_AUTO | | _type [ instance ] = = GPS_TYPE_MTK19 ) & &
2014-03-28 16:52:27 -03:00
AP_GPS_MTK19 : : _detect ( dstate - > mtk19_detect_state , data ) ) {
2016-12-15 09:12:54 -04:00
new_gps = new AP_GPS_MTK19 ( * this , state [ instance ] , _port [ instance ] ) ;
2017-04-02 11:56:26 -03:00
} else if ( ( _type [ instance ] = = GPS_TYPE_AUTO | | _type [ instance ] = = GPS_TYPE_MTK ) & &
AP_GPS_MTK : : _detect ( dstate - > mtk_detect_state , data ) ) {
2016-12-15 09:12:54 -04:00
new_gps = new AP_GPS_MTK ( * this , state [ instance ] , _port [ instance ] ) ;
}
2017-01-27 00:57:34 -04:00
# endif
2017-03-27 22:13:31 -03:00
else if ( ( _type [ instance ] = = GPS_TYPE_AUTO | | _type [ instance ] = = GPS_TYPE_SBP ) & &
AP_GPS_SBP2 : : _detect ( dstate - > sbp2_detect_state , data ) ) {
new_gps = new AP_GPS_SBP2 ( * this , state [ instance ] , _port [ instance ] ) ;
}
2014-04-04 20:05:54 -03:00
else if ( ( _type [ instance ] = = GPS_TYPE_AUTO | | _type [ instance ] = = GPS_TYPE_SBP ) & &
AP_GPS_SBP : : _detect ( dstate - > sbp_detect_state , data ) ) {
2015-01-19 09:36:35 -04:00
new_gps = new AP_GPS_SBP ( * this , state [ instance ] , _port [ instance ] ) ;
2014-04-04 20:05:54 -03:00
}
2017-01-27 00:57:34 -04:00
# if !HAL_MINIMIZE_FEATURES
2016-12-15 09:12:54 -04:00
else if ( ( _type [ instance ] = = GPS_TYPE_AUTO | | _type [ instance ] = = GPS_TYPE_SIRF ) & &
2014-03-28 16:52:27 -03:00
AP_GPS_SIRF : : _detect ( dstate - > sirf_detect_state , data ) ) {
2016-12-15 09:12:54 -04:00
new_gps = new AP_GPS_SIRF ( * this , state [ instance ] , _port [ instance ] ) ;
}
2017-01-27 00:57:34 -04:00
# endif
2016-01-18 17:54:40 -04:00
else if ( ( _type [ instance ] = = GPS_TYPE_AUTO | | _type [ instance ] = = GPS_TYPE_ERB ) & &
AP_GPS_ERB : : _detect ( dstate - > erb_detect_state , data ) ) {
new_gps = new AP_GPS_ERB ( * this , state [ instance ] , _port [ instance ] ) ;
2017-05-28 01:11:31 -03:00
} else if ( ( _type [ instance ] = = GPS_TYPE_NMEA | |
_type [ instance ] = = GPS_TYPE_HEMI ) & &
2017-05-30 03:33:39 -03:00
AP_GPS_NMEA : : _detect ( dstate - > nmea_detect_state , data ) ) {
new_gps = new AP_GPS_NMEA ( * this , state [ instance ] , _port [ instance ] ) ;
2016-12-15 09:12:54 -04:00
}
2019-05-26 22:34:13 -03:00
# endif // HAL_BUILD_AP_PERIPH
if ( new_gps ) {
goto found_gps ;
}
2016-12-15 09:12:54 -04:00
}
2014-03-28 16:52:27 -03:00
2014-11-13 23:44:15 -04:00
found_gps :
2016-12-15 09:12:54 -04:00
if ( new_gps ! = nullptr ) {
2014-03-28 16:52:27 -03:00
state [ instance ] . status = NO_FIX ;
2014-03-31 08:13:55 -03:00
drivers [ instance ] = new_gps ;
timing [ instance ] . last_message_time_ms = now ;
2017-07-08 20:06:19 -03:00
timing [ instance ] . delta_time_ms = GPS_TIMEOUT_MS ;
2016-08-01 21:30:12 -03:00
new_gps - > broadcast_gps_type ( ) ;
2019-10-28 14:14:52 -03:00
if ( instance = = 1 ) {
has_had_second_instance = true ;
}
2016-12-15 09:12:54 -04:00
}
2014-03-28 16:52:27 -03:00
}
2017-04-02 11:56:26 -03:00
AP_GPS : : GPS_Status AP_GPS : : highest_supported_status ( uint8_t instance ) const
2014-06-06 18:58:11 -03:00
{
2017-03-08 05:49:58 -04:00
if ( instance < GPS_MAX_RECEIVERS & & drivers [ instance ] ! = nullptr ) {
2014-06-06 18:58:11 -03:00
return drivers [ instance ] - > highest_supported_status ( ) ;
2016-12-15 09:12:54 -04:00
}
2014-06-06 18:58:11 -03:00
return AP_GPS : : GPS_OK_FIX_3D ;
}
2019-02-11 04:19:08 -04:00
bool AP_GPS : : should_log ( ) const
2018-04-11 09:16:23 -03:00
{
2019-05-26 22:34:13 -03:00
# ifndef HAL_BUILD_AP_PERIPH
2019-02-11 04:19:08 -04:00
AP_Logger * logger = AP_Logger : : get_singleton ( ) ;
if ( logger = = nullptr ) {
2018-04-11 09:16:23 -03:00
return false ;
}
if ( _log_gps_bit = = ( uint32_t ) - 1 ) {
return false ;
}
2019-02-11 04:19:08 -04:00
if ( ! logger - > should_log ( _log_gps_bit ) ) {
2018-04-11 09:16:23 -03:00
return false ;
}
return true ;
2019-05-26 22:34:13 -03:00
# else
return false ;
# endif
2018-04-11 09:16:23 -03:00
}
2014-03-28 16:52:27 -03:00
/*
2014-03-31 06:48:22 -03:00
update one GPS instance . This should be called at 10 Hz or greater
2014-03-28 16:52:27 -03:00
*/
2017-04-02 11:56:26 -03:00
void AP_GPS : : update_instance ( uint8_t instance )
2014-03-28 16:52:27 -03:00
{
2014-04-01 03:25:15 -03:00
if ( _type [ instance ] = = GPS_TYPE_HIL ) {
// in HIL, leave info alone
return ;
}
2014-03-28 16:52:27 -03:00
if ( _type [ instance ] = = GPS_TYPE_NONE ) {
// not enabled
state [ instance ] . status = NO_GPS ;
2017-05-24 16:19:10 -03:00
state [ instance ] . hdop = GPS_UNKNOWN_DOP ;
state [ instance ] . vdop = GPS_UNKNOWN_DOP ;
2014-03-28 16:52:27 -03:00
return ;
}
2014-04-04 17:33:34 -03:00
if ( locked_ports & ( 1U < < instance ) ) {
// the port is locked by another driver
return ;
}
2019-10-09 01:30:39 -03:00
if ( drivers [ instance ] = = nullptr ) {
2014-03-28 16:52:27 -03:00
// we don't yet know the GPS type of this one, or it has timed
// out and needs to be re-initialised
detect_instance ( instance ) ;
return ;
}
2017-06-11 04:58:45 -03:00
if ( _auto_config = = GPS_AUTO_CONFIG_ENABLE ) {
2016-08-08 16:23:17 -03:00
send_blob_update ( instance ) ;
}
2014-03-31 06:48:22 -03:00
2014-03-28 16:52:27 -03:00
// we have an active driver for this instance
bool result = drivers [ instance ] - > read ( ) ;
2018-05-15 22:16:01 -03:00
uint32_t tnow = AP_HAL : : millis ( ) ;
2014-03-28 16:52:27 -03:00
2016-02-02 03:58:33 -04:00
// if we did not get a message, and the idle timer of 2 seconds
2014-03-28 16:52:27 -03:00
// has expired, re-initialise the GPS. This will cause GPS
// detection to run again
2018-04-11 09:16:23 -03:00
bool data_should_be_logged = false ;
2014-03-28 16:52:27 -03:00
if ( ! result ) {
2017-07-08 20:06:19 -03:00
if ( tnow - timing [ instance ] . last_message_time_ms > GPS_TIMEOUT_MS ) {
2018-10-30 14:25:35 -03:00
memset ( ( void * ) & state [ instance ] , 0 , sizeof ( state [ instance ] ) ) ;
2017-09-23 20:55:02 -03:00
state [ instance ] . instance = instance ;
2017-05-24 16:19:10 -03:00
state [ instance ] . hdop = GPS_UNKNOWN_DOP ;
state [ instance ] . vdop = GPS_UNKNOWN_DOP ;
2014-04-01 17:48:36 -03:00
timing [ instance ] . last_message_time_ms = tnow ;
2017-07-08 20:06:19 -03:00
timing [ instance ] . delta_time_ms = GPS_TIMEOUT_MS ;
2018-02-14 20:59:14 -04:00
// do not try to detect again if type is MAV
if ( _type [ instance ] = = GPS_TYPE_MAV ) {
state [ instance ] . status = NO_FIX ;
} else {
// free the driver before we run the next detection, so we
// don't end up with two allocated at any time
delete drivers [ instance ] ;
drivers [ instance ] = nullptr ;
state [ instance ] . status = NO_GPS ;
}
2018-04-11 09:16:23 -03:00
// log this data as a "flag" that the GPS is no longer
// valid (see PR#8144)
data_should_be_logged = true ;
2014-03-28 16:52:27 -03:00
}
} else {
2018-05-15 22:16:01 -03:00
if ( state [ instance ] . uart_timestamp_ms ! = 0 ) {
// set the timestamp for this messages based on
// set_uart_timestamp() in backend, if available
tnow = state [ instance ] . uart_timestamp_ms ;
state [ instance ] . uart_timestamp_ms = 0 ;
}
2017-07-08 20:06:19 -03:00
// delta will only be correct after parsing two messages
timing [ instance ] . delta_time_ms = tnow - timing [ instance ] . last_message_time_ms ;
2014-03-28 16:52:27 -03:00
timing [ instance ] . last_message_time_ms = tnow ;
if ( state [ instance ] . status > = GPS_OK_FIX_2D ) {
timing [ instance ] . last_fix_time_ms = tnow ;
}
2018-04-11 09:16:23 -03:00
data_should_be_logged = true ;
}
2019-05-26 22:34:13 -03:00
# ifndef HAL_BUILD_AP_PERIPH
2018-04-11 09:16:23 -03:00
if ( data_should_be_logged & &
2019-09-06 21:13:19 -03:00
( should_log ( ) | | AP : : ahrs ( ) . have_ekf_logging ( ) ) ) {
2019-01-18 00:24:08 -04:00
AP : : logger ( ) . Write_GPS ( instance ) ;
2014-03-28 16:52:27 -03:00
}
2018-04-10 01:48:30 -03:00
if ( state [ instance ] . status > = GPS_OK_FIX_3D ) {
const uint64_t now = time_epoch_usec ( instance ) ;
2019-07-31 00:17:06 -03:00
if ( now ! = 0 ) {
AP : : rtc ( ) . set_utc_usec ( now , AP_RTC : : SOURCE_GPS ) ;
}
2018-04-10 01:48:30 -03:00
}
2019-05-26 22:34:13 -03:00
# else
( void ) data_should_be_logged ;
# endif
2014-03-28 16:52:27 -03:00
}
/*
update all GPS instances
*/
2017-04-02 11:56:26 -03:00
void AP_GPS : : update ( void )
2014-03-28 16:52:27 -03:00
{
2017-01-29 19:02:57 -04:00
for ( uint8_t i = 0 ; i < GPS_MAX_RECEIVERS ; i + + ) {
2014-03-28 16:52:27 -03:00
update_instance ( i ) ;
}
2014-03-31 16:14:19 -03:00
2017-01-29 19:02:57 -04:00
// calculate number of instances
for ( uint8_t i = 0 ; i < GPS_MAX_RECEIVERS ; i + + ) {
2019-10-09 01:30:39 -03:00
if ( drivers [ i ] ! = nullptr ) {
2014-04-03 18:32:34 -03:00
num_instances = i + 1 ;
}
2017-01-29 19:02:57 -04:00
}
2015-05-21 18:07:59 -03:00
2019-05-26 22:34:13 -03:00
# if defined(GPS_BLENDED_INSTANCE)
2017-01-29 19:02:57 -04:00
// if blending is requested, attempt to calculate weighting for each GPS
if ( _auto_switch = = 2 ) {
_output_is_blended = calc_blend_weights ( ) ;
2017-03-08 20:47:40 -04:00
// adjust blend health counter
if ( ! _output_is_blended ) {
_blend_health_counter = MIN ( _blend_health_counter + BLEND_COUNTER_FAILURE_INCREMENT , 100 ) ;
} else if ( _blend_health_counter > 0 ) {
_blend_health_counter - - ;
}
2017-03-10 03:09:23 -04:00
// stop blending if unhealthy
if ( _blend_health_counter > = 50 ) {
_output_is_blended = false ;
}
2017-01-29 19:02:57 -04:00
} else {
_output_is_blended = false ;
2017-03-10 02:46:16 -04:00
_blend_health_counter = 0 ;
2017-01-29 19:02:57 -04:00
}
2015-05-21 18:07:59 -03:00
2017-01-29 19:02:57 -04:00
if ( _output_is_blended ) {
// Use the weighting to calculate blended GPS states
calc_blended_state ( ) ;
// set primary to the virtual instance
2017-03-10 02:46:16 -04:00
primary_instance = GPS_BLENDED_INSTANCE ;
2017-01-29 19:02:57 -04:00
} else {
// use switch logic to find best GPS
uint32_t now = AP_HAL : : millis ( ) ;
2018-06-15 09:26:51 -03:00
if ( _auto_switch = = 3 ) {
// select the second GPS instance
primary_instance = 1 ;
} else if ( _auto_switch > = 1 ) {
2017-01-29 19:02:57 -04:00
// handling switching away from blended GPS
2017-03-10 02:46:16 -04:00
if ( primary_instance = = GPS_BLENDED_INSTANCE ) {
2017-01-29 19:02:57 -04:00
primary_instance = 0 ;
for ( uint8_t i = 1 ; i < GPS_MAX_RECEIVERS ; i + + ) {
// choose GPS with highest state or higher number of satellites
if ( ( state [ i ] . status > state [ primary_instance ] . status ) | |
( ( state [ i ] . status = = state [ primary_instance ] . status ) & & ( state [ i ] . num_sats > state [ primary_instance ] . num_sats ) ) ) {
primary_instance = i ;
_last_instance_swap_ms = now ;
}
}
} else {
// handle switch between real GPSs
for ( uint8_t i = 0 ; i < GPS_MAX_RECEIVERS ; i + + ) {
if ( i = = primary_instance ) {
continue ;
}
if ( state [ i ] . status > state [ primary_instance ] . status ) {
// we have a higher status lock, or primary is set to the blended GPS, change GPS
primary_instance = i ;
_last_instance_swap_ms = now ;
continue ;
}
bool another_gps_has_1_or_more_sats = ( state [ i ] . num_sats > = state [ primary_instance ] . num_sats + 1 ) ;
if ( state [ i ] . status = = state [ primary_instance ] . status & & another_gps_has_1_or_more_sats ) {
bool another_gps_has_2_or_more_sats = ( state [ i ] . num_sats > = state [ primary_instance ] . num_sats + 2 ) ;
2017-04-02 11:56:26 -03:00
if ( ( another_gps_has_1_or_more_sats & & ( now - _last_instance_swap_ms ) > = 20000 ) | |
( another_gps_has_2_or_more_sats & & ( now - _last_instance_swap_ms ) > = 5000 ) ) {
2017-01-29 19:02:57 -04:00
// this GPS has more satellites than the
// current primary, switch primary. Once we switch we will
// then tend to stick to the new GPS as primary. We don't
// want to switch too often as it will look like a
// position shift to the controllers.
primary_instance = i ;
_last_instance_swap_ms = now ;
}
}
2015-05-21 18:07:59 -03:00
}
2014-06-06 18:58:11 -03:00
}
} else {
2017-01-29 19:02:57 -04:00
// AUTO_SWITCH is 0 so no switching of GPSs
2014-06-06 18:58:11 -03:00
primary_instance = 0 ;
2014-04-03 18:32:34 -03:00
}
2017-01-29 19:02:57 -04:00
// copy the primary instance to the blended instance in case it is enabled later
2017-03-10 02:46:16 -04:00
state [ GPS_BLENDED_INSTANCE ] = state [ primary_instance ] ;
2017-01-29 19:02:57 -04:00
_blended_antenna_offset = _antenna_offset [ primary_instance ] ;
2014-04-03 18:32:34 -03:00
}
2019-05-26 22:34:13 -03:00
# endif // GPS_BLENDED_INSTANCE
2015-11-03 09:17:25 -04:00
2019-05-26 22:34:13 -03:00
# ifndef HAL_BUILD_AP_PERIPH
2016-12-15 09:12:54 -04:00
// update notify with gps status. We always base this on the primary_instance
2014-09-21 09:48:35 -03:00
AP_Notify : : flags . gps_status = state [ primary_instance ] . status ;
2015-12-22 17:14:15 -04:00
AP_Notify : : flags . gps_num_sats = state [ primary_instance ] . num_sats ;
2019-05-26 22:34:13 -03:00
# endif
2014-03-28 16:52:27 -03:00
}
2019-04-30 07:22:48 -03:00
void AP_GPS : : handle_gps_inject ( const mavlink_message_t & msg )
2017-07-25 04:59:14 -03:00
{
mavlink_gps_inject_data_t packet ;
2019-04-30 07:22:48 -03:00
mavlink_msg_gps_inject_data_decode ( & msg , & packet ) ;
2017-07-25 04:59:14 -03:00
2019-10-21 08:11:26 -03:00
if ( packet . len > sizeof ( packet . data ) ) {
// invalid packet
return ;
}
handle_gps_rtcm_fragment ( 0 , packet . data , packet . len ) ;
2017-07-25 04:59:14 -03:00
}
2016-05-19 20:24:08 -03:00
/*
pass along a mavlink message ( for MAV type )
*/
2019-04-30 07:22:48 -03:00
void AP_GPS : : handle_msg ( const mavlink_message_t & msg )
2016-05-19 20:24:08 -03:00
{
2019-04-30 07:22:48 -03:00
switch ( msg . msgid ) {
2017-07-25 21:19:37 -03:00
case MAVLINK_MSG_ID_GPS_RTCM_DATA :
2016-10-04 04:37:01 -03:00
// pass data to de-fragmenter
handle_gps_rtcm_data ( msg ) ;
2017-07-25 21:19:37 -03:00
break ;
case MAVLINK_MSG_ID_GPS_INJECT_DATA :
2017-07-25 04:59:14 -03:00
handle_gps_inject ( msg ) ;
2017-07-25 21:19:37 -03:00
break ;
default : {
uint8_t i ;
for ( i = 0 ; i < num_instances ; i + + ) {
if ( ( drivers [ i ] ! = nullptr ) & & ( _type [ i ] ! = GPS_TYPE_NONE ) ) {
drivers [ i ] - > handle_msg ( msg ) ;
}
2016-05-19 20:24:08 -03:00
}
2017-07-25 21:19:37 -03:00
break ;
}
2016-05-19 20:24:08 -03:00
}
}
2014-03-31 06:48:22 -03:00
/*
set HIL ( hardware in the loop ) status for a GPS instance
*/
2017-04-02 11:56:26 -03:00
void AP_GPS : : setHIL ( uint8_t instance , GPS_Status _status , uint64_t time_epoch_ms ,
const Location & _location , const Vector3f & _velocity , uint8_t _num_sats ,
uint16_t hdop )
2014-03-28 16:52:27 -03:00
{
2017-01-29 19:02:57 -04:00
if ( instance > = GPS_MAX_RECEIVERS ) {
2014-04-01 17:49:29 -03:00
return ;
}
2017-09-23 20:53:40 -03:00
const uint32_t tnow = AP_HAL : : millis ( ) ;
2014-04-01 17:49:29 -03:00
GPS_State & istate = state [ instance ] ;
2014-03-28 16:52:27 -03:00
istate . status = _status ;
istate . location = _location ;
istate . velocity = _velocity ;
2016-04-16 06:58:46 -03:00
istate . ground_speed = norm ( istate . velocity . x , istate . velocity . y ) ;
2016-05-04 22:28:35 -03:00
istate . ground_course = wrap_360 ( degrees ( atan2f ( istate . velocity . y , istate . velocity . x ) ) ) ;
2014-04-01 17:49:29 -03:00
istate . hdop = hdop ;
2014-03-28 16:52:27 -03:00
istate . num_sats = _num_sats ;
istate . last_gps_time_ms = tnow ;
2017-02-11 05:26:18 -04:00
uint64_t gps_time_ms = time_epoch_ms - UNIX_OFFSET_MSEC ;
2017-05-05 07:09:29 -03:00
istate . time_week = gps_time_ms / AP_MSEC_PER_WEEK ;
istate . time_week_ms = gps_time_ms - istate . time_week * AP_MSEC_PER_WEEK ;
2014-04-01 17:49:29 -03:00
timing [ instance ] . last_message_time_ms = tnow ;
timing [ instance ] . last_fix_time_ms = tnow ;
_type [ instance ] . set ( GPS_TYPE_HIL ) ;
2014-03-28 16:52:27 -03:00
}
2014-04-04 17:33:34 -03:00
2016-05-04 05:16:05 -03:00
// set accuracy for HIL
2016-05-05 03:04:18 -03:00
void AP_GPS : : setHIL_Accuracy ( uint8_t instance , float vdop , float hacc , float vacc , float sacc , bool _have_vertical_velocity , uint32_t sample_ms )
2016-05-04 05:16:05 -03:00
{
2017-03-08 05:49:58 -04:00
if ( instance > = GPS_MAX_RECEIVERS ) {
return ;
}
2016-05-04 05:16:05 -03:00
GPS_State & istate = state [ instance ] ;
2016-05-04 22:40:07 -03:00
istate . vdop = vdop * 100 ;
2016-05-04 05:16:05 -03:00
istate . horizontal_accuracy = hacc ;
istate . vertical_accuracy = vacc ;
istate . speed_accuracy = sacc ;
istate . have_horizontal_accuracy = true ;
istate . have_vertical_accuracy = true ;
istate . have_speed_accuracy = true ;
2016-05-04 22:28:35 -03:00
istate . have_vertical_velocity | = _have_vertical_velocity ;
2016-05-05 03:04:18 -03:00
if ( sample_ms ! = 0 ) {
timing [ instance ] . last_message_time_ms = sample_ms ;
timing [ instance ] . last_fix_time_ms = sample_ms ;
}
2016-05-04 05:16:05 -03:00
}
2014-04-04 17:33:34 -03:00
/**
2017-05-05 09:57:22 -03:00
Lock a GPS port , preventing the GPS driver from using it . This can
2014-04-04 17:33:34 -03:00
be used to allow a user to control a GPS port via the
SERIAL_CONTROL protocol
*/
2017-04-02 11:56:26 -03:00
void AP_GPS : : lock_port ( uint8_t instance , bool lock )
2014-04-04 17:33:34 -03:00
{
2015-04-22 20:10:46 -03:00
2017-01-29 19:02:57 -04:00
if ( instance > = GPS_MAX_RECEIVERS ) {
2014-04-04 17:33:34 -03:00
return ;
}
if ( lock ) {
locked_ports | = ( 1U < < instance ) ;
} else {
locked_ports & = ~ ( 1U < < instance ) ;
}
}
2014-06-06 18:58:11 -03:00
2017-04-02 11:56:26 -03:00
// Inject a packet of raw binary to a GPS
2019-10-21 08:11:26 -03:00
void AP_GPS : : inject_data ( const uint8_t * data , uint16_t len )
2015-04-22 20:10:46 -03:00
{
//Support broadcasting to all GPSes.
2015-06-27 04:26:25 -03:00
if ( _inject_to = = GPS_RTK_INJECT_TO_ALL ) {
2017-01-29 19:02:57 -04:00
for ( uint8_t i = 0 ; i < GPS_MAX_RECEIVERS ; i + + ) {
2015-04-22 20:10:46 -03:00
inject_data ( i , data , len ) ;
}
} else {
inject_data ( _inject_to , data , len ) ;
}
}
2019-10-21 08:11:26 -03:00
void AP_GPS : : inject_data ( uint8_t instance , const uint8_t * data , uint16_t len )
2015-04-22 20:10:46 -03:00
{
2017-01-29 19:02:57 -04:00
if ( instance < GPS_MAX_RECEIVERS & & drivers [ instance ] ! = nullptr ) {
2015-04-22 20:10:46 -03:00
drivers [ instance ] - > inject_data ( data , len ) ;
2016-12-15 09:12:54 -04:00
}
2017-04-02 11:56:26 -03:00
}
2015-04-22 20:10:46 -03:00
2017-04-02 11:56:26 -03:00
void AP_GPS : : send_mavlink_gps_raw ( mavlink_channel_t chan )
2014-06-06 18:58:11 -03:00
{
2015-01-02 02:08:11 -04:00
const Location & loc = location ( 0 ) ;
2017-09-22 05:16:12 -03:00
float hacc = 0.0f ;
float vacc = 0.0f ;
float sacc = 0.0f ;
horizontal_accuracy ( 0 , hacc ) ;
vertical_accuracy ( 0 , vacc ) ;
speed_accuracy ( 0 , sacc ) ;
2015-01-02 02:08:11 -04:00
mavlink_msg_gps_raw_int_send (
chan ,
last_fix_time_ms ( 0 ) * ( uint64_t ) 1000 ,
status ( 0 ) ,
loc . lat , // in 1E7 degrees
loc . lng , // in 1E7 degrees
loc . alt * 10UL , // in mm
get_hdop ( 0 ) ,
2015-09-07 19:37:13 -03:00
get_vdop ( 0 ) ,
2015-01-02 02:08:11 -04:00
ground_speed ( 0 ) * 100 , // cm/s
2016-05-04 22:28:35 -03:00
ground_course ( 0 ) * 100 , // 1/100 degrees,
2017-09-22 05:16:12 -03:00
num_sats ( 0 ) ,
0 , // TODO: Elipsoid height in mm
hacc * 1000 , // one-sigma standard deviation in mm
vacc * 1000 , // one-sigma standard deviation in mm
sacc * 1000 , // one-sigma standard deviation in mm/s
0 ) ; // TODO one-sigma heading accuracy standard deviation
2014-06-06 18:58:11 -03:00
}
2019-05-26 22:34:13 -03:00
# if GPS_MAX_RECEIVERS > 1
2017-04-02 11:56:26 -03:00
void AP_GPS : : send_mavlink_gps2_raw ( mavlink_channel_t chan )
2014-06-06 18:58:11 -03:00
{
2019-10-28 14:14:52 -03:00
// always send the message once we've ever seen the GPS
if ( ! has_had_second_instance ) {
2015-01-02 02:08:11 -04:00
return ;
}
const Location & loc = location ( 1 ) ;
mavlink_msg_gps2_raw_send (
chan ,
last_fix_time_ms ( 1 ) * ( uint64_t ) 1000 ,
status ( 1 ) ,
loc . lat ,
loc . lng ,
loc . alt * 10UL ,
get_hdop ( 1 ) ,
2015-09-07 19:37:13 -03:00
get_vdop ( 1 ) ,
2015-01-02 02:08:11 -04:00
ground_speed ( 1 ) * 100 , // cm/s
2016-05-04 22:28:35 -03:00
ground_course ( 1 ) * 100 , // 1/100 degrees,
2015-01-02 02:08:11 -04:00
num_sats ( 1 ) ,
2019-04-20 18:54:21 -03:00
state [ 1 ] . rtk_num_sats ,
state [ 1 ] . rtk_age_ms ) ;
2014-06-06 18:58:11 -03:00
}
2019-05-26 22:34:13 -03:00
# endif // GPS_MAX_RECEIVERS
2014-06-06 18:58:11 -03:00
2017-06-30 10:45:24 -03:00
void AP_GPS : : send_mavlink_gps_rtk ( mavlink_channel_t chan , uint8_t inst )
2014-06-06 18:58:11 -03:00
{
2017-06-30 10:45:24 -03:00
if ( inst > = GPS_MAX_RECEIVERS ) {
return ;
2014-06-06 18:58:11 -03:00
}
2017-09-24 18:48:52 -03:00
if ( drivers [ inst ] ! = nullptr & & drivers [ inst ] - > supports_mavlink_gps_rtk_message ( ) ) {
2017-06-30 10:45:24 -03:00
drivers [ inst ] - > send_mavlink_gps_rtk ( chan ) ;
2014-06-06 18:58:11 -03:00
}
}
2016-02-02 03:58:33 -04:00
2019-07-23 03:20:47 -03:00
bool AP_GPS : : first_unconfigured_gps ( uint8_t & instance ) const
2016-02-09 16:59:12 -04:00
{
2017-04-02 11:56:26 -03:00
for ( int i = 0 ; i < GPS_MAX_RECEIVERS ; i + + ) {
if ( _type [ i ] ! = GPS_TYPE_NONE & & ( drivers [ i ] = = nullptr | | ! drivers [ i ] - > is_configured ( ) ) ) {
2019-07-23 03:20:47 -03:00
instance = i ;
return true ;
2016-02-02 03:58:33 -04:00
}
}
2019-07-23 03:20:47 -03:00
return false ;
2016-02-02 03:58:33 -04:00
}
2016-02-09 16:59:12 -04:00
2017-04-02 11:56:26 -03:00
void AP_GPS : : broadcast_first_configuration_failure_reason ( void ) const
{
2019-07-23 03:20:47 -03:00
uint8_t unconfigured ;
if ( first_unconfigured_gps ( unconfigured ) ) {
if ( drivers [ unconfigured ] = = nullptr ) {
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " GPS %d: was not found " , unconfigured + 1 ) ;
} else {
drivers [ unconfigured ] - > broadcast_configuration_failure_reason ( ) ;
}
2016-04-12 00:16:20 -03:00
}
}
2017-03-08 20:47:40 -04:00
// pre-arm check that all GPSs are close to each other. farthest distance between GPSs (in meters) is returned
bool AP_GPS : : all_consistent ( float & distance ) const
{
// return true immediately if only one valid receiver
if ( num_instances < = 1 | |
2017-03-10 02:46:16 -04:00
drivers [ 0 ] = = nullptr | | _type [ 0 ] = = GPS_TYPE_NONE ) {
2017-03-08 20:47:40 -04:00
distance = 0 ;
return true ;
}
// calculate distance
2019-04-08 10:29:11 -03:00
distance = state [ 0 ] . location . get_distance_NED ( state [ 1 ] . location ) . length ( ) ;
2017-03-08 20:47:40 -04:00
// success if distance is within 50m
return ( distance < 50 ) ;
}
// pre-arm check of GPS blending. True means healthy or that blending is not being used
2017-03-10 04:12:20 -04:00
bool AP_GPS : : blend_health_check ( ) const
2017-03-08 20:47:40 -04:00
{
return ( _blend_health_counter < 50 ) ;
}
2017-04-02 11:56:26 -03:00
/*
2019-10-21 08:11:26 -03:00
re - assemble fragmented RTCM data
2016-10-04 04:37:01 -03:00
*/
2019-10-21 08:11:26 -03:00
void AP_GPS : : handle_gps_rtcm_fragment ( uint8_t flags , const uint8_t * data , uint8_t len )
2016-10-04 04:37:01 -03:00
{
2019-10-21 08:11:26 -03:00
if ( ( flags & 1 ) = = 0 ) {
2016-10-04 04:37:01 -03:00
// it is not fragmented, pass direct
2019-10-21 08:11:26 -03:00
inject_data ( data , len ) ;
2016-10-04 04:37:01 -03:00
return ;
}
// see if we need to allocate re-assembly buffer
if ( rtcm_buffer = = nullptr ) {
rtcm_buffer = ( struct rtcm_buffer * ) calloc ( 1 , sizeof ( * rtcm_buffer ) ) ;
if ( rtcm_buffer = = nullptr ) {
// nothing to do but discard the data
return ;
}
}
2019-10-21 08:11:26 -03:00
uint8_t fragment = ( flags > > 1U ) & 0x03 ;
uint8_t sequence = ( flags > > 3U ) & 0x1F ;
2016-10-04 04:37:01 -03:00
// see if this fragment is consistent with existing fragments
if ( rtcm_buffer - > fragments_received & &
( rtcm_buffer - > sequence ! = sequence | |
2017-03-10 02:49:42 -04:00
( rtcm_buffer - > fragments_received & ( 1U < < fragment ) ) ) ) {
2016-10-04 04:37:01 -03:00
// we have one or more partial fragments already received
// which conflict with the new fragment, discard previous fragments
memset ( rtcm_buffer , 0 , sizeof ( * rtcm_buffer ) ) ;
}
// add this fragment
rtcm_buffer - > sequence = sequence ;
rtcm_buffer - > fragments_received | = ( 1U < < fragment ) ;
// copy the data
2019-10-21 08:11:26 -03:00
memcpy ( & rtcm_buffer - > buffer [ MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN * ( uint16_t ) fragment ] , data , len ) ;
2016-10-04 04:37:01 -03:00
// when we get a fragment of less than max size then we know the
// number of fragments. Note that this means if you want to send a
// block of RTCM data of an exact multiple of the buffer size you
// need to send a final packet of zero length
2019-10-21 08:11:26 -03:00
if ( len < MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN ) {
2016-10-04 04:37:01 -03:00
rtcm_buffer - > fragment_count = fragment + 1 ;
2019-10-21 08:11:26 -03:00
rtcm_buffer - > total_length = ( MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN * fragment ) + len ;
2016-10-04 04:37:01 -03:00
} else if ( rtcm_buffer - > fragments_received = = 0x0F ) {
// special case of 4 full fragments
rtcm_buffer - > fragment_count = 4 ;
rtcm_buffer - > total_length = MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN * 4 ;
}
// see if we have all fragments
if ( rtcm_buffer - > fragment_count ! = 0 & &
rtcm_buffer - > fragments_received = = ( 1U < < rtcm_buffer - > fragment_count ) - 1 ) {
// we have them all, inject
2016-12-08 04:33:39 -04:00
inject_data ( rtcm_buffer - > buffer , rtcm_buffer - > total_length ) ;
2016-10-04 04:37:01 -03:00
memset ( rtcm_buffer , 0 , sizeof ( * rtcm_buffer ) ) ;
}
}
2019-10-21 08:11:26 -03:00
/*
re - assemble GPS_RTCM_DATA message
*/
void AP_GPS : : handle_gps_rtcm_data ( const mavlink_message_t & msg )
{
mavlink_gps_rtcm_data_t packet ;
mavlink_msg_gps_rtcm_data_decode ( & msg , & packet ) ;
if ( packet . len > sizeof ( packet . data ) ) {
// invalid packet
return ;
}
handle_gps_rtcm_fragment ( packet . flags , packet . data , packet . len ) ;
}
2019-01-18 00:23:42 -04:00
void AP_GPS : : Write_AP_Logger_Log_Startup_messages ( )
2016-08-01 08:58:23 -03:00
{
for ( uint8_t instance = 0 ; instance < num_instances ; instance + + ) {
if ( drivers [ instance ] = = nullptr | | state [ instance ] . status = = NO_GPS ) {
continue ;
}
2019-01-18 00:23:42 -04:00
drivers [ instance ] - > Write_AP_Logger_Log_Startup_messages ( ) ;
2016-08-01 08:58:23 -03:00
}
}
2016-12-18 19:31:28 -04:00
/*
2017-01-29 19:02:57 -04:00
return the expected lag ( in seconds ) in the position and velocity readings from the gps
2017-05-21 22:07:26 -03:00
return true if the GPS hardware configuration is known or the delay parameter has been set
2016-12-18 19:31:28 -04:00
*/
2017-05-21 22:07:26 -03:00
bool AP_GPS : : get_lag ( uint8_t instance , float & lag_sec ) const
2016-12-18 19:31:28 -04:00
{
2018-06-28 19:36:35 -03:00
// always enusre a lag is provided
2019-04-04 07:47:33 -03:00
lag_sec = 0.22f ;
2018-06-28 19:36:35 -03:00
2019-03-05 19:24:02 -04:00
if ( instance > = GPS_MAX_INSTANCES ) {
return false ;
}
2019-05-26 22:34:13 -03:00
# if defined(GPS_BLENDED_INSTANCE)
2017-01-29 19:02:57 -04:00
// return lag of blended GPS
2017-05-21 22:07:26 -03:00
if ( instance = = GPS_BLENDED_INSTANCE ) {
lag_sec = _blended_lag_sec ;
// auto switching uses all GPS receivers, so all must be configured
2019-07-23 03:20:47 -03:00
uint8_t inst ; // we don't actually care what the number is, but must pass it
return first_unconfigured_gps ( inst ) ;
2017-01-29 19:02:57 -04:00
}
2019-05-26 22:34:13 -03:00
# endif
2017-01-29 19:02:57 -04:00
2016-12-28 18:20:06 -04:00
if ( _delay_ms [ instance ] > 0 ) {
// if the user has specified a non zero time delay, always return that value
2017-05-21 22:07:26 -03:00
lag_sec = 0.001f * ( float ) _delay_ms [ instance ] ;
// the user is always right !!
return true ;
2016-12-28 18:20:06 -04:00
} else if ( drivers [ instance ] = = nullptr | | state [ instance ] . status = = NO_GPS ) {
2017-05-24 14:42:03 -03:00
// no GPS was detected in this instance so return the worst possible lag term
if ( _type [ instance ] = = GPS_TYPE_NONE ) {
lag_sec = 0.0f ;
return true ;
}
return _type [ instance ] = = GPS_TYPE_AUTO ;
2016-12-28 18:20:06 -04:00
} else {
// the user has not specified a delay so we determine it from the GPS type
2017-05-24 14:42:03 -03:00
return drivers [ instance ] - > get_lag ( lag_sec ) ;
2016-12-18 19:31:28 -04:00
}
}
2017-01-29 19:02:57 -04:00
2017-03-08 05:56:52 -04:00
// return a 3D vector defining the offset of the GPS antenna in meters relative to the body frame origin
const Vector3f & AP_GPS : : get_antenna_offset ( uint8_t instance ) const
{
2019-03-05 19:24:02 -04:00
if ( instance > = GPS_MAX_INSTANCES ) {
// we have to return a reference so use instance 0
return _antenna_offset [ 0 ] ;
}
2019-05-26 22:34:13 -03:00
# if defined(GPS_BLENDED_INSTANCE)
2019-03-05 19:24:02 -04:00
if ( instance = = GPS_BLENDED_INSTANCE ) {
2017-03-08 05:56:52 -04:00
// return an offset for the blended GPS solution
return _blended_antenna_offset ;
}
2019-05-26 22:34:13 -03:00
# endif
2019-03-05 19:24:02 -04:00
return _antenna_offset [ instance ] ;
2017-03-08 05:56:52 -04:00
}
2017-03-07 23:33:31 -04:00
/*
2017-06-11 04:38:11 -03:00
returns the desired gps update rate in milliseconds
2017-09-23 20:51:47 -03:00
this does not provide any guarantee that the GPS is updating at the requested
2017-06-11 04:38:11 -03:00
rate it is simply a helper for use in the backends for determining what rate
they should be configuring the GPS to run at
2017-03-07 23:33:31 -04:00
*/
uint16_t AP_GPS : : get_rate_ms ( uint8_t instance ) const
{
// sanity check
if ( instance > = num_instances | | _rate_ms [ instance ] < = 0 ) {
return GPS_MAX_RATE_MS ;
}
return MIN ( _rate_ms [ instance ] , GPS_MAX_RATE_MS ) ;
}
2019-05-26 22:34:13 -03:00
# if defined(GPS_BLENDED_INSTANCE)
2017-01-29 19:02:57 -04:00
/*
2017-09-23 20:51:47 -03:00
calculate the weightings used to blend GPSs location and velocity data
2017-01-29 19:02:57 -04:00
*/
bool AP_GPS : : calc_blend_weights ( void )
{
// zero the blend weights
memset ( & _blend_weights , 0 , sizeof ( _blend_weights ) ) ;
// exit immediately if not enough receivers to do blending
2019-03-07 17:26:11 -04:00
if ( state [ 0 ] . status < = NO_FIX | | state [ 1 ] . status < = NO_FIX ) {
2017-01-29 19:02:57 -04:00
return false ;
}
// Use the oldest non-zero time, but if time difference is excessive, use newest to prevent a disconnected receiver from blocking updates
uint32_t max_ms = 0 ; // newest non-zero system time of arrival of a GPS message
uint32_t min_ms = - 1 ; // oldest non-zero system time of arrival of a GPS message
int16_t max_rate_ms = 0 ; // largest update interval of a GPS receiver
for ( uint8_t i = 0 ; i < GPS_MAX_RECEIVERS ; i + + ) {
// Find largest and smallest times
if ( state [ i ] . last_gps_time_ms > max_ms ) {
max_ms = state [ i ] . last_gps_time_ms ;
}
if ( ( state [ i ] . last_gps_time_ms < min_ms ) & & ( state [ i ] . last_gps_time_ms > 0 ) ) {
min_ms = state [ i ] . last_gps_time_ms ;
}
2017-03-07 23:33:31 -04:00
if ( get_rate_ms ( i ) > max_rate_ms ) {
max_rate_ms = get_rate_ms ( i ) ;
2017-01-29 19:02:57 -04:00
}
}
if ( ( int32_t ) ( max_ms - min_ms ) < ( int32_t ) ( 2 * max_rate_ms ) ) {
// data is not too delayed so use the oldest time_stamp to give a chance for data from that receiver to be updated
2017-03-10 02:46:16 -04:00
state [ GPS_BLENDED_INSTANCE ] . last_gps_time_ms = min_ms ;
2017-01-29 19:02:57 -04:00
} else {
// receiver data has timed out so fail out of blending
return false ;
}
// calculate the sum squared speed accuracy across all GPS sensors
float speed_accuracy_sum_sq = 0.0f ;
if ( _blend_mask & BLEND_MASK_USE_SPD_ACC ) {
for ( uint8_t i = 0 ; i < GPS_MAX_RECEIVERS ; i + + ) {
if ( state [ i ] . status > = GPS_OK_FIX_3D ) {
if ( state [ i ] . have_speed_accuracy & & state [ i ] . speed_accuracy > 0.0f ) {
speed_accuracy_sum_sq + = state [ i ] . speed_accuracy * state [ i ] . speed_accuracy ;
} else {
// not all receivers support this metric so set it to zero and don't use it
speed_accuracy_sum_sq = 0.0f ;
break ;
}
}
}
}
// calculate the sum squared horizontal position accuracy across all GPS sensors
float horizontal_accuracy_sum_sq = 0.0f ;
if ( _blend_mask & BLEND_MASK_USE_HPOS_ACC ) {
for ( uint8_t i = 0 ; i < GPS_MAX_RECEIVERS ; i + + ) {
if ( state [ i ] . status > = GPS_OK_FIX_2D ) {
if ( state [ i ] . have_horizontal_accuracy & & state [ i ] . horizontal_accuracy > 0.0f ) {
horizontal_accuracy_sum_sq + = state [ i ] . horizontal_accuracy * state [ i ] . horizontal_accuracy ;
} else {
// not all receivers support this metric so set it to zero and don't use it
horizontal_accuracy_sum_sq = 0.0f ;
break ;
}
}
}
}
// calculate the sum squared vertical position accuracy across all GPS sensors
float vertical_accuracy_sum_sq = 0.0f ;
if ( _blend_mask & BLEND_MASK_USE_VPOS_ACC ) {
for ( uint8_t i = 0 ; i < GPS_MAX_RECEIVERS ; i + + ) {
if ( state [ i ] . status > = GPS_OK_FIX_3D ) {
if ( state [ i ] . have_vertical_accuracy & & state [ i ] . vertical_accuracy > 0.0f ) {
vertical_accuracy_sum_sq + = state [ i ] . vertical_accuracy * state [ i ] . vertical_accuracy ;
} else {
// not all receivers support this metric so set it to zero and don't use it
vertical_accuracy_sum_sq = 0.0f ;
break ;
}
}
}
}
// Check if we can do blending using reported accuracy
bool can_do_blending = ( horizontal_accuracy_sum_sq > 0.0f | | vertical_accuracy_sum_sq > 0.0f | | speed_accuracy_sum_sq > 0.0f ) ;
2017-05-05 09:57:22 -03:00
// if we can't do blending using reported accuracy, return false and hard switch logic will be used instead
2017-01-29 19:02:57 -04:00
if ( ! can_do_blending ) {
return false ;
}
float sum_of_all_weights = 0.0f ;
// calculate a weighting using the reported horizontal position
float hpos_blend_weights [ GPS_MAX_RECEIVERS ] = { } ;
if ( horizontal_accuracy_sum_sq > 0.0f & & ( _blend_mask & BLEND_MASK_USE_HPOS_ACC ) ) {
// calculate the weights using the inverse of the variances
float sum_of_hpos_weights = 0.0f ;
for ( uint8_t i = 0 ; i < GPS_MAX_RECEIVERS ; i + + ) {
2017-03-10 22:25:46 -04:00
if ( state [ i ] . status > = GPS_OK_FIX_2D & & state [ i ] . horizontal_accuracy > = 0.001f ) {
2017-01-29 19:02:57 -04:00
hpos_blend_weights [ i ] = horizontal_accuracy_sum_sq / ( state [ i ] . horizontal_accuracy * state [ i ] . horizontal_accuracy ) ;
sum_of_hpos_weights + = hpos_blend_weights [ i ] ;
}
}
// normalise the weights
if ( sum_of_hpos_weights > 0.0f ) {
for ( uint8_t i = 0 ; i < GPS_MAX_RECEIVERS ; i + + ) {
hpos_blend_weights [ i ] = hpos_blend_weights [ i ] / sum_of_hpos_weights ;
}
sum_of_all_weights + = 1.0f ;
}
}
// calculate a weighting using the reported vertical position accuracy
2017-03-10 02:48:07 -04:00
float vpos_blend_weights [ GPS_MAX_RECEIVERS ] = { } ;
2017-01-29 19:02:57 -04:00
if ( vertical_accuracy_sum_sq > 0.0f & & ( _blend_mask & BLEND_MASK_USE_VPOS_ACC ) ) {
// calculate the weights using the inverse of the variances
float sum_of_vpos_weights = 0.0f ;
for ( uint8_t i = 0 ; i < GPS_MAX_RECEIVERS ; i + + ) {
2017-03-10 22:25:46 -04:00
if ( state [ i ] . status > = GPS_OK_FIX_3D & & state [ i ] . vertical_accuracy > = 0.001f ) {
2017-01-29 19:02:57 -04:00
vpos_blend_weights [ i ] = vertical_accuracy_sum_sq / ( state [ i ] . vertical_accuracy * state [ i ] . vertical_accuracy ) ;
sum_of_vpos_weights + = vpos_blend_weights [ i ] ;
}
}
// normalise the weights
if ( sum_of_vpos_weights > 0.0f ) {
for ( uint8_t i = 0 ; i < GPS_MAX_RECEIVERS ; i + + ) {
vpos_blend_weights [ i ] = vpos_blend_weights [ i ] / sum_of_vpos_weights ;
}
sum_of_all_weights + = 1.0f ;
} ;
}
// calculate a weighting using the reported speed accuracy
2017-03-10 02:48:07 -04:00
float spd_blend_weights [ GPS_MAX_RECEIVERS ] = { } ;
2017-01-29 19:02:57 -04:00
if ( speed_accuracy_sum_sq > 0.0f & & ( _blend_mask & BLEND_MASK_USE_SPD_ACC ) ) {
// calculate the weights using the inverse of the variances
float sum_of_spd_weights = 0.0f ;
for ( uint8_t i = 0 ; i < GPS_MAX_RECEIVERS ; i + + ) {
2017-03-10 22:25:46 -04:00
if ( state [ i ] . status > = GPS_OK_FIX_3D & & state [ i ] . speed_accuracy > = 0.001f ) {
2017-01-29 19:02:57 -04:00
spd_blend_weights [ i ] = speed_accuracy_sum_sq / ( state [ i ] . speed_accuracy * state [ i ] . speed_accuracy ) ;
sum_of_spd_weights + = spd_blend_weights [ i ] ;
}
}
// normalise the weights
if ( sum_of_spd_weights > 0.0f ) {
for ( uint8_t i = 0 ; i < GPS_MAX_RECEIVERS ; i + + ) {
spd_blend_weights [ i ] = spd_blend_weights [ i ] / sum_of_spd_weights ;
}
sum_of_all_weights + = 1.0f ;
}
}
// calculate an overall weight
for ( uint8_t i = 0 ; i < GPS_MAX_RECEIVERS ; i + + ) {
_blend_weights [ i ] = ( hpos_blend_weights [ i ] + vpos_blend_weights [ i ] + spd_blend_weights [ i ] ) / sum_of_all_weights ;
}
return true ;
}
/*
calculate a blended GPS state
*/
void AP_GPS : : calc_blended_state ( void )
{
// initialise the blended states so we can accumulate the results using the weightings for each GPS receiver
2017-03-10 02:46:16 -04:00
state [ GPS_BLENDED_INSTANCE ] . instance = GPS_BLENDED_INSTANCE ;
state [ GPS_BLENDED_INSTANCE ] . status = NO_FIX ;
state [ GPS_BLENDED_INSTANCE ] . time_week_ms = 0 ;
state [ GPS_BLENDED_INSTANCE ] . time_week = 0 ;
state [ GPS_BLENDED_INSTANCE ] . ground_speed = 0.0f ;
state [ GPS_BLENDED_INSTANCE ] . ground_course = 0.0f ;
2017-05-24 16:19:10 -03:00
state [ GPS_BLENDED_INSTANCE ] . hdop = GPS_UNKNOWN_DOP ;
state [ GPS_BLENDED_INSTANCE ] . vdop = GPS_UNKNOWN_DOP ;
2017-03-10 02:46:16 -04:00
state [ GPS_BLENDED_INSTANCE ] . num_sats = 0 ;
state [ GPS_BLENDED_INSTANCE ] . velocity . zero ( ) ;
state [ GPS_BLENDED_INSTANCE ] . speed_accuracy = 1e6 f ;
state [ GPS_BLENDED_INSTANCE ] . horizontal_accuracy = 1e6 f ;
state [ GPS_BLENDED_INSTANCE ] . vertical_accuracy = 1e6 f ;
state [ GPS_BLENDED_INSTANCE ] . have_vertical_velocity = false ;
state [ GPS_BLENDED_INSTANCE ] . have_speed_accuracy = false ;
state [ GPS_BLENDED_INSTANCE ] . have_horizontal_accuracy = false ;
state [ GPS_BLENDED_INSTANCE ] . have_vertical_accuracy = false ;
2019-01-01 23:11:38 -04:00
state [ GPS_BLENDED_INSTANCE ] . location = { } ;
2017-01-29 19:02:57 -04:00
_blended_antenna_offset . zero ( ) ;
_blended_lag_sec = 0 ;
2017-03-10 02:46:16 -04:00
timing [ GPS_BLENDED_INSTANCE ] . last_fix_time_ms = 0 ;
timing [ GPS_BLENDED_INSTANCE ] . last_message_time_ms = 0 ;
2017-01-29 19:02:57 -04:00
// combine the states into a blended solution
for ( uint8_t i = 0 ; i < GPS_MAX_RECEIVERS ; i + + ) {
// use the highest status
2017-03-10 02:46:16 -04:00
if ( state [ i ] . status > state [ GPS_BLENDED_INSTANCE ] . status ) {
state [ GPS_BLENDED_INSTANCE ] . status = state [ i ] . status ;
2017-01-29 19:02:57 -04:00
}
// calculate a blended average velocity
2017-03-10 02:46:16 -04:00
state [ GPS_BLENDED_INSTANCE ] . velocity + = state [ i ] . velocity * _blend_weights [ i ] ;
2017-01-29 19:02:57 -04:00
// report the best valid accuracies and DOP metrics
2017-03-10 02:46:16 -04:00
if ( state [ i ] . have_horizontal_accuracy & & state [ i ] . horizontal_accuracy > 0.0f & & state [ i ] . horizontal_accuracy < state [ GPS_BLENDED_INSTANCE ] . horizontal_accuracy ) {
state [ GPS_BLENDED_INSTANCE ] . have_horizontal_accuracy = true ;
state [ GPS_BLENDED_INSTANCE ] . horizontal_accuracy = state [ i ] . horizontal_accuracy ;
2017-01-29 19:02:57 -04:00
}
2017-03-10 02:46:16 -04:00
if ( state [ i ] . have_vertical_accuracy & & state [ i ] . vertical_accuracy > 0.0f & & state [ i ] . vertical_accuracy < state [ GPS_BLENDED_INSTANCE ] . vertical_accuracy ) {
state [ GPS_BLENDED_INSTANCE ] . have_vertical_accuracy = true ;
state [ GPS_BLENDED_INSTANCE ] . vertical_accuracy = state [ i ] . vertical_accuracy ;
2017-01-29 19:02:57 -04:00
}
2017-04-02 11:56:26 -03:00
if ( state [ i ] . have_vertical_velocity ) {
2017-03-10 02:46:16 -04:00
state [ GPS_BLENDED_INSTANCE ] . have_vertical_velocity = true ;
2017-01-29 19:02:57 -04:00
}
2017-03-10 02:46:16 -04:00
if ( state [ i ] . have_speed_accuracy & & state [ i ] . speed_accuracy > 0.0f & & state [ i ] . speed_accuracy < state [ GPS_BLENDED_INSTANCE ] . speed_accuracy ) {
state [ GPS_BLENDED_INSTANCE ] . have_speed_accuracy = true ;
state [ GPS_BLENDED_INSTANCE ] . speed_accuracy = state [ i ] . speed_accuracy ;
2017-01-29 19:02:57 -04:00
}
2017-03-10 02:46:16 -04:00
if ( state [ i ] . hdop > 0 & & state [ i ] . hdop < state [ GPS_BLENDED_INSTANCE ] . hdop ) {
state [ GPS_BLENDED_INSTANCE ] . hdop = state [ i ] . hdop ;
2017-01-29 19:02:57 -04:00
}
2017-03-10 02:46:16 -04:00
if ( state [ i ] . vdop > 0 & & state [ i ] . vdop < state [ GPS_BLENDED_INSTANCE ] . vdop ) {
state [ GPS_BLENDED_INSTANCE ] . vdop = state [ i ] . vdop ;
2017-01-29 19:02:57 -04:00
}
2017-03-10 02:46:16 -04:00
if ( state [ i ] . num_sats > 0 & & state [ i ] . num_sats > state [ GPS_BLENDED_INSTANCE ] . num_sats ) {
state [ GPS_BLENDED_INSTANCE ] . num_sats = state [ i ] . num_sats ;
2017-01-29 19:02:57 -04:00
}
// report a blended average GPS antenna position
Vector3f temp_antenna_offset = _antenna_offset [ i ] ;
temp_antenna_offset * = _blend_weights [ i ] ;
_blended_antenna_offset + = temp_antenna_offset ;
// blend the timing data
2017-03-10 02:46:16 -04:00
if ( timing [ i ] . last_fix_time_ms > timing [ GPS_BLENDED_INSTANCE ] . last_fix_time_ms ) {
timing [ GPS_BLENDED_INSTANCE ] . last_fix_time_ms = timing [ i ] . last_fix_time_ms ;
2017-01-29 19:02:57 -04:00
}
2017-03-10 02:46:16 -04:00
if ( timing [ i ] . last_message_time_ms > timing [ GPS_BLENDED_INSTANCE ] . last_message_time_ms ) {
timing [ GPS_BLENDED_INSTANCE ] . last_message_time_ms = timing [ i ] . last_message_time_ms ;
2017-01-29 19:02:57 -04:00
}
}
/*
* Calculate an instantaneous weighted / blended average location from the available GPS instances and store in the _output_state .
2017-09-04 13:58:22 -03:00
* This will be statistically the most likely location , but will be not stable enough for direct use by the autopilot .
2017-01-29 19:02:57 -04:00
*/
// Use the GPS with the highest weighting as the reference position
float best_weight = 0.0f ;
uint8_t best_index = 0 ;
for ( uint8_t i = 0 ; i < GPS_MAX_RECEIVERS ; i + + ) {
if ( _blend_weights [ i ] > best_weight ) {
best_weight = _blend_weights [ i ] ;
best_index = i ;
2017-03-10 02:46:16 -04:00
state [ GPS_BLENDED_INSTANCE ] . location = state [ i ] . location ;
2017-01-29 19:02:57 -04:00
}
}
// Calculate the weighted sum of horizontal and vertical position offsets relative to the reference position
Vector2f blended_NE_offset_m ;
float blended_alt_offset_cm = 0.0f ;
blended_NE_offset_m . zero ( ) ;
for ( uint8_t i = 0 ; i < GPS_MAX_RECEIVERS ; i + + ) {
if ( _blend_weights [ i ] > 0.0f & & i ! = best_index ) {
2019-04-08 10:16:19 -03:00
blended_NE_offset_m + = state [ GPS_BLENDED_INSTANCE ] . location . get_distance_NE ( state [ i ] . location ) * _blend_weights [ i ] ;
2017-03-10 02:46:16 -04:00
blended_alt_offset_cm + = ( float ) ( state [ i ] . location . alt - state [ GPS_BLENDED_INSTANCE ] . location . alt ) * _blend_weights [ i ] ;
2017-01-29 19:02:57 -04:00
}
}
// Add the sum of weighted offsets to the reference location to obtain the blended location
2019-02-24 20:16:21 -04:00
state [ GPS_BLENDED_INSTANCE ] . location . offset ( blended_NE_offset_m . x , blended_NE_offset_m . y ) ;
2017-03-10 02:46:16 -04:00
state [ GPS_BLENDED_INSTANCE ] . location . alt + = ( int ) blended_alt_offset_cm ;
2017-01-29 19:02:57 -04:00
// Calculate ground speed and course from blended velocity vector
2017-03-10 02:46:16 -04:00
state [ GPS_BLENDED_INSTANCE ] . ground_speed = norm ( state [ GPS_BLENDED_INSTANCE ] . velocity . x , state [ GPS_BLENDED_INSTANCE ] . velocity . y ) ;
2017-03-12 23:21:47 -03:00
state [ GPS_BLENDED_INSTANCE ] . ground_course = wrap_360 ( degrees ( atan2f ( state [ GPS_BLENDED_INSTANCE ] . velocity . y , state [ GPS_BLENDED_INSTANCE ] . velocity . x ) ) ) ;
2017-01-29 19:02:57 -04:00
/*
* The blended location in _output_state . location is not stable enough to be used by the autopilot
* as it will move around as the relative accuracy changes . To mitigate this effect a low - pass filtered
* offset from each GPS location to the blended location is calculated and the filtered offset is applied
* to each receiver .
*/
// Calculate filter coefficients to be applied to the offsets for each GPS position and height offset
// A weighting of 1 will make the offset adjust the slowest, a weighting of 0 will make it adjust with zero filtering
float alpha [ GPS_MAX_RECEIVERS ] = { } ;
for ( uint8_t i = 0 ; i < GPS_MAX_RECEIVERS ; i + + ) {
if ( state [ i ] . last_gps_time_ms - _last_time_updated [ i ] > 0 ) {
float min_alpha = constrain_float ( _omega_lpf * 0.001f * ( float ) ( state [ i ] . last_gps_time_ms - _last_time_updated [ i ] ) , 0.0f , 1.0f ) ;
if ( _blend_weights [ i ] > min_alpha ) {
alpha [ i ] = min_alpha / _blend_weights [ i ] ;
} else {
alpha [ i ] = 1.0f ;
}
_last_time_updated [ i ] = state [ i ] . last_gps_time_ms ;
}
}
// Calculate the offset from each GPS solution to the blended solution
for ( uint8_t i = 0 ; i < GPS_MAX_RECEIVERS ; i + + ) {
2019-04-08 10:16:19 -03:00
_NE_pos_offset_m [ i ] = state [ i ] . location . get_distance_NE ( state [ GPS_BLENDED_INSTANCE ] . location ) * alpha [ i ] + _NE_pos_offset_m [ i ] * ( 1.0f - alpha [ i ] ) ;
2017-03-10 02:46:16 -04:00
_hgt_offset_cm [ i ] = ( float ) ( state [ GPS_BLENDED_INSTANCE ] . location . alt - state [ i ] . location . alt ) * alpha [ i ] + _hgt_offset_cm [ i ] * ( 1.0f - alpha [ i ] ) ;
2017-01-29 19:02:57 -04:00
}
// Calculate a corrected location for each GPS
Location corrected_location [ GPS_MAX_RECEIVERS ] ;
for ( uint8_t i = 0 ; i < GPS_MAX_RECEIVERS ; i + + ) {
corrected_location [ i ] = state [ i ] . location ;
2019-02-24 20:16:21 -04:00
corrected_location [ i ] . offset ( _NE_pos_offset_m [ i ] . x , _NE_pos_offset_m [ i ] . y ) ;
2017-01-29 19:02:57 -04:00
corrected_location [ i ] . alt + = ( int ) ( _hgt_offset_cm [ i ] ) ;
}
// If the GPS week is the same then use a blended time_week_ms
// If week is different, then use time stamp from GPS with largest weighting
// detect inconsistent week data
uint8_t last_week_instance = 0 ;
bool weeks_consistent = true ;
for ( uint8_t i = 0 ; i < GPS_MAX_RECEIVERS ; i + + ) {
if ( last_week_instance = = 0 & & _blend_weights [ i ] > 0 ) {
// this is our first valid sensor week data
last_week_instance = state [ i ] . time_week ;
} else if ( last_week_instance ! = 0 & & _blend_weights [ i ] > 0 & & last_week_instance ! = state [ i ] . time_week ) {
// there is valid sensor week data that is inconsistent
weeks_consistent = false ;
}
}
// calculate output
if ( ! weeks_consistent ) {
// use data from highest weighted sensor
2017-03-10 02:46:16 -04:00
state [ GPS_BLENDED_INSTANCE ] . time_week = state [ best_index ] . time_week ;
state [ GPS_BLENDED_INSTANCE ] . time_week_ms = state [ best_index ] . time_week_ms ;
2017-01-29 19:02:57 -04:00
} else {
// use week number from highest weighting GPS (they should all have the same week number)
2017-03-10 02:46:16 -04:00
state [ GPS_BLENDED_INSTANCE ] . time_week = state [ best_index ] . time_week ;
2017-01-29 19:02:57 -04:00
// calculate a blended value for the number of ms lapsed in the week
double temp_time_0 = 0.0 ;
for ( uint8_t i = 0 ; i < GPS_MAX_RECEIVERS ; i + + ) {
if ( _blend_weights [ i ] > 0.0f ) {
2017-03-17 07:27:23 -03:00
temp_time_0 + = ( double ) state [ i ] . time_week_ms * ( double ) _blend_weights [ i ] ;
2017-01-29 19:02:57 -04:00
}
}
2017-03-10 02:46:16 -04:00
state [ GPS_BLENDED_INSTANCE ] . time_week_ms = ( uint32_t ) temp_time_0 ;
2017-01-29 19:02:57 -04:00
}
// calculate a blended value for the timing data and lag
double temp_time_1 = 0.0 ;
double temp_time_2 = 0.0 ;
for ( uint8_t i = 0 ; i < GPS_MAX_RECEIVERS ; i + + ) {
if ( _blend_weights [ i ] > 0.0f ) {
2017-03-17 07:27:23 -03:00
temp_time_1 + = ( double ) timing [ i ] . last_fix_time_ms * ( double ) _blend_weights [ i ] ;
temp_time_2 + = ( double ) timing [ i ] . last_message_time_ms * ( double ) _blend_weights [ i ] ;
2017-05-21 22:07:26 -03:00
float gps_lag_sec = 0 ;
get_lag ( i , gps_lag_sec ) ;
_blended_lag_sec + = gps_lag_sec * _blend_weights [ i ] ;
2017-01-29 19:02:57 -04:00
}
}
2017-03-10 02:46:16 -04:00
timing [ GPS_BLENDED_INSTANCE ] . last_fix_time_ms = ( uint32_t ) temp_time_1 ;
timing [ GPS_BLENDED_INSTANCE ] . last_message_time_ms = ( uint32_t ) temp_time_2 ;
2017-01-29 19:02:57 -04:00
}
2019-05-26 22:34:13 -03:00
# endif // GPS_BLENDED_INSTANCE
2017-09-21 22:22:22 -03:00
2019-03-05 19:24:02 -04:00
bool AP_GPS : : is_healthy ( uint8_t instance ) const
{
if ( instance > = GPS_MAX_INSTANCES ) {
return false ;
}
2019-04-04 07:47:33 -03:00
const uint16_t gps_max_delta_ms = 245 ; // 200 ms (5Hz) + 45 ms buffer
bool last_msg_valid = last_message_delta_time_ms ( instance ) < gps_max_delta_ms ;
2019-03-05 19:24:02 -04:00
2019-05-26 22:34:13 -03:00
# if defined(GPS_BLENDED_INSTANCE)
2019-03-05 19:24:02 -04:00
if ( instance = = GPS_BLENDED_INSTANCE ) {
return last_msg_valid & & blend_health_check ( ) ;
}
2019-05-26 22:34:13 -03:00
# endif
2019-03-05 19:24:02 -04:00
return last_msg_valid & &
drivers [ instance ] ! = nullptr & &
2017-09-21 22:22:22 -03:00
drivers [ instance ] - > is_healthy ( ) ;
}
2017-10-19 22:15:11 -03:00
bool AP_GPS : : prepare_for_arming ( void ) {
bool all_passed = true ;
for ( uint8_t i = 0 ; i < GPS_MAX_RECEIVERS ; i + + ) {
if ( drivers [ i ] ! = nullptr ) {
all_passed & = drivers [ i ] - > prepare_for_arming ( ) ;
}
}
return all_passed ;
}
2017-10-25 00:58:57 -03:00
2019-11-12 02:16:26 -04:00
bool AP_GPS : : logging_failed ( void ) const {
if ( ! logging_enabled ( ) ) {
return false ;
}
for ( uint8_t i = 0 ; i < GPS_MAX_RECEIVERS ; i + + ) {
if ( ( drivers [ i ] ! = nullptr ) & & ! ( drivers [ i ] - > logging_healthy ( ) ) ) {
return true ;
}
}
return false ;
}
2017-10-25 00:58:57 -03:00
namespace AP {
AP_GPS & gps ( )
{
2019-04-05 03:21:03 -03:00
return * AP_GPS : : get_singleton ( ) ;
2017-10-25 00:58:57 -03:00
}
} ;