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

Move GPS blending from ekf2 to sensors module

Browse Source 
- new sensors work item that subscribes to N x sensor_gps and publishes vehicle_gps_position
 - blending is now configurable with SENS_GPS_MASK and SENS_GPS_TAU

Co-authored-by: Jacob Crabill <jacob@volans-i.com>
Co-authored-by: Jacob Dahl <dahl.jakejacob@gmail.com>
This commit is contained in:
Jacob Dahl
2020-09-25 23:28:31 -04:00
committed by GitHub
co-authored by Jacob Crabill Jacob Dahl
parent e792c46f20
commit a24488328f
27 changed files with 1147 additions and 861 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/sensors.cpp
+28
View File
@@ -73,6 +73,7 @@
#include "vehicle_acceleration/VehicleAcceleration.hpp"
#include "vehicle_angular_velocity/VehicleAngularVelocity.hpp"
#include "vehicle_air_data/VehicleAirData.hpp"
#include "vehicle_gps_position/VehicleGPSPosition.hpp"
#include "vehicle_imu/VehicleIMU.hpp"
#include "vehicle_magnetometer/VehicleMagnetometer.hpp"
@@ -172,6 +173,7 @@ private:
VehicleAngularVelocity _vehicle_angular_velocity;
VehicleAirData *_vehicle_air_data{nullptr};
VehicleMagnetometer *_vehicle_magnetometer{nullptr};
VehicleGPSPosition *_vehicle_gps_position{nullptr};
static constexpr int MAX_SENSOR_COUNT = 3;
VehicleIMU *_vehicle_imu_list[MAX_SENSOR_COUNT] {};
@@ -204,6 +206,7 @@ private:
void adc_poll();
void InitializeVehicleAirData();
void InitializeVehicleGPSPosition();
void InitializeVehicleIMU();
void InitializeVehicleMagnetometer();
@@ -275,6 +278,11 @@ Sensors::~Sensors()
delete _vehicle_air_data;
}
if (_vehicle_gps_position) {
_vehicle_gps_position->Stop();
delete _vehicle_gps_position;
}
if (_vehicle_magnetometer) {
_vehicle_magnetometer->Stop();
delete _vehicle_magnetometer;
@@ -483,6 +491,19 @@ void Sensors::InitializeVehicleAirData()
}
}
void Sensors::InitializeVehicleGPSPosition()
{
if (_vehicle_gps_position == nullptr) {
if (orb_exists(ORB_ID(sensor_gps), 0) == PX4_OK) {
_vehicle_gps_position = new VehicleGPSPosition();
if (_vehicle_gps_position) {
_vehicle_gps_position->Start();
}
}
}
}
void Sensors::InitializeVehicleIMU()
{
// create a VehicleIMU instance for each accel/gyro pair
@@ -553,6 +574,7 @@ void Sensors::Run()
if (_last_config_update == 0) {
InitializeVehicleAirData();
InitializeVehicleIMU();
InitializeVehicleGPSPosition();
InitializeVehicleMagnetometer();
_voted_sensors_update.init(_sensor_combined);
parameter_update_poll(true);
@@ -592,6 +614,7 @@ void Sensors::Run()
_voted_sensors_update.initializeSensors();
InitializeVehicleAirData();
InitializeVehicleIMU();
InitializeVehicleGPSPosition();
InitializeVehicleMagnetometer();
_last_config_update = hrt_absolute_time();
@@ -678,6 +701,11 @@ int Sensors::print_status()
PX4_INFO_RAW("\n");
_vehicle_angular_velocity.PrintStatus();
if (_vehicle_gps_position) {
PX4_INFO_RAW("\n");
_vehicle_gps_position->PrintStatus();
}
PX4_INFO_RAW("\n");
for (auto &i : _vehicle_imu_list) {