2015-09-02 06:13:38 -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/>.
*/
//
// Septentrio GPS driver for ArduPilot.
// Code by Michael Oborne
//
2016-02-17 21:25:23 -04:00
# pragma once
2015-09-02 06:13:38 -03:00
# include "AP_GPS.h"
2016-04-13 14:20:05 -03:00
# include "GPS_Backend.h"
2015-09-02 06:13:38 -03:00
2022-01-09 19:15:32 -04:00
# if AP_GPS_SBF_ENABLED
2017-04-30 04:31:26 -03:00
# define SBF_DISK_ACTIVITY (1 << 7)
# define SBF_DISK_FULL (1 << 8)
# define SBF_DISK_MOUNTED (1 << 9)
2015-09-02 06:13:38 -03:00
class AP_GPS_SBF : public AP_GPS_Backend
{
public :
AP_GPS_SBF ( AP_GPS & _gps , AP_GPS : : GPS_State & _state , AP_HAL : : UARTDriver * _port ) ;
2020-06-13 17:23:44 -03:00
~ AP_GPS_SBF ( ) ;
2015-09-02 06:13:38 -03:00
2018-11-07 06:58:08 -04:00
AP_GPS : : GPS_Status highest_supported_status ( void ) override { return AP_GPS : : GPS_OK_FIX_3D_RTK_FIXED ; }
2015-09-02 06:13:38 -03:00
// Methods
2018-11-07 06:58:08 -04:00
bool read ( ) override ;
2015-09-02 06:13:38 -03:00
2016-08-01 08:58:23 -03:00
const char * name ( ) const override { return " SBF " ; }
2021-02-23 16:18:17 -04:00
bool is_configured ( void ) const override ;
2017-04-30 04:31:26 -03:00
void broadcast_configuration_failure_reason ( void ) const override ;
2020-08-13 19:09:12 -03:00
bool supports_mavlink_gps_rtk_message ( void ) const override { return true ; } ;
2017-05-24 14:42:03 -03:00
// get the velocity lag, returns true if the driver is confident in the returned value
bool get_lag ( float & lag_sec ) const override { lag_sec = 0.08f ; return true ; } ;
2017-05-24 23:06:35 -03:00
2017-09-21 22:22:22 -03:00
bool is_healthy ( void ) const override ;
2019-11-12 02:16:26 -04:00
bool logging_healthy ( void ) const override ;
2017-10-19 22:15:11 -03:00
bool prepare_for_arming ( void ) override ;
2021-02-23 16:18:17 -04:00
bool get_error_codes ( uint32_t & error_codes ) const override { error_codes = RxError ; return true ; } ;
2017-09-21 22:22:22 -03:00
2015-09-02 06:13:38 -03:00
private :
bool parse ( uint8_t temp ) ;
bool process_message ( ) ;
static const uint8_t SBF_PREAMBLE1 = ' $ ' ;
static const uint8_t SBF_PREAMBLE2 = ' @ ' ;
2015-12-07 08:45:06 -04:00
2020-06-13 17:23:44 -03:00
uint8_t _init_blob_index ;
uint32_t _init_blob_time ;
2021-02-11 19:14:37 -04:00
enum class Config_State {
Baud_Rate ,
SSO ,
Blob ,
2021-04-05 18:30:30 -03:00
SBAS ,
2021-02-11 19:14:37 -04:00
Complete
} ;
Config_State config_step ;
char * config_string ;
2021-01-22 15:35:19 -04:00
static constexpr const char * _initialisation_blob [ ] = {
2021-02-11 19:14:37 -04:00
" srd,Moderate,UAV " ,
" sem,PVT,5 " ,
" spm,Rover,all " ,
" sso,Stream2,Dsk1,postprocess+event+comment+ReceiverStatus,msec100 " ,
2021-01-22 15:35:19 -04:00
# if defined (GPS_SBF_EXTRA_CONFIG)
GPS_SBF_EXTRA_CONFIG
# endif
} ;
2021-04-05 18:30:30 -03:00
static constexpr const char * sbas_off = " sst, -SBAS " ;
static constexpr const char * sbas_on_blob [ ] = {
" snt,+GEOL1+GEOL5 " ,
" sst,+SBAS " ,
" ssbc,auto,Operational,MixedSystems,auto " ,
} ;
2017-09-21 23:49:41 -03:00
uint32_t _config_last_ack_time ;
const char * _port_enable = " \n SSSSSSSSSS \n " ;
2015-09-02 06:13:38 -03:00
uint32_t crc_error_counter = 0 ;
2017-04-30 04:31:26 -03:00
uint32_t RxState ;
2017-09-21 22:22:22 -03:00
uint32_t RxError ;
2015-09-02 06:13:38 -03:00
2017-10-19 22:15:11 -03:00
void mount_disk ( void ) const ;
void unmount_disk ( void ) const ;
bool _has_been_armed ;
2017-05-24 18:39:47 -03:00
enum sbf_ids {
DOP = 4001 ,
PVTGeodetic = 4007 ,
ReceiverStatus = 4014 ,
2020-08-13 19:09:12 -03:00
BaseVectorGeod = 4028 ,
2017-05-24 18:39:47 -03:00
VelCovGeodetic = 5908
} ;
struct PACKED msg4007 // PVTGeodetic
2015-09-02 06:13:38 -03:00
{
uint32_t TOW ;
uint16_t WNc ;
uint8_t Mode ;
uint8_t Error ;
double Latitude ;
double Longitude ;
double Height ;
float Undulation ;
float Vn ;
float Ve ;
float Vu ;
float COG ;
double RxClkBias ;
float RxClkDrift ;
uint8_t TimeSystem ;
uint8_t Datum ;
uint8_t NrSV ;
uint8_t WACorrInfo ;
uint16_t ReferenceID ;
uint16_t MeanCorrAge ;
uint32_t SignalInfo ;
uint8_t AlertFlag ;
2015-12-07 08:47:51 -04:00
// rev1
uint8_t NrBases ;
uint16_t PPPInfo ;
// rev2
uint16_t Latency ;
uint16_t HAccuracy ;
uint16_t VAccuracy ;
uint8_t Misc ;
2015-09-02 06:13:38 -03:00
} ;
2020-08-13 19:09:12 -03:00
2017-05-24 18:39:47 -03:00
struct PACKED msg4001 // DOP
2015-09-02 06:13:38 -03:00
{
uint32_t TOW ;
uint16_t WNc ;
uint8_t NrSV ;
uint8_t Reserved ;
uint16_t PDOP ;
uint16_t TDOP ;
uint16_t HDOP ;
uint16_t VDOP ;
float HPL ;
float VPL ;
2015-12-07 08:47:51 -04:00
} ;
2017-04-30 04:31:26 -03:00
struct PACKED msg4014 // ReceiverStatus (v2)
{
uint32_t TOW ;
uint16_t WNc ;
uint8_t CPULoad ;
uint8_t ExtError ;
uint32_t UpTime ;
uint32_t RxState ;
uint32_t RxError ;
// remaining data is AGCData, which we don't have a use for, don't extract the data
} ;
2020-08-13 19:09:12 -03:00
struct PACKED VectorInfoGeod {
uint8_t NrSV ;
uint8_t Error ;
uint8_t Mode ;
uint8_t Misc ;
double DeltaEast ;
double DeltaNorth ;
double DeltaUp ;
float DeltaVe ;
float DeltaVn ;
float DeltaVu ;
uint16_t Azimuth ;
int16_t Elevation ;
uint8_t ReferenceID ;
uint16_t CorrAge ;
uint32_t SignalInfo ;
} ;
struct PACKED msg4028 // BaseVectorGeod
{
uint32_t TOW ;
uint16_t WNc ;
uint8_t N ; // number of baselines
uint8_t SBLength ;
VectorInfoGeod info ; // there can be multiple baselines here, but we will only consume the first one, so don't worry about anything after
} ;
2017-05-24 18:39:47 -03:00
struct PACKED msg5908 // VelCovGeodetic
{
uint32_t TOW ;
uint16_t WNc ;
uint8_t Mode ;
uint8_t Error ;
float Cov_VnVn ;
float Cov_VeVe ;
float Cov_VuVu ;
float Cov_DtDt ;
float Cov_VnVe ;
float Cov_VnVu ;
float Cov_VnDt ;
float Cov_VeVu ;
float Cov_VeDt ;
float Cov_VuDt ;
} ;
2015-12-07 08:47:51 -04:00
union PACKED msgbuffer {
2015-09-02 06:13:38 -03:00
msg4007 msg4007u ;
2015-12-07 08:47:51 -04:00
msg4001 msg4001u ;
2017-04-30 04:31:26 -03:00
msg4014 msg4014u ;
2020-08-13 19:09:12 -03:00
msg4028 msg4028u ;
2017-05-24 18:39:47 -03:00
msg5908 msg5908u ;
2017-09-04 21:09:03 -03:00
uint8_t bytes [ 256 ] ;
2015-09-02 06:13:38 -03:00
} ;
2015-12-07 08:47:51 -04:00
2015-09-02 06:13:38 -03:00
struct sbf_msg_parser_t
{
enum
{
PREAMBLE1 = 0 ,
PREAMBLE2 ,
CRC1 ,
CRC2 ,
BLOCKID1 ,
BLOCKID2 ,
LENGTH1 ,
LENGTH2 ,
2017-09-21 20:20:40 -03:00
DATA ,
COMMAND_LINE // used to parse command responses
2015-09-02 06:13:38 -03:00
} sbf_state ;
uint16_t preamble ;
uint16_t crc ;
uint16_t blockid ;
uint16_t length ;
msgbuffer data ;
uint16_t read ;
} sbf_msg ;
2015-12-07 08:47:51 -04:00
2017-09-21 22:22:22 -03:00
enum {
2019-04-05 18:14:57 -03:00
SOFTWARE = ( 1 < < 3 ) , // set upon detection of a software warning or error. This bit is reset by the command lif, error
2018-04-25 18:55:05 -03:00
WATCHDOG = ( 1 < < 4 ) , // set when the watch-dog expired at least once since the last power-on.
CONGESTION = ( 1 < < 6 ) , // set when an output data congestion has been detected on at least one of the communication ports of the receiver during the last second.
MISSEDEVENT = ( 1 < < 8 ) , // set when an external event congestion has been detected during the last second. It indicates that the receiver is receiving too many events on its EVENTx pins.
CPUOVERLOAD = ( 1 < < 9 ) , // set when the CPU load is larger than 90%. If this bit is set, receiver operation may be unreliable and the user must decrease the processing load by following the recommendations in the User Manual.
INVALIDCONFIG = ( 1 < < 10 ) , // set if one or more configuration file (permission or channel configuration) is invalid or absent.
OUTOFGEOFENCE = ( 1 < < 11 ) , // set if the receiver is currently out of its permitted region of operation (geo-fencing).
2018-02-28 01:16:54 -04:00
} ;
2021-01-22 15:35:19 -04:00
static constexpr const char * portIdentifiers [ ] = { " COM " , " USB " , " IP1 " , " NTR " , " IPS " , " IPR " } ;
char portIdentifier [ 5 ] ;
uint8_t portLength ;
bool readyForCommand ;
2015-09-02 06:13:38 -03:00
} ;
2022-01-09 19:15:32 -04:00
# endif