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

move baro and magnetometer data out of sensor_combined

Browse Source
This commit is contained in:
Daniel Agar
2018-04-11 23:26:58 -04:00
parent 3e6ba1d541
commit 3b5b12e1d1
31 changed files with 289 additions and 266 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/geofence.cpp
+11 -9
View File
@@ -54,7 +54,8 @@
Geofence::Geofence(Navigator *navigator) :
ModuleParams(navigator),
_navigator(navigator)
_navigator(navigator),
_sub_airdata(ORB_ID(vehicle_air_data))
{
// we assume there's no concurrent fence update on startup
_updateFence();
@@ -181,15 +182,13 @@ bool Geofence::checkAll(const struct vehicle_global_position_s &global_position)
return checkAll(global_position.lat, global_position.lon, global_position.alt);
}
bool Geofence::checkAll(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl)
bool Geofence::checkAll(const struct vehicle_global_position_s &global_position, const float alt)
{
return checkAll(global_position.lat, global_position.lon, baro_altitude_amsl);
return checkAll(global_position.lat, global_position.lon, alt);
}
bool Geofence::check(const struct vehicle_global_position_s &global_position,
const struct vehicle_gps_position_s &gps_position, float baro_altitude_amsl,
const struct home_position_s home_pos, bool home_position_set)
bool Geofence::check(const vehicle_global_position_s &global_position, const vehicle_gps_position_s &gps_position,
const home_position_s home_pos, bool home_position_set)
{
if (getAltitudeMode() == Geofence::GF_ALT_MODE_WGS84) {
if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
@@ -201,12 +200,15 @@ bool Geofence::check(const struct vehicle_global_position_s &global_position,
}
} else {
// get baro altitude
_sub_airdata.update();
const float baro_altitude_amsl = _sub_airdata.get().baro_alt_meter;
if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
return checkAll(global_position, baro_altitude_amsl);
} else {
return checkAll((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7,
baro_altitude_amsl);
return checkAll((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, baro_altitude_amsl);
}
}
}
src/modules/navigator/geofence.h
+9 -6
View File
@@ -46,9 +46,11 @@
#include <drivers/drv_hrt.h>
#include <lib/ecl/geo/geo.h>
#include <px4_defines.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_air_data.h>
#define GEOFENCE_FILENAME PX4_ROOTFSDIR"/fs/microsd/etc/geofence.txt"
@@ -85,9 +87,8 @@ public:
*
* @return true: system is obeying fence, false: system is violating fence
*/
bool check(const struct vehicle_global_position_s &global_position,
const struct vehicle_gps_position_s &gps_position, float baro_altitude_amsl,
const struct home_position_s home_pos, bool home_position_set);
bool check(const vehicle_global_position_s &global_position,
const vehicle_gps_position_s &gps_position, const home_position_s home_pos, bool home_position_set);
/**
* Return whether a mission item obeys the geofence.
@@ -165,6 +166,8 @@ private:
(ParamFloat<px4::params::GF_MAX_VER_DIST>) _param_max_ver_distance
)
uORB::Subscription<vehicle_air_data_s> _sub_airdata;
int _outside_counter{0};
uint16_t _update_counter{0}; ///< dataman update counter: if it does not match, we polygon data was updated
@@ -193,8 +196,8 @@ private:
*/
bool checkAll(double lat, double lon, float altitude);
bool checkAll(const struct vehicle_global_position_s &global_position);
bool checkAll(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl);
bool checkAll(const vehicle_global_position_s &global_position);
bool checkAll(const vehicle_global_position_s &global_position, float baro_altitude_amsl);
/**
* Check if a single point is within a polygon
src/modules/navigator/navigator.h
-3
View File
@@ -277,7 +277,6 @@ private:
int _local_pos_sub{-1}; /**< local position subscription */
int _offboard_mission_sub{-1}; /**< offboard mission subscription */
int _param_update_sub{-1}; /**< param update subscription */
int _sensor_combined_sub{-1}; /**< sensor combined subscription */
int _traffic_sub{-1}; /**< traffic subscription */
int _vehicle_command_sub{-1}; /**< vehicle commands (onboard and offboard) */
int _vstatus_sub{-1}; /**< vehicle status subscription */
@@ -294,7 +293,6 @@ private:
fw_pos_ctrl_status_s _fw_pos_ctrl_status{}; /**< fixed wing navigation capabilities */
home_position_s _home_pos{}; /**< home position for RTL */
mission_result_s _mission_result{};
sensor_combined_s _sensor_combined{}; /**< sensor values */
vehicle_global_position_s _global_pos{}; /**< global vehicle position */
vehicle_gps_position_s _gps_pos{}; /**< gps position */
vehicle_land_detected_s _land_detected{}; /**< vehicle land_detected */
@@ -366,7 +364,6 @@ private:
void home_position_update(bool force = false);
void local_position_update();
void params_update();
void sensor_combined_update();
void vehicle_land_detected_update();
void vehicle_status_update();
src/modules/navigator/navigator_main.cpp
+1 -17
View File
@@ -132,12 +132,6 @@ Navigator::gps_position_update()
orb_copy(ORB_ID(vehicle_gps_position), _gps_pos_sub, &_gps_pos);
}
void
Navigator::sensor_combined_update()
{
orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
}
void
Navigator::home_position_update(bool force)
{
@@ -201,7 +195,6 @@ Navigator::run()
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
_gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
_fw_pos_ctrl_status_sub = orb_subscribe(ORB_ID(fw_pos_ctrl_status));
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
@@ -217,7 +210,6 @@ Navigator::run()
global_position_update();
local_position_update();
gps_position_update();
sensor_combined_update();
home_position_update(true);
fw_pos_ctrl_status_update(true);
params_update();
@@ -281,13 +273,6 @@ Navigator::run()
}
}
/* sensors combined updated */
orb_check(_sensor_combined_sub, &updated);
if (updated) {
sensor_combined_update();
}
/* parameters updated */
orb_check(_param_update_sub, &updated);
@@ -538,7 +523,7 @@ Navigator::run()
(_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) &&
(hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL)) {
bool inside = _geofence.check(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter, _home_pos,
bool inside = _geofence.check(_global_pos, _gps_pos, _home_pos,
home_position_valid());
last_geofence_check = hrt_absolute_time();
have_geofence_position_data = false;
@@ -711,7 +696,6 @@ Navigator::run()
orb_unsubscribe(_global_pos_sub);
orb_unsubscribe(_local_pos_sub);
orb_unsubscribe(_gps_pos_sub);
orb_unsubscribe(_sensor_combined_sub);
orb_unsubscribe(_fw_pos_ctrl_status_sub);
orb_unsubscribe(_vstatus_sub);
orb_unsubscribe(_land_detected_sub);