Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-02 20:28:37 +08:00
Code Issues Actions 20 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
+20 -11
View File
@@ -70,6 +70,7 @@
#include <arch/board/board.h> #include <arch/board/board.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <mathlib/mathlib.h> #include <mathlib/mathlib.h>
#include <matrix/math.hpp>
#include <systemlib/err.h> #include <systemlib/err.h>
#include <parameters/param.h> #include <parameters/param.h>
#include <drivers/drv_gps.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)); _orb_inject_data_fd = orb_subscribe(ORB_ID(gps_inject_data));
initializeCommunicationDump(); initializeCommunicationDump();
@@ -655,13 +671,9 @@ GPS::run()
_mode = GPS_DRIVER_MODE_UBX; _mode = GPS_DRIVER_MODE_UBX;
/* FALLTHROUGH */ /* FALLTHROUGH */
case GPS_DRIVER_MODE_UBX: { 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, _helper = new GPSDriverUBX(_interface, &GPS::callback, this, &_report_gps_pos, _p_report_sat_info,
param_gps_ubx_dynmodel); gps_ubx_dynmodel);
}
break; break;
case GPS_DRIVER_MODE_MTK: case GPS_DRIVER_MODE_MTK:
@@ -669,18 +681,15 @@ GPS::run()
break; break;
case GPS_DRIVER_MODE_ASHTECH: 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; break;
default: default:
break; 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) { if (_helper && _helper->configure(_baudrate, GPSHelper::OutputMode::GPS) == 0) {
/* reset report */ /* reset report */
src/drivers/gps/params.c
+24
View File
@@ -63,3 +63,27 @@ PARAM_DEFINE_INT32(GPS_DUMP_COMM, 0);
* @group GPS * @group GPS
*/ */
PARAM_DEFINE_INT32(GPS_UBX_DYNMODEL, 7); 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);