You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
489 lines
16 KiB
489 lines
16 KiB
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- |
|
/* |
|
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/>. |
|
*/ |
|
|
|
#include <AP_Common.h> |
|
#include <AP_Math.h> |
|
#include <AP_HAL.h> |
|
#include <AP_Notify.h> |
|
#include <AP_GPS.h> |
|
|
|
extern const AP_HAL::HAL& hal; |
|
|
|
// table of user settable parameters |
|
const AP_Param::GroupInfo AP_GPS::var_info[] PROGMEM = { |
|
// @Param: TYPE |
|
// @DisplayName: GPS type |
|
// @Description: GPS type |
|
// @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL,8:SwiftNav |
|
AP_GROUPINFO("TYPE", 0, AP_GPS, _type[0], 1), |
|
|
|
#if GPS_MAX_INSTANCES > 1 |
|
|
|
// @Param: TYPE2 |
|
// @DisplayName: 2nd GPS type |
|
// @Description: GPS type of 2nd GPS |
|
// @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL,8:SwiftNav |
|
AP_GROUPINFO("TYPE2", 1, AP_GPS, _type[1], 0), |
|
|
|
#endif |
|
|
|
// @Param: NAVFILTER |
|
// @DisplayName: Navigation filter setting |
|
// @Description: Navigation filter engine setting |
|
// @Values: 0:Portable,2:Stationary,3:Pedestrian,4:Automotive,5:Sea,6:Airborne1G,7:Airborne2G,8:Airborne4G |
|
AP_GROUPINFO("NAVFILTER", 2, AP_GPS, _navfilter, GPS_ENGINE_AIRBORNE_4G), |
|
|
|
#if GPS_MAX_INSTANCES > 1 |
|
// @Param: AUTO_SWITCH |
|
// @DisplayName: Automatic Switchover Setting |
|
// @Description: Automatic switchover to GPS reporting best lock |
|
// @Values: 0:Disabled,1:Enabled |
|
// @User: Advanced |
|
AP_GROUPINFO("AUTO_SWITCH", 3, AP_GPS, _auto_switch, 1), |
|
#endif |
|
|
|
#if GPS_RTK_AVAILABLE |
|
// @Param: DGPS_MIN_LOCK |
|
// @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 |
|
AP_GROUPINFO("MIN_DGPS", 4, AP_GPS, _min_dgps, 100), |
|
#endif |
|
|
|
AP_GROUPEND |
|
}; |
|
|
|
/// Startup initialisation. |
|
void AP_GPS::init(DataFlash_Class *dataflash) |
|
{ |
|
_DataFlash = dataflash; |
|
hal.uartB->begin(38400UL, 256, 16); |
|
#if GPS_MAX_INSTANCES > 1 |
|
primary_instance = 0; |
|
if (hal.uartE != NULL) { |
|
hal.uartE->begin(38400UL, 256, 16); |
|
} |
|
#endif |
|
} |
|
|
|
// baudrates to try to detect GPSes with |
|
const uint32_t AP_GPS::_baudrates[] PROGMEM = {4800U, 38400U, 115200U, 57600U, 9600U}; |
|
|
|
// initialisation blobs to send to the GPS to try to get it into the |
|
// right mode |
|
const prog_char AP_GPS::_initialisation_blob[] PROGMEM = UBLOX_SET_BINARY MTK_SET_BINARY SIRF_SET_BINARY; |
|
|
|
/* |
|
send some more initialisation string bytes if there is room in the |
|
UART transmit buffer |
|
*/ |
|
void AP_GPS::send_blob_start(uint8_t instance, const prog_char *_blob, uint16_t size) |
|
{ |
|
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) |
|
{ |
|
// see if we can write some more of the initialisation blob |
|
if (initblob_state[instance].remaining > 0) { |
|
AP_HAL::UARTDriver *port = instance==0?hal.uartB:hal.uartE; |
|
int16_t space = port->txspace(); |
|
if (space > (int16_t)initblob_state[instance].remaining) { |
|
space = initblob_state[instance].remaining; |
|
} |
|
while (space > 0) { |
|
port->write(pgm_read_byte(initblob_state[instance].blob)); |
|
initblob_state[instance].blob++; |
|
space--; |
|
initblob_state[instance].remaining--; |
|
} |
|
} |
|
} |
|
|
|
/* |
|
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. |
|
*/ |
|
void |
|
AP_GPS::detect_instance(uint8_t instance) |
|
{ |
|
AP_GPS_Backend *new_gps = NULL; |
|
AP_HAL::UARTDriver *port = instance==0?hal.uartB:hal.uartE; |
|
struct detect_state *dstate = &detect_state[instance]; |
|
|
|
if (port == NULL) { |
|
// UART not available |
|
return; |
|
} |
|
|
|
uint32_t now = hal.scheduler->millis(); |
|
|
|
state[instance].instance = instance; |
|
state[instance].status = NO_GPS; |
|
|
|
// record the time when we started detection. This is used to try |
|
// to avoid initialising a uBlox as a NMEA GPS |
|
if (dstate->detect_started_ms == 0) { |
|
dstate->detect_started_ms = now; |
|
} |
|
|
|
if (now - dstate->last_baud_change_ms > 1200) { |
|
// try the next baud rate |
|
dstate->last_baud++; |
|
if (dstate->last_baud == sizeof(_baudrates) / sizeof(_baudrates[0])) { |
|
dstate->last_baud = 0; |
|
} |
|
uint32_t baudrate = pgm_read_dword(&_baudrates[dstate->last_baud]); |
|
port->begin(baudrate, 256, 16); |
|
dstate->last_baud_change_ms = now; |
|
send_blob_start(instance, _initialisation_blob, sizeof(_initialisation_blob)); |
|
} |
|
|
|
send_blob_update(instance); |
|
|
|
while (port->available() > 0 && new_gps == NULL) { |
|
uint8_t data = port->read(); |
|
/* |
|
running a uBlox at less than 38400 will lead to packet |
|
corruption, as we can't receive the packets in the 200ms |
|
window for 5Hz fixes. The NMEA startup message should force |
|
the uBlox into 38400 no matter what rate it is configured |
|
for. |
|
*/ |
|
if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_UBLOX) && |
|
pgm_read_dword(&_baudrates[dstate->last_baud]) >= 38400 && |
|
AP_GPS_UBLOX::_detect(dstate->ublox_detect_state, data)) { |
|
hal.console->print_P(PSTR(" ublox ")); |
|
new_gps = new AP_GPS_UBLOX(*this, state[instance], port); |
|
} |
|
else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_MTK19) && |
|
AP_GPS_MTK19::_detect(dstate->mtk19_detect_state, data)) { |
|
hal.console->print_P(PSTR(" MTK19 ")); |
|
new_gps = new AP_GPS_MTK19(*this, state[instance], port); |
|
} |
|
else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_MTK) && |
|
AP_GPS_MTK::_detect(dstate->mtk_detect_state, data)) { |
|
hal.console->print_P(PSTR(" MTK ")); |
|
new_gps = new AP_GPS_MTK(*this, state[instance], port); |
|
} |
|
#if GPS_RTK_AVAILABLE |
|
else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SBP) && |
|
AP_GPS_SBP::_detect(dstate->sbp_detect_state, data)) { |
|
hal.console->print_P(PSTR(" SBP ")); |
|
new_gps = new AP_GPS_SBP(*this, state[instance], port); |
|
} |
|
#endif // HAL_CPU_CLASS |
|
#if !defined(GPS_SKIP_SIRF_NMEA) |
|
// save a bit of code space on a 1280 |
|
else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SIRF) && |
|
AP_GPS_SIRF::_detect(dstate->sirf_detect_state, data)) { |
|
hal.console->print_P(PSTR(" SIRF ")); |
|
new_gps = new AP_GPS_SIRF(*this, state[instance], port); |
|
} |
|
else if (now - dstate->detect_started_ms > 5000) { |
|
// prevent false detection of NMEA mode in |
|
// a MTK or UBLOX which has booted in NMEA mode |
|
if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_NMEA) && |
|
AP_GPS_NMEA::_detect(dstate->nmea_detect_state, data)) { |
|
hal.console->print_P(PSTR(" NMEA ")); |
|
new_gps = new AP_GPS_NMEA(*this, state[instance], port); |
|
} |
|
} |
|
#endif |
|
} |
|
|
|
if (new_gps != NULL) { |
|
state[instance].status = NO_FIX; |
|
drivers[instance] = new_gps; |
|
timing[instance].last_message_time_ms = now; |
|
} |
|
} |
|
|
|
bool |
|
AP_GPS::can_calculate_base_pos(void) |
|
{ |
|
#if GPS_RTK_AVAILABLE |
|
for (uint8_t i=0; i<GPS_MAX_INSTANCES; i++) { |
|
if (drivers[i] != NULL && drivers[i]->can_calculate_base_pos()) { |
|
return true; |
|
} |
|
} |
|
#endif |
|
return false; |
|
} |
|
|
|
/* |
|
Tells the underlying GPS drivers to capture its current position as home. |
|
*/ |
|
void |
|
AP_GPS::calculate_base_pos(void) |
|
{ |
|
#if GPS_RTK_AVAILABLE |
|
for (uint8_t i = 0; i<GPS_MAX_INSTANCES; i++) { |
|
if (drivers[i] != NULL && drivers[i]->can_calculate_base_pos()) { |
|
drivers[i]->calculate_base_pos(); |
|
} |
|
} |
|
#endif |
|
} |
|
|
|
AP_GPS::GPS_Status |
|
AP_GPS::highest_supported_status(uint8_t instance) const |
|
{ |
|
#if GPS_RTK_AVAILABLE |
|
if (drivers[instance] != NULL) |
|
return drivers[instance]->highest_supported_status(); |
|
#endif |
|
return AP_GPS::GPS_OK_FIX_3D; |
|
} |
|
|
|
AP_GPS::GPS_Status |
|
AP_GPS::highest_supported_status(void) const |
|
{ |
|
#if GPS_RTK_AVAILABLE |
|
|
|
if (drivers[primary_instance] != NULL) |
|
return drivers[primary_instance]->highest_supported_status(); |
|
#endif |
|
return AP_GPS::GPS_OK_FIX_3D; |
|
} |
|
|
|
|
|
/* |
|
update one GPS instance. This should be called at 10Hz or greater |
|
*/ |
|
void |
|
AP_GPS::update_instance(uint8_t instance) |
|
{ |
|
if (_type[instance] == GPS_TYPE_HIL) { |
|
// in HIL, leave info alone |
|
return; |
|
} |
|
if (_type[instance] == GPS_TYPE_NONE) { |
|
// not enabled |
|
state[instance].status = NO_GPS; |
|
return; |
|
} |
|
if (locked_ports & (1U<<instance)) { |
|
// the port is locked by another driver |
|
return; |
|
} |
|
|
|
if (drivers[instance] == NULL || state[instance].status == NO_GPS) { |
|
// 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; |
|
} |
|
|
|
send_blob_update(instance); |
|
|
|
// we have an active driver for this instance |
|
bool result = drivers[instance]->read(); |
|
uint32_t tnow = hal.scheduler->millis(); |
|
|
|
// if we did not get a message, and the idle timer of 1.2 seconds |
|
// has expired, re-initialise the GPS. This will cause GPS |
|
// detection to run again |
|
if (!result) { |
|
if (tnow - timing[instance].last_message_time_ms > 1200) { |
|
// 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] = NULL; |
|
memset(&state[instance], 0, sizeof(state[instance])); |
|
state[instance].instance = instance; |
|
state[instance].status = NO_GPS; |
|
timing[instance].last_message_time_ms = tnow; |
|
} |
|
} else { |
|
timing[instance].last_message_time_ms = tnow; |
|
if (state[instance].status >= GPS_OK_FIX_2D) { |
|
timing[instance].last_fix_time_ms = tnow; |
|
} |
|
} |
|
} |
|
|
|
/* |
|
update all GPS instances |
|
*/ |
|
void |
|
AP_GPS::update(void) |
|
{ |
|
for (uint8_t i=0; i<GPS_MAX_INSTANCES; i++) { |
|
update_instance(i); |
|
} |
|
|
|
// update notify with gps status. We always base this on the first GPS |
|
AP_Notify::flags.gps_status = state[0].status; |
|
|
|
#if GPS_MAX_INSTANCES > 1 |
|
// work out which GPS is the primary, and how many sensors we have |
|
for (uint8_t i=0; i<GPS_MAX_INSTANCES; i++) { |
|
if (state[i].status != NO_GPS) { |
|
num_instances = i+1; |
|
} |
|
if (_auto_switch) { |
|
if (i == primary_instance) { |
|
continue; |
|
} |
|
if (state[i].status > state[primary_instance].status) { |
|
// we have a higher status lock, change GPS |
|
primary_instance = i; |
|
continue; |
|
} |
|
if (state[i].status == state[primary_instance].status && |
|
state[i].num_sats >= state[primary_instance].num_sats + 2) { |
|
// this GPS has at least 2 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; |
|
} |
|
} else { |
|
primary_instance = 0; |
|
} |
|
} |
|
#else |
|
num_instances = 1; |
|
#endif // GPS_MAX_INSTANCES |
|
} |
|
|
|
/* |
|
set HIL (hardware in the loop) status for a GPS instance |
|
*/ |
|
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, bool _have_vertical_velocity) |
|
{ |
|
if (instance >= GPS_MAX_INSTANCES) { |
|
return; |
|
} |
|
uint32_t tnow = hal.scheduler->millis(); |
|
GPS_State &istate = state[instance]; |
|
istate.status = _status; |
|
istate.location = _location; |
|
istate.location.options = 0; |
|
istate.velocity = _velocity; |
|
istate.ground_speed = pythagorous2(istate.velocity.x, istate.velocity.y); |
|
istate.ground_course_cd = degrees(atan2f(istate.velocity.y, istate.velocity.x)) * 100UL; |
|
istate.hdop = hdop; |
|
istate.num_sats = _num_sats; |
|
istate.have_vertical_velocity = _have_vertical_velocity; |
|
istate.last_gps_time_ms = tnow; |
|
istate.time_week = time_epoch_ms / (86400*7*(uint64_t)1000); |
|
istate.time_week_ms = time_epoch_ms - istate.time_week*(86400*7*(uint64_t)1000); |
|
timing[instance].last_message_time_ms = tnow; |
|
timing[instance].last_fix_time_ms = tnow; |
|
_type[instance].set(GPS_TYPE_HIL); |
|
} |
|
|
|
/** |
|
Lock a GPS port, prevening the GPS driver from using it. This can |
|
be used to allow a user to control a GPS port via the |
|
SERIAL_CONTROL protocol |
|
*/ |
|
void |
|
AP_GPS::lock_port(uint8_t instance, bool lock) |
|
{ |
|
if (instance >= GPS_MAX_INSTANCES) { |
|
return; |
|
} |
|
if (lock) { |
|
locked_ports |= (1U<<instance); |
|
} else { |
|
locked_ports &= ~(1U<<instance); |
|
} |
|
} |
|
|
|
void |
|
AP_GPS::send_mavlink_gps_raw(mavlink_channel_t chan) |
|
{ |
|
static uint32_t last_send_time_ms; |
|
if (last_send_time_ms == 0 || last_send_time_ms != last_message_time_ms(0)) { |
|
last_send_time_ms = last_message_time_ms(0); |
|
const Location &loc = location(0); |
|
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), |
|
65535, |
|
ground_speed(0)*100, // cm/s |
|
ground_course_cd(0), // 1/100 degrees, |
|
num_sats(0)); |
|
} |
|
} |
|
|
|
#if GPS_MAX_INSTANCES > 1 |
|
void |
|
AP_GPS::send_mavlink_gps2_raw(mavlink_channel_t chan) |
|
{ |
|
static uint32_t last_send_time_ms2; |
|
if (num_sensors() > 1 && |
|
status(1) > AP_GPS::NO_GPS && |
|
(last_send_time_ms2 == 0 || last_send_time_ms2 != last_message_time_ms(1))) { |
|
const Location &loc = location(1); |
|
last_send_time_ms2 = last_message_time_ms(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), |
|
65535, |
|
ground_speed(1)*100, // cm/s |
|
ground_course_cd(1), // 1/100 degrees, |
|
num_sats(1), |
|
0, |
|
0); |
|
} |
|
} |
|
#endif |
|
|
|
#if GPS_RTK_AVAILABLE |
|
void |
|
AP_GPS::send_mavlink_gps_rtk(mavlink_channel_t chan) |
|
{ |
|
if (drivers[0] != NULL && drivers[0]->highest_supported_status() > AP_GPS::GPS_OK_FIX_3D) { |
|
drivers[0]->send_mavlink_gps_rtk(chan); |
|
} |
|
} |
|
|
|
#if GPS_MAX_INSTANCES > 1 |
|
void |
|
AP_GPS::send_mavlink_gps2_rtk(mavlink_channel_t chan) |
|
{ |
|
if (drivers[1] != NULL && drivers[1]->highest_supported_status() > AP_GPS::GPS_OK_FIX_3D) { |
|
drivers[1]->send_mavlink_gps_rtk(chan); |
|
} |
|
} |
|
#endif |
|
#endif
|
|
|