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

sensors: always start baro/GPS/mag aggregators if SYS_HAS_* set

Browse Source 
- add new SYS_HAS_GPS parameter
This commit is contained in:
Daniel Agar
2021-08-20 09:12:37 -04:00
parent 6d3120d00d
commit 61702d0d97
3 changed files with 30 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp
+7 -3
View File
@@ -67,8 +67,11 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &
int32_t arm_without_gps = 0;
param_get(param_find("COM_ARM_WO_GPS"), &arm_without_gps);
bool gps_success = true;
bool gps_present = true;
int32_t sys_has_gps = 1;
param_get(param_find("SYS_HAS_GPS"), &sys_has_gps);
bool gps_success = false;
bool gps_present = false;
// Get estimator status data if available and exit with a fail recorded if not
uORB::SubscriptionData<estimator_selector_status_s> estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
@@ -154,10 +157,11 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &
}
// If GPS aiding is required, declare fault condition if the required GPS quality checks are failing
{
if (sys_has_gps == 1) {
const bool ekf_gps_fusion = status.control_mode_flags & (1 << estimator_status_s::CS_GPS);
const bool ekf_gps_check_fail = status.gps_check_fail_flags > 0;
gps_present = true;
gps_success = ekf_gps_fusion; // default to success if gps data is fused
if (ekf_gps_check_fail) {
src/modules/sensors/sensors.cpp
+9 -12
View File
@@ -220,6 +220,7 @@ private:
DEFINE_PARAMETERS(
(ParamBool<px4::params::SYS_HAS_BARO>) _param_sys_has_baro,
(ParamBool<px4::params::SYS_HAS_GPS>) _param_sys_has_gps,
(ParamBool<px4::params::SYS_HAS_MAG>) _param_sys_has_mag,
(ParamBool<px4::params::SENS_IMU_MODE>) _param_sens_imu_mode
)
@@ -535,12 +536,10 @@ void Sensors::InitializeVehicleAirData()
{
if (_param_sys_has_baro.get()) {
if (_vehicle_air_data == nullptr) {
if (orb_exists(ORB_ID(sensor_baro), 0) == PX4_OK) {
_vehicle_air_data = new VehicleAirData();
_vehicle_air_data = new VehicleAirData();
if (_vehicle_air_data) {
_vehicle_air_data->Start();
}
if (_vehicle_air_data) {
_vehicle_air_data->Start();
}
}
}
@@ -548,8 +547,8 @@ void Sensors::InitializeVehicleAirData()
void Sensors::InitializeVehicleGPSPosition()
{
if (_vehicle_gps_position == nullptr) {
if (orb_exists(ORB_ID(sensor_gps), 0) == PX4_OK) {
if (_param_sys_has_gps.get()) {
if (_vehicle_gps_position == nullptr) {
_vehicle_gps_position = new VehicleGPSPosition();
if (_vehicle_gps_position) {
@@ -603,12 +602,10 @@ void Sensors::InitializeVehicleMagnetometer()
{
if (_param_sys_has_mag.get()) {
if (_vehicle_magnetometer == nullptr) {
if (orb_exists(ORB_ID(sensor_mag), 0) == PX4_OK) {
_vehicle_magnetometer = new VehicleMagnetometer();
_vehicle_magnetometer = new VehicleMagnetometer();
if (_vehicle_magnetometer) {
_vehicle_magnetometer->Start();
}
if (_vehicle_magnetometer) {
_vehicle_magnetometer->Start();
}
}
}