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

Navigator: add MissionFeasibilityChecker class; performs validation of landing waypoint set-up for fixed wing for now, but can be extended for other checks (e.g. check mission against geofence)

Browse Source
This commit is contained in:
Thomas Gubler
2013-12-25 17:10:38 +01:00
parent d07cc95339
commit b02b48290f
9 changed files with 350 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+23 -6
View File
@@ -311,6 +311,11 @@ private:
*/
void vehicle_setpoint_poll();
/**
* Publish navigation capabilities
*/
void navigation_capabilities_publish();
/**
* Control position.
*/
@@ -538,6 +543,12 @@ FixedwingPositionControl::parameters_update()
/* Update the landing slope */
landingslope.update(math::radians(_parameters.land_slope_angle), _parameters.land_flare_alt_relative, _parameters.land_thrust_lim_horizontal_distance, _parameters.land_H1_virt);
/* Update and publish the navigation capabilities */
_nav_capabilities.landing_slope_angle_rad = landingslope.landing_slope_angle_rad();
_nav_capabilities.landing_horizontal_slope_displacement = landingslope.horizontal_slope_displacement();
_nav_capabilities.landing_flare_length = landingslope.flare_length();
navigation_capabilities_publish();
return OK;
}
@@ -709,6 +720,15 @@ FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector2f &cu
}
}
void FixedwingPositionControl::navigation_capabilities_publish()
{
if (_nav_capabilities_pub > 0) {
orb_publish(ORB_ID(navigation_capabilities), _nav_capabilities_pub, &_nav_capabilities);
} else {
_nav_capabilities_pub = orb_advertise(ORB_ID(navigation_capabilities), &_nav_capabilities);
}
}
bool
FixedwingPositionControl::control_position(const math::Vector2f &current_position, const math::Vector2f &ground_speed,
const struct mission_item_triplet_s &mission_item_triplet)
@@ -875,7 +895,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
}
float flare_curve_alt = landingslope.getFlarceCurveAltitude(wp_distance, _mission_item_triplet.current.altitude);
float flare_curve_alt = landingslope.getFlareCurveAltitude(wp_distance, _mission_item_triplet.current.altitude);
/* avoid climbout */
if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground)
@@ -1214,11 +1234,8 @@ FixedwingPositionControl::task_main()
/* set new turn distance */
_nav_capabilities.turn_distance = turn_distance;
if (_nav_capabilities_pub > 0) {
orb_publish(ORB_ID(navigation_capabilities), _nav_capabilities_pub, &_nav_capabilities);
} else {
_nav_capabilities_pub = orb_advertise(ORB_ID(navigation_capabilities), &_nav_capabilities);
}
navigation_capabilities_publish();
}
}
src/modules/fw_pos_control_l1/landingslope.cpp
+5 -7
View File
@@ -1,9 +1,7 @@
/****************************************************************************
*
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -37,7 +35,6 @@
/*
* @file: landingslope.cpp
*
* @author: Thomas Gubler <thomasgubler@gmail.com>
*/
#include "landingslope.h"
@@ -74,11 +71,12 @@ void Landingslope::calculateSlopeValues()
float Landingslope::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude)
{
return (wp_distance - _horizontal_slope_displacement) * tanf(_landing_slope_angle_rad) + wp_altitude; //flare_relative_alt is negative
return Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, wp_altitude, _horizontal_slope_displacement, _landing_slope_angle_rad);
}
float Landingslope::getFlarceCurveAltitude(float wp_distance, float wp_altitude)
float Landingslope::getFlareCurveAltitude(float wp_landing_distance, float wp_landing_altitude)
{
return wp_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_distance)/_flare_constant) - _H1_virt;
return wp_landing_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance)/_flare_constant) - _H1_virt;
}
src/modules/fw_pos_control_l1/landingslope.h
+28 -6
View File
@@ -1,9 +1,7 @@
/****************************************************************************
*
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -37,15 +35,18 @@
/*
* @file: landingslope.h
*
* @author: Thomas Gubler <thomasgubler@gmail.com>
*/
#ifndef LANDINGSLOPE_H_
#define LANDINGSLOPE_H_
#include <math.h>
#include <systemlib/err.h>
class Landingslope
{
private:
//xxx: documentation of landing pending
float _landing_slope_angle_rad;
float _flare_relative_alt;
float _motor_lim_horizontal_distance;
@@ -62,8 +63,29 @@ public:
Landingslope() {}
~Landingslope() {}
float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude);
float getFlarceCurveAltitude(float wp_distance, float wp_altitude);
float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_landing_altitude);
/**
*
* @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
*/
__EXPORT static float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_landing_altitude, float horizontal_slope_displacement, float landing_slope_angle_rad)
{
return (wp_landing_distance - horizontal_slope_displacement) * tanf(landing_slope_angle_rad) + wp_landing_altitude; //flare_relative_alt is negative
}
/**
*
* @return distance to landing waypoint of point on landing slope at altitude=slope_altitude
*/
__EXPORT static float getLandingSlopeWPDistance(float slope_altitude, float wp_landing_altitude, float horizontal_slope_displacement, float landing_slope_angle_rad)
{
return (slope_altitude - wp_landing_altitude)/tanf(landing_slope_angle_rad) + horizontal_slope_displacement;
}
float getFlareCurveAltitude(float wp_distance, float wp_altitude);
void update(float landing_slope_angle_rad,
float flare_relative_alt,