forked from Archive/PX4-Autopilot
navigator/mission feasibility: prepare for pre-mission fence checking
This commit is contained in:
parent
09f29d0972
commit
4191ae33c2
|
@ -48,6 +48,7 @@
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
#include <fcntl.h>
|
#include <fcntl.h>
|
||||||
#include <errno.h>
|
#include <errno.h>
|
||||||
|
#include <uORB/topics/fence.h>
|
||||||
|
|
||||||
/* oddly, ERROR is not defined for c++ */
|
/* oddly, ERROR is not defined for c++ */
|
||||||
#ifdef ERROR
|
#ifdef ERROR
|
||||||
|
@ -61,7 +62,7 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capab
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nItems)
|
bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence)
|
||||||
{
|
{
|
||||||
/* Init if not done yet */
|
/* Init if not done yet */
|
||||||
init();
|
init();
|
||||||
|
@ -74,39 +75,39 @@ bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_
|
||||||
|
|
||||||
|
|
||||||
if (isRotarywing)
|
if (isRotarywing)
|
||||||
return checkMissionFeasibleRotarywing(dm_current, nItems);
|
return checkMissionFeasibleRotarywing(dm_current, nMissionItems, fence);
|
||||||
else
|
else
|
||||||
return checkMissionFeasibleFixedwing(dm_current, nItems);
|
return checkMissionFeasibleFixedwing(dm_current, nMissionItems, fence);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nItems)
|
bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence)
|
||||||
{
|
{
|
||||||
|
|
||||||
return checkGeofence(dm_current, nItems);
|
return checkGeofence(dm_current, nMissionItems, fence);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nItems)
|
bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence)
|
||||||
{
|
{
|
||||||
/* Update fixed wing navigation capabilites */
|
/* Update fixed wing navigation capabilites */
|
||||||
updateNavigationCapabilities();
|
updateNavigationCapabilities();
|
||||||
// warnx("_nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_horizontal_slope_displacement %.4f", _nav_caps.landing_slope_angle_rad, _nav_caps.landing_horizontal_slope_displacement);
|
// warnx("_nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_horizontal_slope_displacement %.4f", _nav_caps.landing_slope_angle_rad, _nav_caps.landing_horizontal_slope_displacement);
|
||||||
|
|
||||||
return (checkFixedWingLanding(dm_current, nItems) && checkGeofence(dm_current, nItems));
|
return (checkFixedWingLanding(dm_current, nMissionItems) && checkGeofence(dm_current, nMissionItems, fence));
|
||||||
}
|
}
|
||||||
|
|
||||||
bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nItems)
|
bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence)
|
||||||
{
|
{
|
||||||
//xxx: check geofence
|
//xxx: check geofence
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nItems)
|
bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems)
|
||||||
{
|
{
|
||||||
/* Go through all mission items and search for a landing waypoint
|
/* 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 */
|
* 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 < nItems; i++) {
|
for (size_t i = 0; i < nMissionItems; i++) {
|
||||||
static struct mission_item_s missionitem;
|
static struct mission_item_s missionitem;
|
||||||
const ssize_t len = sizeof(struct mission_item_s);
|
const ssize_t len = sizeof(struct mission_item_s);
|
||||||
if (dm_read(dm_current, i, &missionitem, len) != len) {
|
if (dm_read(dm_current, i, &missionitem, len) != len) {
|
||||||
|
|
|
@ -57,15 +57,15 @@ private:
|
||||||
void init();
|
void init();
|
||||||
|
|
||||||
/* Checks for all airframes */
|
/* Checks for all airframes */
|
||||||
bool checkGeofence(dm_item_t dm_current, size_t nItems);
|
bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence);
|
||||||
|
|
||||||
/* Checks specific to fixedwing airframes */
|
/* Checks specific to fixedwing airframes */
|
||||||
bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nItems);
|
bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence);
|
||||||
bool checkFixedWingLanding(dm_item_t dm_current, size_t nItems);
|
bool checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems);
|
||||||
void updateNavigationCapabilities();
|
void updateNavigationCapabilities();
|
||||||
|
|
||||||
/* Checks specific to rotarywing airframes */
|
/* Checks specific to rotarywing airframes */
|
||||||
bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nItems);
|
bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence);
|
||||||
public:
|
public:
|
||||||
|
|
||||||
MissionFeasibilityChecker();
|
MissionFeasibilityChecker();
|
||||||
|
@ -74,7 +74,7 @@ public:
|
||||||
/*
|
/*
|
||||||
* Returns true if mission is feasible and false otherwise
|
* Returns true if mission is feasible and false otherwise
|
||||||
*/
|
*/
|
||||||
bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nItems);
|
bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence);
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -456,7 +456,7 @@ Navigator::offboard_mission_update(bool isrotaryWing)
|
||||||
} else {
|
} else {
|
||||||
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
|
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
|
||||||
}
|
}
|
||||||
missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count);
|
missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _fence);
|
||||||
|
|
||||||
_mission.set_offboard_dataman_id(offboard_mission.dataman_id);
|
_mission.set_offboard_dataman_id(offboard_mission.dataman_id);
|
||||||
_mission.set_current_offboard_mission_index(offboard_mission.current_index);
|
_mission.set_current_offboard_mission_index(offboard_mission.current_index);
|
||||||
|
|
Loading…
Reference in New Issue