Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

ekf2: Allow use of RTK GPS heading #10355

Merged
merged 8 commits into from
Oct 17, 2018
Merged
2 changes: 2 additions & 0 deletions msg/ekf_gps_position.msg
Original file line number Diff line number Diff line change
@@ -14,4 +14,6 @@ float32 vel_e_m_s # GPS East velocity, (metres/sec)
float32 vel_d_m_s # GPS Down velocity, (metres/sec)
bool vel_ned_valid # True if NED velocity is valid
uint8 satellites_used # Number of satellites used
float32 heading # heading angle of XYZ body frame rel to NED. Set to NaN if not available and updated (used for dual antenna GPS), (rad, [-PI, PI])
float32 heading_offset # heading offset of dual antenna array in body frame. Set to NaN if not applicable. (rad, [-PI, PI])
uint8 selected # GPS selection: 0: GPS1, 1: GPS2. 2: GPS1+GPS2 blend
3 changes: 2 additions & 1 deletion msg/vehicle_gps_position.msg
Original file line number Diff line number Diff line change
@@ -32,4 +32,5 @@ uint64 time_utc_usec # Timestamp (microseconds, UTC), this is the timestamp whi

uint8 satellites_used # Number of satellites used

float32 heading # heading in NED. Set to NaN if not set (used for dual antenna GPS), (rad, [-PI, PI])
float32 heading # heading angle of XYZ body frame rel to NED. Set to NaN if not available and updated (used for dual antenna GPS), (rad, [-PI, PI])
float32 heading_offset # heading offset of dual antenna array in body frame. Set to NaN if not applicable. (rad, [-PI, PI])
3 changes: 3 additions & 0 deletions src/drivers/gps/gps.cpp
Original file line number Diff line number Diff line change
@@ -268,6 +268,7 @@ GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interfac
_port[sizeof(_port) - 1] = '\0';

_report_gps_pos.heading = NAN;
_report_gps_pos.heading_offset = NAN;

/* create satellite info data object if requested */
if (enable_sat_info) {
@@ -655,6 +656,7 @@ GPS::run()
_report_gps_pos.vel_ned_valid = true;
_report_gps_pos.satellites_used = 10;
_report_gps_pos.heading = NAN;
_report_gps_pos.heading_offset = NAN;

/* no time and satellite information simulated */

@@ -699,6 +701,7 @@ GPS::run()
/* reset report */
memset(&_report_gps_pos, 0, sizeof(_report_gps_pos));
_report_gps_pos.heading = NAN;
_report_gps_pos.heading_offset = heading_offset;

if (_mode == GPS_DRIVER_MODE_UBX) {

2 changes: 1 addition & 1 deletion src/lib/ecl
Submodule ecl updated from 2354c3 to f240ea
24 changes: 24 additions & 0 deletions src/modules/ekf2/ekf2_main.cpp
Original file line number Diff line number Diff line change
@@ -965,6 +965,8 @@ void Ekf2::run()
_gps_state[0].lat = gps.lat;
_gps_state[0].lon = gps.lon;
_gps_state[0].alt = gps.alt;
_gps_state[0].yaw = gps.heading;
_gps_state[0].yaw_offset = gps.heading_offset;
_gps_state[0].fix_type = gps.fix_type;
_gps_state[0].eph = gps.eph;
_gps_state[0].epv = gps.epv;
@@ -994,6 +996,8 @@ void Ekf2::run()
_gps_state[1].lat = gps.lat;
_gps_state[1].lon = gps.lon;
_gps_state[1].alt = gps.alt;
_gps_state[1].yaw = gps.heading;
_gps_state[1].yaw_offset = gps.heading_offset;
_gps_state[1].fix_type = gps.fix_type;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

fix_type line repeated

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

will fix

_gps_state[1].eph = gps.eph;
_gps_state[1].epv = gps.epv;
@@ -1074,6 +1078,8 @@ void Ekf2::run()
gps.vel_d_m_s = _gps_output[_gps_select_index].vel_ned[2];
gps.vel_ned_valid = _gps_output[_gps_select_index].vel_ned_valid;
gps.satellites_used = _gps_output[_gps_select_index].nsats;
gps.heading = _gps_output[_gps_select_index].yaw;
gps.heading_offset = _gps_output[_gps_select_index].yaw_offset;
gps.selected = _gps_select_index;

// Publish to the EKF blended GPS topic
@@ -2165,6 +2171,19 @@ void Ekf2::update_gps_blend_states()
_gps_blended_state.lon = (int32_t)(1.0E7 * lon_deg_res);
_gps_blended_state.alt += (int32_t)blended_alt_offset_mm;

// Take GPS heading from the highest weighted receiver that is publishing a valid .heading value
uint8_t gps_best_yaw_index = 0;
best_weight = 0.0f;

for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
if (PX4_ISFINITE(_gps_state[i].yaw) && (_blend_weights[i] > best_weight)) {
priseborough marked this conversation as resolved.
Show resolved Hide resolved
best_weight = _blend_weights[i];
gps_best_yaw_index = i;
}
}

_gps_blended_state.yaw = _gps_state[gps_best_yaw_index].yaw;
_gps_blended_state.yaw_offset = _gps_state[gps_best_yaw_index].yaw_offset;
}

/*
@@ -2266,6 +2285,9 @@ void Ekf2::apply_gps_offsets()
_gps_output[i].gdop = _gps_state[i].gdop;
_gps_output[i].nsats = _gps_state[i].nsats;
_gps_output[i].vel_ned_valid = _gps_state[i].vel_ned_valid;
_gps_output[i].yaw = _gps_state[i].yaw;
_gps_output[i].yaw_offset = _gps_state[i].yaw_offset;

}
}

@@ -2325,6 +2347,8 @@ void Ekf2::calc_gps_blend_output()
_gps_output[GPS_BLENDED_INSTANCE].gdop = _gps_blended_state.gdop;
_gps_output[GPS_BLENDED_INSTANCE].nsats = _gps_blended_state.nsats;
_gps_output[GPS_BLENDED_INSTANCE].vel_ned_valid = _gps_blended_state.vel_ned_valid;
_gps_output[GPS_BLENDED_INSTANCE].yaw = _gps_blended_state.yaw;
_gps_output[GPS_BLENDED_INSTANCE].yaw_offset = _gps_blended_state.yaw_offset;

}

9 changes: 6 additions & 3 deletions src/modules/ekf2/ekf2_params.c
Original file line number Diff line number Diff line change
@@ -490,13 +490,14 @@ PARAM_DEFINE_INT32(EKF2_DECL_TYPE, 7);
* If set to '3-axis' 3-axis field fusion is used at all times.
* If set to 'VTOL custom' the behaviour is the same as 'Automatic', but if fusing airspeed, magnetometer fusion is only allowed to modify the magnetic field states. This can be used by VTOL platforms with large magnetic field disturbances to prevent incorrect bias states being learned during forward flight operation which can adversely affect estimation accuracy after transition to hovering flight.
* If set to 'MC custom' the behaviour is the same as 'Automatic, but if there are no earth frame position or velocity observations being used, the magnetometer will not be used. This enables vehicles to operate with no GPS in environments where the magnetic field cannot be used to provide a heading reference. Prior to flight, the yaw angle is assumed to be constant if movement tests controlled by the EKF2_MOVE_TEST parameter indicate that the vehicle is static. This allows the vehicle to be placed on the ground to learn the yaw gyro bias prior to flight.
*
* If set to 'None' the magnetometer will not be used under any circumstance. Other sources of yaw may be used if selected via the EKF2_AID_MASK parameter.
* @group EKF2
* @value 0 Automatic
* @value 1 Magnetic heading
* @value 2 3-axis
* @value 3 VTOL customn
* @value 4 MC custom
* @value 5 None
* @reboot_required true
*/
PARAM_DEFINE_INT32(EKF2_MAG_TYPE, 0);
@@ -581,20 +582,22 @@ PARAM_DEFINE_FLOAT(EKF2_TAS_GATE, 3.0f);
* 1 : Set to true to use optical flow data if available
* 2 : Set to true to inhibit IMU bias estimation
* 3 : Set to true to enable vision position fusion
* 4 : Set to true to enable vision yaw fusion
* 4 : Set to true to enable vision yaw fusion. Cannot be used if bit position 7 is true.
* 5 : Set to true to enable multi-rotor drag specific force fusion
* 6 : set to true if the EV observations are in a non NED reference frame and need to be rotated before being used
* 7 : Set to true to enable GPS yaw fusion. Cannot be used if bit position 4 is true.
*
* @group EKF2
* @min 0
* @max 127
* @max 255
* @bit 0 use GPS
* @bit 1 use optical flow
* @bit 2 inhibit IMU bias estimation
* @bit 3 vision position fusion
* @bit 4 vision yaw fusion
* @bit 5 multi-rotor drag fusion
* @bit 6 rotate external vision
* @bit 7 GPS yaw fusion
* @reboot_required true
*/
PARAM_DEFINE_INT32(EKF2_AID_MASK, 1);
1 change: 1 addition & 0 deletions src/modules/mavlink/mavlink_receiver.cpp
Original file line number Diff line number Diff line change
@@ -2189,6 +2189,7 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
hil_gps.satellites_used = gps.satellites_visible; //TODO: rename mavlink_hil_gps_t sats visible to used?

hil_gps.heading = NAN;
hil_gps.heading_offset = NAN;

if (_gps_pub == nullptr) {
_gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps);
1 change: 1 addition & 0 deletions src/modules/simulator/gpssim/gpssim.cpp
Original file line number Diff line number Diff line change
@@ -179,6 +179,7 @@ GPSSIM::GPSSIM(bool fake_gps, bool enable_sat_info,
/* we need this potentially before it could be set in task_main */
g_dev = this;
_report_gps_pos.heading = NAN;
_report_gps_pos.heading_offset = NAN;

/* create satellite info data object if requested */
if (enable_sat_info) {
1 change: 1 addition & 0 deletions src/modules/uavcan/sensors/gnss.cpp
Original file line number Diff line number Diff line change
@@ -380,6 +380,7 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType>
report.vdop = msg.pdop;

report.heading = NAN;
report.heading_offset = NAN;

// Publish to a multi-topic
int32_t gps_orb_instance;