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

FW add mavlink NAV_CONTROLLER_OUTPUT

Browse Source
This commit is contained in:
Daniel Agar
2016-05-13 12:56:04 +02:00
committed by Lorenz Meier
parent 5fb1de906c
commit eae726e345
9 changed files with 170 additions and 84 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission_feasibility_checker.cpp
+7 -8
View File
@@ -55,11 +55,11 @@
MissionFeasibilityChecker::MissionFeasibilityChecker() :
_mavlink_log_pub(nullptr),
_capabilities_sub(-1),
_fw_pos_ctrl_status_sub(-1),
_initDone(false),
_dist_1wp_ok(false)
{
_nav_caps = {0};
_fw_pos_ctrl_status = {};
}
@@ -275,7 +275,6 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size
/* Go through all mission items and search for a landing waypoint
* if landing waypoint is found: the previous waypoint is checked to be at a feasible distance and altitude given the landing slope */
for (size_t i = 0; i < nMissionItems; i++) {
struct mission_item_s missionitem;
const ssize_t len = sizeof(missionitem);
@@ -293,15 +292,15 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size
}
float wp_distance = get_distance_to_next_waypoint(missionitem_previous.lat , missionitem_previous.lon, missionitem.lat, missionitem.lon);
float slope_alt_req = Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, missionitem.altitude, _nav_caps.landing_horizontal_slope_displacement, _nav_caps.landing_slope_angle_rad);
float wp_distance_req = Landingslope::getLandingSlopeWPDistance(missionitem_previous.altitude, missionitem.altitude, _nav_caps.landing_horizontal_slope_displacement, _nav_caps.landing_slope_angle_rad);
float slope_alt_req = Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, missionitem.altitude, _fw_pos_ctrl_status.landing_horizontal_slope_displacement, _fw_pos_ctrl_status.landing_slope_angle_rad);
float wp_distance_req = Landingslope::getLandingSlopeWPDistance(missionitem_previous.altitude, missionitem.altitude, _fw_pos_ctrl_status.landing_horizontal_slope_displacement, _fw_pos_ctrl_status.landing_slope_angle_rad);
float delta_altitude = missionitem.altitude - missionitem_previous.altitude;
// warnx("wp_distance %.2f, delta_altitude %.2f, missionitem_previous.altitude %.2f, missionitem.altitude %.2f, slope_alt_req %.2f, wp_distance_req %.2f",
// wp_distance, delta_altitude, missionitem_previous.altitude, missionitem.altitude, slope_alt_req, wp_distance_req);
// warnx("_nav_caps.landing_horizontal_slope_displacement %.4f, _nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_flare_length %.4f",
// _nav_caps.landing_horizontal_slope_displacement, _nav_caps.landing_slope_angle_rad, _nav_caps.landing_flare_length);
if (wp_distance > _nav_caps.landing_flare_length) {
if (wp_distance > _fw_pos_ctrl_status.landing_flare_length) {
/* Last wp is before flare region */
if (delta_altitude < 0) {
@@ -424,14 +423,14 @@ MissionFeasibilityChecker::isPositionCommand(unsigned cmd){
void MissionFeasibilityChecker::updateNavigationCapabilities()
{
(void)orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps);
(void)orb_copy(ORB_ID(fw_pos_ctrl_status), _fw_pos_ctrl_status_sub, &_fw_pos_ctrl_status);
}
void MissionFeasibilityChecker::init()
{
if (!_initDone) {
_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
_fw_pos_ctrl_status_sub = orb_subscribe(ORB_ID(fw_pos_ctrl_status));
_initDone = true;
}
src/modules/navigator/mission_feasibility_checker.h
+3 -3
View File
@@ -44,7 +44,7 @@
#include <unistd.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/navigation_capabilities.h>
#include <uORB/topics/fw_pos_ctrl_status.h>
#include <dataman/dataman.h>
#include "geofence.h"
@@ -54,8 +54,8 @@ class MissionFeasibilityChecker
private:
orb_advert_t *_mavlink_log_pub;
int _capabilities_sub;
struct navigation_capabilities_s _nav_caps;
int _fw_pos_ctrl_status_sub;
struct fw_pos_ctrl_status_s _fw_pos_ctrl_status;
bool _initDone;
bool _dist_1wp_ok;
src/modules/navigator/navigator.h
+4 -4
View File
@@ -207,7 +207,7 @@ private:
int _home_pos_sub; /**< home position subscription */
int _vstatus_sub; /**< vehicle status subscription */
int _land_detected_sub; /**< vehicle land detected subscription */
int _capabilities_sub; /**< notification of vehicle capabilities updates */
int _fw_pos_ctrl_status_sub; /**< notification of vehicle capabilities updates */
int _control_mode_sub; /**< vehicle control mode subscription */
int _onboard_mission_sub; /**< onboard mission subscription */
int _offboard_mission_sub; /**< offboard mission subscription */
@@ -229,7 +229,7 @@ private:
sensor_combined_s _sensor_combined; /**< sensor values */
home_position_s _home_pos; /**< home position for RTL */
mission_item_s _mission_item; /**< current mission item */
navigation_capabilities_s _nav_caps; /**< navigation capabilities */
fw_pos_ctrl_status_s _fw_pos_ctrl_status; /**< fixed wing navigation capabilities */
position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */
position_setpoint_triplet_s _reposition_triplet; /**< triplet for non-mission direct position command */
position_setpoint_triplet_s _takeoff_triplet; /**< triplet for non-mission direct takeoff command */
@@ -301,9 +301,9 @@ private:
void home_position_update(bool force=false);
/**
* Retreive navigation capabilities
* Retrieve fixed wing navigation capabilities
*/
void navigation_capabilities_update();
void fw_pos_ctrl_status_update();
/**
* Retrieve vehicle status
src/modules/navigator/navigator_main.cpp
+13 -13
View File
@@ -70,7 +70,7 @@
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/fence.h>
#include <uORB/topics/navigation_capabilities.h>
#include <uORB/topics/fw_pos_ctrl_status.h>
#include <uORB/topics/vehicle_command.h>
#include <drivers/drv_baro.h>
@@ -108,7 +108,7 @@ Navigator::Navigator() :
_home_pos_sub(-1),
_vstatus_sub(-1),
_land_detected_sub(-1),
_capabilities_sub(-1),
_fw_pos_ctrl_status_sub(-1),
_control_mode_sub(-1),
_onboard_mission_sub(-1),
_offboard_mission_sub(-1),
@@ -126,7 +126,7 @@ Navigator::Navigator() :
_sensor_combined{},
_home_pos{},
_mission_item{},
_nav_caps{},
_fw_pos_ctrl_status{},
_pos_sp_triplet{},
_reposition_triplet{},
_takeoff_triplet{},
@@ -231,9 +231,9 @@ Navigator::home_position_update(bool force)
}
void
Navigator::navigation_capabilities_update()
Navigator::fw_pos_ctrl_status_update()
{
orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps);
orb_copy(ORB_ID(fw_pos_ctrl_status), _fw_pos_ctrl_status_sub, &_fw_pos_ctrl_status);
}
void
@@ -298,7 +298,7 @@ Navigator::task_main()
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
_gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
_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));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
@@ -316,7 +316,7 @@ Navigator::task_main()
gps_position_update();
sensor_combined_update();
home_position_update(true);
navigation_capabilities_update();
fw_pos_ctrl_status_update();
params_update();
/* wakeup source(s) */
@@ -395,9 +395,9 @@ Navigator::task_main()
}
/* navigation capabilities updated */
orb_check(_capabilities_sub, &updated);
orb_check(_fw_pos_ctrl_status_sub, &updated);
if (updated) {
navigation_capabilities_update();
fw_pos_ctrl_status_update();
}
/* home position updated */
@@ -524,7 +524,7 @@ Navigator::task_main()
_can_loiter_at_sp = false;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
if (_nav_caps.abort_landing) {
if (_fw_pos_ctrl_status.abort_landing) {
// pos controller aborted landing, requests loiter
// above landing waypoint
_navigation_mode = &_loiter;
@@ -732,9 +732,9 @@ 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 && hrt_elapsed_time(&_nav_caps.timestamp) < 5000000) {
if (_nav_caps.turn_distance > radius) {
radius = _nav_caps.turn_distance;
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;
}
}