Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: Factor out moving base offset helper

Browse Source
This commit is contained in:
Michael du Breuil 2020-09-29 12:47:37 -07:00 committed by Andrew Tridgell
parent 230798546e
commit 4161e2eb28
8 changed files with 199 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_GPS/AP_GPS.cpp
View File

@ -293,7 +293,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
AP_GROUPINFO("BLEND_TC", 21, AP_GPS, _blend_tc, 10.0f),
#endif
#if GPS_UBLOX_MOVING_BASELINE
#if GPS_MOVING_BASELINE
// @Param: DRV_OPTIONS
// @DisplayName: driver options
// @Description: Additional backend specific options
@ -322,6 +322,20 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
AP_GROUPINFO("COM_PORT2", 24, AP_GPS, _com_port[1], 1),
#endif
#if GPS_MOVING_BASELINE
// @Group: MB1_
// @Path: MovingBase.cpp
AP_SUBGROUPINFO(mb_params[0], "MB1_", 25, AP_GPS, MovingBase),
#if GPS_MAX_RECEIVERS > 1
// @Group: MB2_
// @Path: MovingBase.cpp
AP_SUBGROUPINFO(mb_params[1], "MB2_", 26, AP_GPS, MovingBase),
#endif // GPS_MAX_RECEIVERS > 1
#endif // GPS_MOVING_BASELINE
AP_GROUPEND
};

12
libraries/AP_GPS/AP_GPS.h
View File

@ -43,14 +43,18 @@
#define UNIX_OFFSET_MSEC (17000ULL * 86400ULL + 52ULL * 10ULL * AP_MSEC_PER_WEEK - GPS_LEAPSECONDS_MILLIS)
#ifndef GPS_UBLOX_MOVING_BASELINE
#define GPS_UBLOX_MOVING_BASELINE !HAL_MINIMIZE_FEATURES && GPS_MAX_RECEIVERS>1
#ifndef GPS_MOVING_BASELINE
#define GPS_MOVING_BASELINE !HAL_MINIMIZE_FEATURES && GPS_MAX_RECEIVERS>1
#endif
#ifndef HAL_MSP_GPS_ENABLED
#define HAL_MSP_GPS_ENABLED HAL_MSP_SENSORS_ENABLED
#endif
#if GPS_MOVING_BASELINE
#include "MovingBase.h"
#endif // GPS_MOVING_BASELINE
class AP_GPS_Backend;
/// @class AP_GPS
@ -519,6 +523,10 @@ protected:
AP_Float _blend_tc;
AP_Int16 _driver_options;
#if GPS_MOVING_BASELINE
MovingBase mb_params[GPS_MAX_RECEIVERS];
#endif // GPS_MOVING_BASELINE
uint32_t _log_gps_bit = -1;
private:

74
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -79,7 +79,7 @@ AP_GPS_UBLOX::AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UART
_unconfigured_messages |= CONFIG_TP5;
#endif
#if GPS_UBLOX_MOVING_BASELINE
#if GPS_MOVING_BASELINE
if (role == AP_GPS::GPS_ROLE_MB_BASE && !mb_use_uart2()) {
rtcm3_parser = new RTCM3_Parser;
if (rtcm3_parser == nullptr) {
@ -96,14 +96,14 @@ AP_GPS_UBLOX::AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UART
AP_GPS_UBLOX::~AP_GPS_UBLOX()
{
#if GPS_UBLOX_MOVING_BASELINE
#if GPS_MOVING_BASELINE
if (rtcm3_parser) {
delete rtcm3_parser;
}
#endif
}
#if GPS_UBLOX_MOVING_BASELINE
#if GPS_MOVING_BASELINE
/*
config for F9 GPS in moving baseline base role
See ZED-F9P integration manual section 3.1.5.6.1
@ -205,7 +205,7 @@ const AP_GPS_UBLOX::config_list AP_GPS_UBLOX::config_MB_Rover_uart2[] {
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1127_UART1, 0},
{ ConfigKey::MSGOUT_RTCM_3X_TYPE1230_UART1, 0},
};
#endif // GPS_UBLOX_MOVING_BASELINE
#endif // GPS_MOVING_BASELINE
void
@ -357,7 +357,7 @@ AP_GPS_UBLOX::_request_next_config(void)
}
break;
case STEP_RTK_MOVBASE:
#if GPS_UBLOX_MOVING_BASELINE
#if GPS_MOVING_BASELINE
if (supports_F9_config()) {
static_assert(sizeof(active_config.done_mask)*8 >= ARRAY_SIZE(config_MB_Base_uart1), "done_mask too small, base1");
static_assert(sizeof(active_config.done_mask)*8 >= ARRAY_SIZE(config_MB_Base_uart2), "done_mask too small, base2");
@ -572,7 +572,7 @@ AP_GPS_UBLOX::read(void)
// read the next byte
data = port->read();
#if GPS_UBLOX_MOVING_BASELINE
#if GPS_MOVING_BASELINE
if (rtcm3_parser) {
if (rtcm3_parser->read(data)) {
// we've found a RTCMv3 packet. We stop parsing at
@ -682,7 +682,7 @@ AP_GPS_UBLOX::read(void)
break; // bad checksum
}
#if GPS_UBLOX_MOVING_BASELINE
#if GPS_MOVING_BASELINE
if (rtcm3_parser) {
// this is a uBlox packet, discard any partial RTCMv3 state
rtcm3_parser->reset();
@ -856,7 +856,7 @@ uint8_t AP_GPS_UBLOX::config_key_size(ConfigKey key) const
*/
int8_t AP_GPS_UBLOX::find_active_config_index(ConfigKey key) const
{
#if GPS_UBLOX_MOVING_BASELINE
#if GPS_MOVING_BASELINE
if (active_config.list == nullptr) {
return -1;
}
@ -1122,7 +1122,7 @@ AP_GPS_UBLOX::_parse_gps(void)
default:
break;
}
#if GPS_UBLOX_MOVING_BASELINE
#if GPS_MOVING_BASELINE
// see if it is in active config list
int8_t cfg_idx = find_active_config_index(id);
if (cfg_idx >= 0) {
@ -1141,7 +1141,7 @@ AP_GPS_UBLOX::_parse_gps(void)
}
}
}
#endif // GPS_UBLOX_MOVING_BASELINE
#endif // GPS_MOVING_BASELINE
// step over the value
uint8_t step_size = config_key_size(id);
@ -1326,10 +1326,10 @@ AP_GPS_UBLOX::_parse_gps(void)
state.hdop = 130;
#endif
break;
#if GPS_MOVING_BASELINE
case MSG_RELPOSNED:
{
const Vector3f &offset0 = gps._antenna_offset[state.instance^1].get();
const Vector3f &offset1 = gps._antenna_offset[state.instance].get();
// note that we require the yaw to come from a fixed solution, not a float solution
// yaw from a float solution would only be acceptable with a very large separation between
// GPS modules
@ -1342,21 +1342,6 @@ AP_GPS_UBLOX::_parse_gps(void)
static_cast<uint32_t>(RELPOSNED::refObsMiss) |
static_cast<uint32_t>(RELPOSNED::carrSolnFloat);
const Vector3f antenna_offset = offset0 - offset1;
const float offset_dist = antenna_offset.length();
const float rel_dist = _buffer.relposned.relPosLength * 0.01;
const float dist_error = offset_dist - rel_dist;
const float strict_length_error_allowed = 0.2; // allow for up to 20% error
const float min_separation = 0.05;
bool tilt_ok = true;
const float min_dist = MIN(offset_dist, rel_dist);
#ifndef HAL_BUILD_AP_PERIPH
// when ahrs is available use it to constrain vertical component
const Vector3f antenna_tilt = AP::ahrs().get_rotation_body_to_ned() * antenna_offset;
const float alt_error = _buffer.relposned.relPosD*0.01 + antenna_tilt.z;
tilt_ok = fabsf(alt_error) < strict_length_error_allowed * min_dist;
#endif
_check_new_itow(_buffer.relposned.iTOW);
if (_buffer.relposned.iTOW != _last_relposned_itow+200) {
// useful for looking at packet loss on links
@ -1365,35 +1350,20 @@ AP_GPS_UBLOX::_parse_gps(void)
_last_relposned_itow = _buffer.relposned.iTOW;
_last_relposned_ms = AP_HAL::millis();
/*
RELPOSNED messages gives the NED distance from base to
rover. It comes from the rover
*/
MB_Debug("RELPOSNED[%u]: od:%.2f rd:%.2f ae:%.2f flags:0x%04x t=%u",
state.instance+1,
offset_dist, rel_dist, alt_error,
unsigned(_buffer.relposned.flags),
unsigned(_buffer.relposned.iTOW));
if (((_buffer.relposned.flags & valid_mask) == valid_mask) &&
((_buffer.relposned.flags & invalid_mask) == 0) &&
rel_dist > min_separation &&
offset_dist > min_separation &&
fabsf(dist_error) < strict_length_error_allowed * min_dist &&
tilt_ok) {
float rotation_offset_rad;
const Vector3f diff = offset1 - offset0;
rotation_offset_rad = Vector2f(diff.x, diff.y).angle();
state.gps_yaw = wrap_360(_buffer.relposned.relPosHeading * 1e-5 - degrees(rotation_offset_rad));
state.have_gps_yaw = true;
calculate_moving_base_yaw(_buffer.relposned.relPosHeading * 1e-5,
_buffer.relposned.relPosLength * 0.01,
_buffer.relposned.relPosD*0.01)) {
state.gps_yaw_accuracy = _buffer.relposned.accHeading * 1e-5;
state.have_gps_yaw_accuracy = true;
} else {
state.have_gps_yaw = false;
state.have_gps_yaw_accuracy = false;
}
}
break;
#endif // GPS_MOVING_BASELINE
case MSG_PVT:
Debug("MSG_PVT");
@ -1707,7 +1677,7 @@ AP_GPS_UBLOX::_configure_valget(ConfigKey key)
bool
AP_GPS_UBLOX::_configure_config_set(const config_list *list, uint8_t count, uint32_t unconfig_bit)
{
#if GPS_UBLOX_MOVING_BASELINE
#if GPS_MOVING_BASELINE
active_config.list = list;
active_config.count = count;
active_config.done_mask = 0;
@ -1887,7 +1857,7 @@ bool AP_GPS_UBLOX::get_lag(float &lag_sec) const
// F9 lag not verified yet from flight log, but likely to be at least
// as good as M8
lag_sec = 0.12f;
#if GPS_UBLOX_MOVING_BASELINE
#if GPS_MOVING_BASELINE
if (role == AP_GPS::GPS_ROLE_MB_BASE ||
role == AP_GPS::GPS_ROLE_MB_ROVER) {
// the moving baseline rover will lag about 40ms from the
@ -1924,7 +1894,7 @@ void AP_GPS_UBLOX::_check_new_itow(uint32_t itow)
// support for retrieving RTCMv3 data from a moving baseline base
bool AP_GPS_UBLOX::get_RTCMV3(const uint8_t *&bytes, uint16_t &len)
{
#if GPS_UBLOX_MOVING_BASELINE
#if GPS_MOVING_BASELINE
if (rtcm3_parser) {
len = rtcm3_parser->get_len(bytes);
return len > 0;
@ -1936,7 +1906,7 @@ bool AP_GPS_UBLOX::get_RTCMV3(const uint8_t *&bytes, uint16_t &len)
// clear previous RTCM3 packet
void AP_GPS_UBLOX::clear_RTCMV3(void)
{
#if GPS_UBLOX_MOVING_BASELINE
#if GPS_MOVING_BASELINE
if (rtcm3_parser) {
rtcm3_parser->clear_packet();
}
@ -1952,7 +1922,7 @@ bool AP_GPS_UBLOX::is_healthy(void) const
return true;
}
#endif
#if GPS_UBLOX_MOVING_BASELINE
#if GPS_MOVING_BASELINE
if ((role == AP_GPS::GPS_ROLE_MB_BASE ||
role == AP_GPS::GPS_ROLE_MB_ROVER) &&
!supports_F9_config()) {

6
libraries/AP_GPS/AP_GPS_UBLOX.h
View File

@ -742,7 +742,7 @@ private:
return (uint8_t)(ubx_msg + (state.instance * UBX_MSG_TYPES));
}
#if GPS_UBLOX_MOVING_BASELINE
#if GPS_MOVING_BASELINE
// see if we should use uart2 for moving baseline config
bool mb_use_uart2(void) const {
return (driver_options() & unsigned(DRV_OPTIONS::MB_USE_UART2))?true:false;
@ -770,7 +770,7 @@ private:
// return true if GPS is capable of F9 config
bool supports_F9_config(void) const;
#if GPS_UBLOX_MOVING_BASELINE
#if GPS_MOVING_BASELINE
// config for moving baseline base
static const config_list config_MB_Base_uart1[];
static const config_list config_MB_Base_uart2[];
@ -789,5 +789,5 @@ private:
// RTCM3 parser for when in moving baseline base mode
RTCM3_Parser *rtcm3_parser;
#endif // GPS_UBLOX_MOVING_BASELINE
#endif // GPS_MOVING_BASELINE
};

78
libraries/AP_GPS/GPS_Backend.cpp
View File

@ -18,6 +18,7 @@
#include <AP_Logger/AP_Logger.h>
#include <time.h>
#include <AP_RTC/AP_RTC.h>
#include <AP_InternalError/AP_InternalError.h>
#define GPS_BACKEND_DEBUGGING 0
@ -301,3 +302,80 @@ void AP_GPS_Backend::check_new_itow(uint32_t itow, uint32_t msg_length)
}
}
}
#if GPS_MOVING_BASELINE
bool AP_GPS_Backend::calculate_moving_base_yaw(const float reported_heading_deg, const float reported_distance, const float reported_D)
{
constexpr float minimum_antenna_seperation = 0.05; // meters
constexpr float permitted_error_length_pct = 0.2; // percentage
bool selectedOffset = false;
Vector3f offset;
switch (MovingBase::Type(gps.mb_params[state.instance].type.get())) {
case MovingBase::Type::RelativeToAlternateInstance:
offset = gps._antenna_offset[state.instance^1].get() - gps._antenna_offset[state.instance].get();
selectedOffset = true;
break;
case MovingBase::Type::RelativeToCustomBase:
offset = gps.mb_params[state.instance].base_offset.get();
selectedOffset = true;
break;
}
if (!selectedOffset) {
// invalid type, let's throw up a flag
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
goto bad_yaw;
}
{
const float offset_dist = offset.length();
const float min_dist = MIN(offset_dist, reported_distance);
if (offset_dist < minimum_antenna_seperation) {
// offsets have to be sufficently large to get a meaningful angle off of them
Debug("Insufficent antenna offset (%f, %f, %f)", (double)offset.x, (double)offset.y, (double)offset.z);
goto bad_yaw;
}
if (reported_distance < minimum_antenna_seperation) {
// if the reported distance is less then the minimum seperation it's not sufficently robust
Debug("Reported baseline distance (%f) was less then the minimum antenna seperation (%f)",
(double)reported_distance, (double)minimum_antenna_seperation);
goto bad_yaw;
}
if ((offset_dist - reported_distance) > (min_dist * permitted_error_length_pct)) {
// the magnitude of the vector is much further then we were expecting
Debug("Exceeded the permitted error margin %f > %f",
(double)(offset_dist - reported_distance), (double)(min_dist * permitted_error_length_pct));
goto bad_yaw;
}
#ifndef HAL_BUILD_AP_PERIPH
{
const Vector3f antenna_tilt = AP::ahrs().get_rotation_body_to_ned() * offset;
const float alt_error = reported_D + antenna_tilt.z;
if (fabsf(alt_error) > permitted_error_length_pct * min_dist) {
// the vertical component is out of range, reject it
goto bad_yaw;
}
}
#endif // HAL_BUILD_AP_PERIPH
{
// at this point the offsets are looking okay, go ahead and actually calculate a useful heading
const float rotation_offset_rad = Vector2f(-offset.x, -offset.y).angle();
state.gps_yaw = wrap_360(reported_heading_deg - degrees(rotation_offset_rad));
state.have_gps_yaw = true;
}
}
return true;
bad_yaw:
state.have_gps_yaw = false;
return false;
}
#endif // GPS_MOVING_BASELINE

4
libraries/AP_GPS/GPS_Backend.h
View File

@ -118,6 +118,10 @@ protected:
return uint16_t(gps._driver_options.get());
}
#if GPS_MOVING_BASELINE
bool calculate_moving_base_yaw(const float reported_heading_deg, const float reported_distance, const float reported_D);
#endif //GPS_MOVING_BASELINE
private:
// itow from previous message
uint32_t _last_itow;

43
libraries/AP_GPS/MovingBase.cpp Normal file
View File

@ -0,0 +1,43 @@
#include "MovingBase.h"
const AP_Param::GroupInfo MovingBase::var_info[] = {
// @Param: TYPE
// @DisplayName: Moving base type
// @Description: Controls the type of moving base used if using moving base.
// @Values: 0:Relative to alternate GPS instance,1:RelativeToCustomBase
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO_FLAGS("TYPE", 1, MovingBase, type, int8_t(Type::RelativeToAlternateInstance), AP_PARAM_FLAG_ENABLE),
// @Param: OFS_X
// @DisplayName: Base antenna X position offset
// @Description: X position of the base GPS antenna in body frame. Positive X is forward of the origin. Use antenna phase centroid location if provided by the manufacturer.
// @Units: m
// @Range: -5 5
// @Increment: 0.01
// @User: Advanced
// @Param: OFS_Y
// @DisplayName: Base antenna Y position offset
// @Description: Y position of the base GPS antenna in body frame. Positive Y is to the right of the origin. Use antenna phase centroid location if provided by the manufacturer.
// @Units: m
// @Range: -5 5
// @Increment: 0.01
// @User: Advanced
// @Param: OFS_Z
// @DisplayName: Base antenna Z position offset
// @Description: Z position of the base GPS antenna in body frame. Positive Z is down from the origin. Use antenna phase centroid location if provided by the manufacturer.
// @Units: m
// @Range: -5 5
// @Increment: 0.01
// @User: Advanced
AP_GROUPINFO("OFS", 2, MovingBase, base_offset, 0.0f),
AP_GROUPEND
};
MovingBase::MovingBase(void) {
AP_Param::setup_object_defaults(this, var_info);
}

24
libraries/AP_GPS/MovingBase.h Normal file
View File

@ -0,0 +1,24 @@
#pragma once
#include <AP_Param/AP_Param.h>
#include <AP_Math/AP_Math.h>
class MovingBase {
public:
static const struct AP_Param::GroupInfo var_info[];
MovingBase(void);
/* Do not allow copies */
MovingBase(const MovingBase &other) = delete;
MovingBase &operator=(const MovingBase&) = delete;
enum class Type : int8_t {
RelativeToAlternateInstance = 0,
RelativeToCustomBase = 1,
};
AP_Int8 type; // an option from MovingBaseType
AP_Vector3f base_offset; // base position offset from the selected GPS reciever
};