fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-21 07:07:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

gps: add GPS_YAW_OFFSET param & properly initialize _baudrate

Browse Source
This commit is contained in:
Beat Küng
2018-07-16 09:59:57 +02:00
parent a947ad2506
commit 4d71686ac6
2 changed files with 45 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/gps/gps.cpp
+21 -12
View File
@@ -70,6 +70,7 @@
#include <arch/board/board.h>
#include <drivers/drv_hrt.h>
#include <mathlib/mathlib.h>
#include <matrix/math.hpp>
#include <systemlib/err.h>
#include <parameters/param.h>
#include <drivers/drv_gps.h>
@@ -604,6 +605,21 @@ GPS::run()
}
}
param_t handle = param_find("GPS_YAW_OFFSET");
float heading_offset = 0.f;
if (handle != PARAM_INVALID) {
param_get(handle, &heading_offset);
heading_offset = matrix::wrap_pi(math::radians(heading_offset));
}
int32_t gps_ubx_dynmodel = 7; // default to 7: airborne with <2g acceleration
handle = param_find("GPS_UBX_DYNMODEL");
if (handle != PARAM_INVALID) {
param_get(handle, &gps_ubx_dynmodel);
}
_orb_inject_data_fd = orb_subscribe(ORB_ID(gps_inject_data));
initializeCommunicationDump();
@@ -655,13 +671,9 @@ GPS::run()
_mode = GPS_DRIVER_MODE_UBX;
/* FALLTHROUGH */
case GPS_DRIVER_MODE_UBX: {
int32_t param_gps_ubx_dynmodel = 7; // default to 7: airborne with <2g acceleration
param_get(param_find("GPS_UBX_DYNMODEL"), &param_gps_ubx_dynmodel);
_helper = new GPSDriverUBX(_interface, &GPS::callback, this, &_report_gps_pos, _p_report_sat_info,
param_gps_ubx_dynmodel);
}
case GPS_DRIVER_MODE_UBX:
_helper = new GPSDriverUBX(_interface, &GPS::callback, this, &_report_gps_pos, _p_report_sat_info,
gps_ubx_dynmodel);
break;
case GPS_DRIVER_MODE_MTK:
@@ -669,18 +681,15 @@ GPS::run()
break;
case GPS_DRIVER_MODE_ASHTECH:
_helper = new GPSDriverAshtech(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info);
_helper = new GPSDriverAshtech(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info, heading_offset);
break;
default:
break;
}
_baudrate = 0; // auto-detect
/* the Ashtech driver lies about successful configuration and the
* MTK driver is not well tested, so we really only trust the UBX
* driver for an advance publication
*/
if (_helper && _helper->configure(_baudrate, GPSHelper::OutputMode::GPS) == 0) {
/* reset report */
src/drivers/gps/params.c
+24
View File
@@ -63,3 +63,27 @@ PARAM_DEFINE_INT32(GPS_DUMP_COMM, 0);
* @group GPS
*/
PARAM_DEFINE_INT32(GPS_UBX_DYNMODEL, 7);
/**
* Heading/Yaw offset for dual antenna GPS
*
* Heading offset angle for dual antenna GPS setups that support heading estimation.
* (currently only for the Trimble MB-Two).
*
* Set this to 0 if the antennas are parallel to the forward-facing direction of the vehicle and the first antenna is in
* front. The offset angle increases counterclockwise.
*
* Set this to 90 if the first antenna is placed on the right side and the second on the left side of the vehicle.
*
* @min 0
* @max 360
* @unit deg
* @reboot_required true
* @decimal 0
*
* @group GPS
*/
PARAM_DEFINE_FLOAT(GPS_YAW_OFFSET, 0.f);