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

generic position controller status/feedback message

Browse Source
This commit is contained in:
Daniel Agar
2018-08-31 14:43:51 -04:00
parent 223dacee64
commit 5207c420c3
16 changed files with 303 additions and 279 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission_feasibility_checker.cpp
+10 -5
View File
@@ -47,8 +47,10 @@
#include <drivers/drv_pwm_output.h>
#include <fw_pos_control_l1/Landingslope.hpp>
#include <lib/ecl/geo/geo.h>
#include <mathlib/mathlib.h>
#include <lib/mathlib/mathlib.h>
#include <systemlib/mavlink_log.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/position_controller_landing_status.h>
bool
MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission,
@@ -395,19 +397,22 @@ MissionFeasibilityChecker::checkFixedWingLanding(const mission_s &mission, bool
if (MissionBlock::item_contains_position(missionitem_previous)) {
const bool fw_status_valid = (_navigator->get_fw_pos_ctrl_status()->timestamp > 0);
uORB::Subscription<position_controller_landing_status_s> landing_status{ORB_ID(position_controller_landing_status)};
landing_status.forcedUpdate();
const bool landing_status_valid = (landing_status.get().timestamp > 0);
const float wp_distance = get_distance_to_next_waypoint(missionitem_previous.lat, missionitem_previous.lon,
missionitem.lat, missionitem.lon);
if (fw_status_valid && (wp_distance > _navigator->get_fw_pos_ctrl_status()->landing_flare_length)) {
if (landing_status_valid && (wp_distance > landing_status.get().flare_length)) {
/* Last wp is before flare region */
const float delta_altitude = missionitem.altitude - missionitem_previous.altitude;
if (delta_altitude < 0) {
const float horizontal_slope_displacement = _navigator->get_fw_pos_ctrl_status()->landing_horizontal_slope_displacement;
const float slope_angle_rad = _navigator->get_fw_pos_ctrl_status()->landing_slope_angle_rad;
const float horizontal_slope_displacement = landing_status.get().horizontal_slope_displacement;
const float slope_angle_rad = landing_status.get().slope_angle_rad;
const float slope_alt_req = Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, missionitem.altitude,
horizontal_slope_displacement, slope_angle_rad);
src/modules/navigator/mission_feasibility_checker.h
-1
View File
@@ -43,7 +43,6 @@
#include <dataman/dataman.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/fw_pos_ctrl_status.h>
class Geofence;
class Navigator;
src/modules/navigator/navigator.h
+12 -9
View File
@@ -55,15 +55,17 @@
#include "rtl.h"
#include "takeoff.h"
#include <navigator/navigation.h>
#include <px4_module_params.h>
#include "navigation.h"
#include <lib/perf/perf_counter.h>
#include <px4_module.h>
#include <perf/perf_counter.h>
#include <uORB/topics/fw_pos_ctrl_status.h>
#include <px4_module_params.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/geofence_result.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/position_controller_status.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_command.h>
@@ -145,7 +147,6 @@ public:
/**
* Getters
*/
struct fw_pos_ctrl_status_s *get_fw_pos_ctrl_status() { return &_fw_pos_ctrl_status; }
struct home_position_s *get_home_position() { return &_home_pos; }
struct mission_result_s *get_mission_result() { return &_mission_result; }
struct position_setpoint_triplet_s *get_position_setpoint_triplet() { return &_pos_sp_triplet; }
@@ -278,7 +279,6 @@ public:
bool force_vtol();
private:
int _fw_pos_ctrl_status_sub{-1}; /**< notification of vehicle capabilities updates */
int _global_pos_sub{-1}; /**< global position subscription */
int _gps_pos_sub{-1}; /**< gps position subscription */
int _home_pos_sub{-1}; /**< home position subscription */
@@ -286,6 +286,7 @@ 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 _pos_ctrl_landing_status_sub{-1}; /**< position controller landing status subscription */
int _traffic_sub{-1}; /**< traffic subscription */
int _vehicle_command_sub{-1}; /**< vehicle commands (onboard and offboard) */
int _vstatus_sub{-1}; /**< vehicle status subscription */
@@ -299,7 +300,6 @@ private:
orb_advert_t _vehicle_roi_pub{nullptr};
// Subscriptions
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{};
vehicle_global_position_s _global_pos{}; /**< global vehicle position */
@@ -307,7 +307,11 @@ private:
vehicle_land_detected_s _land_detected{}; /**< vehicle land_detected */
vehicle_local_position_s _local_pos{}; /**< local vehicle position */
vehicle_status_s _vstatus{}; /**< vehicle status */
uint8_t _previous_nav_state{}; /**< nav_state of the previous iteration*/
uORB::Subscription<position_controller_status_s> _position_controller_status_sub{ORB_ID(position_controller_status)};
uint8_t _previous_nav_state{}; /**< nav_state of the previous iteration*/
// Publications
geofence_result_s _geofence_result{};
position_setpoint_triplet_s _pos_sp_triplet{}; /**< triplet of position setpoints */
@@ -369,7 +373,6 @@ private:
float _mission_throttle{-1.0f};
// update subscriptions
void fw_pos_ctrl_status_update(bool force = false);
void global_position_update();
void gps_position_update();
void home_position_update(bool force = false);
src/modules/navigator/navigator_main.cpp
+34 -37
View File
@@ -46,26 +46,24 @@
#include "navigator.h"
#include <cfloat>
#include <sys/ioctl.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <dataman/dataman.h>
#include <drivers/drv_hrt.h>
#include <lib/ecl/geo/geo.h>
#include <mathlib/mathlib.h>
#include <lib/mathlib/mathlib.h>
#include <px4_config.h>
#include <px4_defines.h>
#include <px4_posix.h>
#include <px4_tasks.h>
#include <systemlib/mavlink_log.h>
#include <uORB/topics/fw_pos_ctrl_status.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/position_controller_landing_status.h>
#include <uORB/topics/transponder_report.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/transponder_report.h>
#include <uORB/uORB.h>
/**
@@ -77,6 +75,8 @@ extern "C" __EXPORT int navigator_main(int argc, char *argv[]);
#define GEOFENCE_CHECK_INTERVAL 200000
using namespace time_literals;
namespace navigator
{
Navigator *g_navigator;
@@ -141,17 +141,6 @@ Navigator::home_position_update(bool force)
}
}
void
Navigator::fw_pos_ctrl_status_update(bool force)
{
bool updated = false;
orb_check(_fw_pos_ctrl_status_sub, &updated);
if (updated || force) {
orb_copy(ORB_ID(fw_pos_ctrl_status), _fw_pos_ctrl_status_sub, &_fw_pos_ctrl_status);
}
}
void
Navigator::vehicle_status_update()
{
@@ -193,7 +182,7 @@ 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));
_fw_pos_ctrl_status_sub = orb_subscribe(ORB_ID(fw_pos_ctrl_status));
_pos_ctrl_landing_status_sub = orb_subscribe(ORB_ID(position_controller_landing_status));
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
_home_pos_sub = orb_subscribe(ORB_ID(home_position));
@@ -209,7 +198,6 @@ Navigator::run()
local_position_update();
gps_position_update();
home_position_update(true);
fw_pos_ctrl_status_update(true);
params_update();
/* wakeup source(s) */
@@ -293,11 +281,7 @@ Navigator::run()
}
/* navigation capabilities updated */
orb_check(_fw_pos_ctrl_status_sub, &updated);
if (updated) {
fw_pos_ctrl_status_update();
}
_position_controller_status_sub.update();
/* home position updated */
orb_check(_home_pos_sub, &updated);
@@ -762,14 +746,11 @@ Navigator::run()
}
if (_pos_sp_triplet_updated) {
_pos_sp_triplet.timestamp = hrt_absolute_time();
publish_position_setpoint_triplet();
_pos_sp_triplet_updated = false;
}
if (_mission_result_updated) {
publish_mission_result();
_mission_result_updated = false;
}
perf_end(_loop_perf);
@@ -778,7 +759,7 @@ Navigator::run()
orb_unsubscribe(_global_pos_sub);
orb_unsubscribe(_local_pos_sub);
orb_unsubscribe(_gps_pos_sub);
orb_unsubscribe(_fw_pos_ctrl_status_sub);
orb_unsubscribe(_pos_ctrl_landing_status_sub);
orb_unsubscribe(_vstatus_sub);
orb_unsubscribe(_land_detected_sub);
orb_unsubscribe(_home_pos_sub);
@@ -827,11 +808,13 @@ Navigator::print_status()
void
Navigator::publish_position_setpoint_triplet()
{
/* do not publish an empty triplet */
// do not publish an invalid setpoint
if (!_pos_sp_triplet.current.valid) {
return;
}
_pos_sp_triplet.timestamp = hrt_absolute_time();
/* lazily publish the position setpoint triplet only once available */
if (_pos_sp_triplet_pub != nullptr) {
orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet);
@@ -839,6 +822,8 @@ Navigator::publish_position_setpoint_triplet()
} else {
_pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), &_pos_sp_triplet);
}
_pos_sp_triplet_updated = false;
}
float
@@ -949,10 +934,11 @@ Navigator::get_acceptance_radius(float mission_item_radius)
// when in fixed wing mode
// this might need locking against a commanded transition
// so that a stale _vstatus doesn't trigger an accepted mission item.
if (!_vstatus.is_rotary_wing && !_vstatus.in_transition_mode) {
if ((hrt_elapsed_time(&_fw_pos_ctrl_status.timestamp) < 5000000) && (_fw_pos_ctrl_status.turn_distance > radius)) {
radius = _fw_pos_ctrl_status.turn_distance;
}
const position_controller_status_s &pos_ctrl_status = _position_controller_status_sub.get();
if ((pos_ctrl_status.timestamp > _pos_sp_triplet.timestamp) && pos_ctrl_status.acceptance_radius > radius) {
radius = pos_ctrl_status.acceptance_radius;
}
return radius;
@@ -1114,15 +1100,24 @@ void Navigator::check_traffic()
bool
Navigator::abort_landing()
{
// only abort if currently landing and position controller status updated
bool should_abort = false;
if (!_vstatus.is_rotary_wing && !_vstatus.in_transition_mode) {
if (hrt_elapsed_time(&_fw_pos_ctrl_status.timestamp) < 1000000) {
if (_pos_sp_triplet.current.valid
&& _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
if (get_position_setpoint_triplet()->current.valid
&& get_position_setpoint_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
bool updated = false;
should_abort = _fw_pos_ctrl_status.abort_landing;
orb_check(_pos_ctrl_landing_status_sub, &updated);
if (updated) {
position_controller_landing_status_s landing_status = {};
// landing status from position controller must be newer than navigator's last position setpoint
if (orb_copy(ORB_ID(position_controller_landing_status), _pos_ctrl_landing_status_sub, &landing_status) == PX4_OK) {
if (landing_status.timestamp > _pos_sp_triplet.timestamp) {
should_abort = landing_status.abort_landing;
}
}
}
}
@@ -1214,6 +1209,8 @@ Navigator::publish_mission_result()
_mission_result.item_do_jump_changed = false;
_mission_result.item_changed_index = 0;
_mission_result.item_do_jump_remaining = 0;
_mission_result_updated = false;
}
void