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

Navigator move get_time_inside and cleanup (#7062)

Browse Source
This commit is contained in:
Daniel Agar
2017-04-20 11:24:55 -04:00
committed by GitHub
parent 1913b970d7
commit 56b028148b
9 changed files with 234 additions and 436 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission_block.cpp
+28 -49
View File
@@ -40,41 +40,24 @@
* @author Andreas Antener <andreas@uaventure.com>
*/
#include <sys/types.h>
#include <string.h>
#include <stdlib.h>
#include <unistd.h>
#include "mission_block.h"
#include "navigator.h"
#include <math.h>
#include <float.h>
#include <systemlib/err.h>
#include <geo/geo.h>
#include <systemlib/mavlink_log.h>
#include <mathlib/mathlib.h>
#include <uORB/uORB.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vtol_vehicle_status.h>
#include "navigator.h"
#include "mission_block.h"
actuator_controls_s actuators;
orb_advert_t actuator_pub_fd;
MissionBlock::MissionBlock(Navigator *navigator, const char *name) :
NavigatorMode(navigator, name),
_mission_item{},
_waypoint_position_reached(false),
_waypoint_yaw_reached(false),
_time_first_inside_orbit(0),
_action_start(0),
_time_wp_reached(0),
_actuators{},
_actuator_pub(nullptr),
_cmd_pub(nullptr),
_param_loiter_min_alt(this, "MIS_LTRMIN_ALT", false),
_param_yaw_timeout(this, "MIS_YAW_TMT", false),
_param_yaw_err(this, "MIS_YAW_ERR", false),
@@ -86,10 +69,6 @@ MissionBlock::MissionBlock(Navigator *navigator, const char *name) :
{
}
MissionBlock::~MissionBlock()
{
}
bool
MissionBlock::is_mission_item_reached()
{
@@ -125,6 +104,7 @@ MissionBlock::is_mission_item_reached()
* We wait half a second to give the transition command time to propagate.
* Then monitor the transition status for completion.
*/
// TODO: check desired transition state achieved and drop _action_start
if (hrt_absolute_time() - _action_start > 500000 &&
!_navigator->get_vstatus()->in_transition_mode) {
@@ -145,8 +125,7 @@ MissionBlock::is_mission_item_reached()
hrt_abstime now = hrt_absolute_time();
if ((_navigator->get_land_detected()->landed == false)
&& !_waypoint_position_reached) {
if (!_navigator->get_land_detected()->landed && !_waypoint_position_reached) {
float dist = -1.0f;
float dist_xy = -1.0f;
@@ -375,8 +354,8 @@ MissionBlock::is_mission_item_reached()
}
/* check if the MAV was long enough inside the waypoint orbit */
if ((Navigator::get_time_inside(_mission_item) < FLT_EPSILON) ||
(now - _time_first_inside_orbit >= (hrt_abstime)(Navigator::get_time_inside(_mission_item) * 1e6f))) {
if ((get_time_inside(_mission_item) < FLT_EPSILON) ||
(now - _time_first_inside_orbit >= (hrt_abstime)(get_time_inside(_mission_item) * 1e6f))) {
// exit xtrack location
if (_mission_item.loiter_exit_xtrack &&
@@ -459,17 +438,17 @@ MissionBlock::issue_command(const struct mission_item_s *item)
if (item->nav_cmd == NAV_CMD_DO_SET_SERVO) {
PX4_INFO("do_set_servo command");
// XXX: we should issue a vehicle command and handle this somewhere else
memset(&actuators, 0, sizeof(actuators));
_actuators = {};
// params[0] actuator number to be set 0..5 (corresponds to AUX outputs 1..6)
// params[1] new value for selected actuator in ms 900...2000
actuators.control[(int)item->params[0]] = 1.0f / 2000 * -item->params[1];
actuators.timestamp = hrt_absolute_time();
_actuators.control[(int)item->params[0]] = 1.0f / 2000 * -item->params[1];
_actuators.timestamp = hrt_absolute_time();
if (_actuator_pub != nullptr) {
orb_publish(ORB_ID(actuator_controls_2), _actuator_pub, &actuators);
orb_publish(ORB_ID(actuator_controls_2), _actuator_pub, &_actuators);
} else {
_actuator_pub = orb_advertise(ORB_ID(actuator_controls_2), &actuators);
_actuator_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators);
}
} else {
@@ -478,15 +457,20 @@ MissionBlock::issue_command(const struct mission_item_s *item)
mission_item_to_vehicle_command(item, &cmd);
_action_start = hrt_absolute_time();
if (_cmd_pub != nullptr) {
orb_publish(ORB_ID(vehicle_command), _cmd_pub, &cmd);
} else {
_cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
}
_navigator->publish_vehicle_cmd(cmd);
}
}
float
MissionBlock::get_time_inside(const struct mission_item_s &item)
{
if (item.nav_cmd == NAV_CMD_TAKEOFF) {
return item.time_inside;
}
return 0.0f;
}
bool
MissionBlock::item_contains_position(const struct mission_item_s *item)
{
@@ -663,7 +647,6 @@ MissionBlock::set_follow_target_item(struct mission_item_s *item, float min_clea
item->nav_cmd = NAV_CMD_DO_FOLLOW_REPOSITION;
/* use current target position */
item->lat = target.lat;
item->lon = target.lon;
item->altitude = _navigator->get_home_position()->alt;
@@ -709,18 +692,15 @@ MissionBlock::set_land_item(struct mission_item_s *item, bool at_current_locatio
{
/* VTOL transition to RW before landing */
if (_navigator->get_vstatus()->is_vtol && !_navigator->get_vstatus()->is_rotary_wing &&
if (_navigator->get_vstatus()->is_vtol &&
!_navigator->get_vstatus()->is_rotary_wing &&
_param_force_vtol.get() == 1) {
struct vehicle_command_s cmd = {};
cmd.command = NAV_CMD_DO_VTOL_TRANSITION;
cmd.param1 = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
if (_cmd_pub != nullptr) {
orb_publish(ORB_ID(vehicle_command), _cmd_pub, &cmd);
} else {
_cmd_pub = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH);
}
_navigator->publish_vehicle_cmd(cmd);
}
/* set the land item */
@@ -732,9 +712,8 @@ MissionBlock::set_land_item(struct mission_item_s *item, bool at_current_locatio
item->lon = _navigator->get_global_position()->lon;
item->yaw = _navigator->get_local_position()->yaw;
/* use home position */
} else {
/* use home position */
item->lat = _navigator->get_home_position()->lat;
item->lon = _navigator->get_home_position()->lon;
item->yaw = _navigator->get_home_position()->yaw;