mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
Merge pull request #1518 from bansiesta/do_jump_feedback
DO_JUMP feedback in QGC
This commit is contained in:
commit
6a8ebc8afd
@ -81,6 +81,7 @@
|
||||
#include <uORB/topics/system_power.h>
|
||||
#include <uORB/topics/mission.h>
|
||||
#include <uORB/topics/mission_result.h>
|
||||
#include <uORB/topics/geofence_result.h>
|
||||
#include <uORB/topics/telemetry_status.h>
|
||||
#include <uORB/topics/vtol_vehicle_status.h>
|
||||
|
||||
@ -926,6 +927,11 @@ int commander_thread_main(int argc, char *argv[])
|
||||
struct mission_result_s mission_result;
|
||||
memset(&mission_result, 0, sizeof(mission_result));
|
||||
|
||||
/* Subscribe to geofence result topic */
|
||||
int geofence_result_sub = orb_subscribe(ORB_ID(geofence_result));
|
||||
struct geofence_result_s geofence_result;
|
||||
memset(&geofence_result, 0, sizeof(geofence_result));
|
||||
|
||||
/* Subscribe to manual control data */
|
||||
int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
|
||||
struct manual_control_setpoint_s sp_man;
|
||||
@ -1545,27 +1551,34 @@ int commander_thread_main(int argc, char *argv[])
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result);
|
||||
|
||||
/* Check for geofence violation */
|
||||
if (armed.armed && (mission_result.geofence_violated || mission_result.flight_termination)) {
|
||||
//XXX: make this configurable to select different actions (e.g. navigation modes)
|
||||
/* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */
|
||||
armed.force_failsafe = true;
|
||||
status_changed = true;
|
||||
static bool flight_termination_printed = false;
|
||||
|
||||
if (!flight_termination_printed) {
|
||||
warnx("Flight termination because of navigator request or geofence");
|
||||
mavlink_log_critical(mavlink_fd, "GF violation: flight termination");
|
||||
flight_termination_printed = true;
|
||||
}
|
||||
|
||||
if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
|
||||
mavlink_log_critical(mavlink_fd, "GF violation: flight termination");
|
||||
}
|
||||
} // no reset is done here on purpose, on geofence violation we want to stay in flighttermination
|
||||
}
|
||||
|
||||
/* start geofence result check */
|
||||
orb_check(geofence_result_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(geofence_result), geofence_result_sub, &geofence_result);
|
||||
}
|
||||
|
||||
/* Check for geofence violation */
|
||||
if (armed.armed && (geofence_result.geofence_violated || mission_result.flight_termination)) {
|
||||
//XXX: make this configurable to select different actions (e.g. navigation modes)
|
||||
/* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */
|
||||
armed.force_failsafe = true;
|
||||
status_changed = true;
|
||||
static bool flight_termination_printed = false;
|
||||
|
||||
if (!flight_termination_printed) {
|
||||
warnx("Flight termination because of navigator request or geofence");
|
||||
mavlink_log_critical(mavlink_fd, "GF violation: flight termination");
|
||||
flight_termination_printed = true;
|
||||
}
|
||||
|
||||
if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
|
||||
mavlink_log_critical(mavlink_fd, "GF violation: flight termination");
|
||||
}
|
||||
} // no reset is done here on purpose, on geofence violation we want to stay in flighttermination
|
||||
|
||||
/* RC input check */
|
||||
if (!status.rc_input_blocked && sp_man.timestamp != 0 &&
|
||||
hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) {
|
||||
|
||||
@ -292,9 +292,6 @@ MavlinkMissionManager::send_mission_item_reached(uint16_t seq)
|
||||
void
|
||||
MavlinkMissionManager::send(const hrt_abstime now)
|
||||
{
|
||||
/* update interval for slow rate limiter */
|
||||
_slow_rate_limiter.set_interval(_interval * 10 / _mavlink->get_rate_mult());
|
||||
|
||||
bool updated = false;
|
||||
orb_check(_mission_result_sub, &updated);
|
||||
|
||||
@ -312,6 +309,12 @@ MavlinkMissionManager::send(const hrt_abstime now)
|
||||
|
||||
send_mission_current(_current_seq);
|
||||
|
||||
if (mission_result.item_do_jump_changed) {
|
||||
/* send a mission item again if the remaining DO_JUMPs has changed */
|
||||
send_mission_item(_transfer_partner_sysid, _transfer_partner_compid,
|
||||
(uint16_t)mission_result.item_changed_index);
|
||||
}
|
||||
|
||||
} else {
|
||||
if (_slow_rate_limiter.check(now)) {
|
||||
send_mission_current(_current_seq);
|
||||
@ -811,7 +814,7 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
|
||||
|
||||
case NAV_CMD_DO_JUMP:
|
||||
mavlink_mission_item->param1 = mission_item->do_jump_mission_index;
|
||||
mavlink_mission_item->param2 = mission_item->do_jump_repeat_count;
|
||||
mavlink_mission_item->param2 = mission_item->do_jump_repeat_count - mission_item->do_jump_current_count;
|
||||
break;
|
||||
|
||||
default:
|
||||
|
||||
@ -155,7 +155,7 @@ DataLinkLoss::set_dll_item()
|
||||
case DLL_STATE_TERMINATE: {
|
||||
/* Request flight termination from the commander */
|
||||
_navigator->get_mission_result()->flight_termination = true;
|
||||
_navigator->publish_mission_result();
|
||||
_navigator->set_mission_result_updated();
|
||||
reset_mission_item_reached();
|
||||
warnx("not switched to manual: request flight termination");
|
||||
pos_sp_triplet->previous.valid = false;
|
||||
@ -188,7 +188,7 @@ DataLinkLoss::advance_dll()
|
||||
_navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get());
|
||||
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to airfield home");
|
||||
_navigator->get_mission_result()->stay_in_failsafe = true;
|
||||
_navigator->publish_mission_result();
|
||||
_navigator->set_mission_result_updated();
|
||||
reset_mission_item_reached();
|
||||
_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
|
||||
} else {
|
||||
@ -209,7 +209,7 @@ DataLinkLoss::advance_dll()
|
||||
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home");
|
||||
_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
|
||||
_navigator->get_mission_result()->stay_in_failsafe = true;
|
||||
_navigator->publish_mission_result();
|
||||
_navigator->set_mission_result_updated();
|
||||
reset_mission_item_reached();
|
||||
break;
|
||||
case DLL_STATE_FLYTOAIRFIELDHOMEWP:
|
||||
@ -217,7 +217,7 @@ DataLinkLoss::advance_dll()
|
||||
warnx("time is up, state should have been changed manually by now");
|
||||
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no manual control, terminating");
|
||||
_navigator->get_mission_result()->stay_in_failsafe = true;
|
||||
_navigator->publish_mission_result();
|
||||
_navigator->set_mission_result_updated();
|
||||
reset_mission_item_reached();
|
||||
break;
|
||||
case DLL_STATE_TERMINATE:
|
||||
|
||||
@ -141,7 +141,7 @@ GpsFailure::set_gpsf_item()
|
||||
case GPSF_STATE_TERMINATE: {
|
||||
/* Request flight termination from the commander */
|
||||
_navigator->get_mission_result()->flight_termination = true;
|
||||
_navigator->publish_mission_result();
|
||||
_navigator->set_mission_result_updated();
|
||||
warnx("gps fail: request flight termination");
|
||||
}
|
||||
default:
|
||||
|
||||
@ -38,6 +38,7 @@
|
||||
* @author Julian Oes <julian@oes.ch>
|
||||
* @author Thomas Gubler <thomasgubler@gmail.com>
|
||||
* @author Anton Babushkin <anton.babushkin@me.com>
|
||||
* @author Ban Siesta <bansiesta@gmail.com>
|
||||
*/
|
||||
|
||||
#include <sys/types.h>
|
||||
@ -149,18 +150,12 @@ Mission::on_active()
|
||||
|
||||
/* lets check if we reached the current mission item */
|
||||
if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) {
|
||||
set_mission_item_reached();
|
||||
if (_mission_item.autocontinue) {
|
||||
/* switch to next waypoint if 'autocontinue' flag set */
|
||||
advance_mission();
|
||||
set_mission_items();
|
||||
|
||||
} else {
|
||||
/* else just report that item reached */
|
||||
if (_mission_type == MISSION_TYPE_OFFBOARD) {
|
||||
if (!(_navigator->get_mission_result()->seq_reached == _current_offboard_mission_index && _navigator->get_mission_result()->reached)) {
|
||||
set_mission_item_reached();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} else if (_mission_type != MISSION_TYPE_NONE &&_param_altmode.get() == MISSION_ALTMODE_FOH) {
|
||||
@ -395,7 +390,6 @@ Mission::set_mission_items()
|
||||
/* reuse setpoint for LOITER only if it's not IDLE */
|
||||
_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER);
|
||||
|
||||
reset_mission_item_reached();
|
||||
set_mission_finished();
|
||||
|
||||
_navigator->set_position_setpoint_triplet_updated();
|
||||
@ -636,6 +630,8 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
|
||||
"ERROR DO JUMP waypoint could not be written");
|
||||
return false;
|
||||
}
|
||||
report_do_jump_mission_changed(*mission_index_ptr,
|
||||
mission_item_tmp.do_jump_repeat_count);
|
||||
}
|
||||
/* set new mission item index and repeat
|
||||
* we don't have to validate here, if it's invalid, we should realize this later .*/
|
||||
@ -706,23 +702,32 @@ Mission::save_offboard_mission_state()
|
||||
dm_unlock(DM_KEY_MISSION_STATE);
|
||||
}
|
||||
|
||||
void
|
||||
Mission::report_do_jump_mission_changed(int index, int do_jumps_remaining)
|
||||
{
|
||||
/* inform about the change */
|
||||
_navigator->get_mission_result()->item_do_jump_changed = true;
|
||||
_navigator->get_mission_result()->item_changed_index = index;
|
||||
_navigator->get_mission_result()->item_do_jump_remaining = do_jumps_remaining;
|
||||
_navigator->set_mission_result_updated();
|
||||
}
|
||||
|
||||
void
|
||||
Mission::set_mission_item_reached()
|
||||
{
|
||||
_navigator->get_mission_result()->reached = true;
|
||||
_navigator->get_mission_result()->seq_reached = _current_offboard_mission_index;
|
||||
_navigator->publish_mission_result();
|
||||
_navigator->set_mission_result_updated();
|
||||
reset_mission_item_reached();
|
||||
}
|
||||
|
||||
void
|
||||
Mission::set_current_offboard_mission_item()
|
||||
{
|
||||
warnx("current offboard mission index: %d", _current_offboard_mission_index);
|
||||
_navigator->get_mission_result()->reached = false;
|
||||
_navigator->get_mission_result()->finished = false;
|
||||
_navigator->get_mission_result()->seq_current = _current_offboard_mission_index;
|
||||
_navigator->publish_mission_result();
|
||||
_navigator->set_mission_result_updated();
|
||||
|
||||
save_offboard_mission_state();
|
||||
}
|
||||
@ -731,5 +736,5 @@ void
|
||||
Mission::set_mission_finished()
|
||||
{
|
||||
_navigator->get_mission_result()->finished = true;
|
||||
_navigator->publish_mission_result();
|
||||
_navigator->set_mission_result_updated();
|
||||
}
|
||||
|
||||
@ -38,6 +38,7 @@
|
||||
* @author Julian Oes <julian@oes.ch>
|
||||
* @author Thomas Gubler <thomasgubler@gmail.com>
|
||||
* @author Anton Babushkin <anton.babushkin@me.com>
|
||||
* @author Ban Siesta <bansiesta@gmail.com>
|
||||
*/
|
||||
|
||||
#ifndef NAVIGATOR_MISSION_H
|
||||
@ -130,6 +131,11 @@ private:
|
||||
*/
|
||||
void save_offboard_mission_state();
|
||||
|
||||
/**
|
||||
* Inform about a changed mission item after a DO_JUMP
|
||||
*/
|
||||
void report_do_jump_mission_changed(int index, int do_jumps_remaining);
|
||||
|
||||
/**
|
||||
* Set a mission item as reached
|
||||
*/
|
||||
|
||||
@ -54,6 +54,7 @@
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/mission_result.h>
|
||||
#include <uORB/topics/geofence_result.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
|
||||
#include "navigator_mode.h"
|
||||
@ -107,9 +108,9 @@ public:
|
||||
void load_fence_from_file(const char *filename);
|
||||
|
||||
/**
|
||||
* Publish the mission result so commander and mavlink know what is going on
|
||||
* Publish the geofence result
|
||||
*/
|
||||
void publish_mission_result();
|
||||
void publish_geofence_result();
|
||||
|
||||
/**
|
||||
* Publish the attitude sp, only to be used in very special modes when position control is deactivated
|
||||
@ -122,6 +123,7 @@ public:
|
||||
*/
|
||||
void set_can_loiter_at_sp(bool can_loiter) { _can_loiter_at_sp = can_loiter; }
|
||||
void set_position_setpoint_triplet_updated() { _pos_sp_triplet_updated = true; }
|
||||
void set_mission_result_updated() { _mission_result_updated = true; }
|
||||
|
||||
/**
|
||||
* Getters
|
||||
@ -134,6 +136,7 @@ public:
|
||||
struct home_position_s* get_home_position() { return &_home_pos; }
|
||||
struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; }
|
||||
struct mission_result_s* get_mission_result() { return &_mission_result; }
|
||||
struct geofence_result_s* get_geofence_result() { return &_geofence_result; }
|
||||
struct vehicle_attitude_setpoint_s* get_att_sp() { return &_att_sp; }
|
||||
|
||||
int get_onboard_mission_sub() { return _onboard_mission_sub; }
|
||||
@ -164,6 +167,7 @@ private:
|
||||
|
||||
orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */
|
||||
orb_advert_t _mission_result_pub;
|
||||
orb_advert_t _geofence_result_pub;
|
||||
orb_advert_t _att_sp_pub; /**< publish att sp
|
||||
used only in very special failsafe modes
|
||||
when pos control is deactivated */
|
||||
@ -179,7 +183,8 @@ private:
|
||||
position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */
|
||||
|
||||
mission_result_s _mission_result;
|
||||
vehicle_attitude_setpoint_s _att_sp;
|
||||
geofence_result_s _geofence_result;
|
||||
vehicle_attitude_setpoint_s _att_sp;
|
||||
|
||||
bool _mission_item_valid; /**< flags if the current mission item is valid */
|
||||
|
||||
@ -206,6 +211,7 @@ private:
|
||||
bool _can_loiter_at_sp; /**< flags if current position SP can be used to loiter */
|
||||
bool _pos_sp_triplet_updated; /**< flags if position SP triplet needs to be published */
|
||||
bool _pos_sp_triplet_published_invalid_once; /**< flags if position SP triplet has been published once to UORB */
|
||||
bool _mission_result_updated; /**< flags if mission result has seen an update */
|
||||
|
||||
control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */
|
||||
control::BlockParamFloat _param_acceptance_radius; /**< acceptance for takeoff */
|
||||
@ -271,6 +277,12 @@ private:
|
||||
*/
|
||||
void publish_position_setpoint_triplet();
|
||||
|
||||
|
||||
/**
|
||||
* Publish the mission result so commander and mavlink know what is going on
|
||||
*/
|
||||
void publish_mission_result();
|
||||
|
||||
/* this class has ptr data members, so it should not be copied,
|
||||
* consequently the copy constructors are private.
|
||||
*/
|
||||
|
||||
@ -110,6 +110,7 @@ Navigator::Navigator() :
|
||||
_param_update_sub(-1),
|
||||
_pos_sp_triplet_pub(-1),
|
||||
_mission_result_pub(-1),
|
||||
_geofence_result_pub(-1),
|
||||
_att_sp_pub(-1),
|
||||
_vstatus{},
|
||||
_control_mode{},
|
||||
@ -138,6 +139,7 @@ Navigator::Navigator() :
|
||||
_can_loiter_at_sp(false),
|
||||
_pos_sp_triplet_updated(false),
|
||||
_pos_sp_triplet_published_invalid_once(false),
|
||||
_mission_result_updated(false),
|
||||
_param_loiter_radius(this, "LOITER_RAD"),
|
||||
_param_acceptance_radius(this, "ACC_RAD"),
|
||||
_param_datalinkloss_obc(this, "DLL_OBC"),
|
||||
@ -398,8 +400,8 @@ Navigator::task_main()
|
||||
have_geofence_position_data = false;
|
||||
if (!inside) {
|
||||
/* inform other apps via the mission result */
|
||||
_mission_result.geofence_violated = true;
|
||||
publish_mission_result();
|
||||
_geofence_result.geofence_violated = true;
|
||||
publish_geofence_result();
|
||||
|
||||
/* Issue a warning about the geofence violation once */
|
||||
if (!_geofence_violation_warning_sent) {
|
||||
@ -408,8 +410,8 @@ Navigator::task_main()
|
||||
}
|
||||
} else {
|
||||
/* inform other apps via the mission result */
|
||||
_mission_result.geofence_violated = false;
|
||||
publish_mission_result();
|
||||
_geofence_result.geofence_violated = false;
|
||||
publish_geofence_result();
|
||||
/* Reset the _geofence_violation_warning_sent field */
|
||||
_geofence_violation_warning_sent = false;
|
||||
}
|
||||
@ -490,6 +492,11 @@ Navigator::task_main()
|
||||
_pos_sp_triplet_updated = false;
|
||||
}
|
||||
|
||||
if (_mission_result_updated) {
|
||||
publish_mission_result();
|
||||
_mission_result_updated = false;
|
||||
}
|
||||
|
||||
perf_end(_loop_perf);
|
||||
}
|
||||
warnx("exiting.");
|
||||
@ -639,6 +646,28 @@ Navigator::publish_mission_result()
|
||||
/* advertise and publish */
|
||||
_mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result);
|
||||
}
|
||||
|
||||
/* reset some of the flags */
|
||||
_mission_result.seq_reached = false;
|
||||
_mission_result.seq_current = 0;
|
||||
_mission_result.item_do_jump_changed = false;
|
||||
_mission_result.item_changed_index = 0;
|
||||
_mission_result.item_do_jump_remaining = 0;
|
||||
}
|
||||
|
||||
void
|
||||
Navigator::publish_geofence_result()
|
||||
{
|
||||
|
||||
/* lazily publish the geofence result only once available */
|
||||
if (_geofence_result_pub > 0) {
|
||||
/* publish mission result */
|
||||
orb_publish(ORB_ID(geofence_result), _geofence_result_pub, &_geofence_result);
|
||||
|
||||
} else {
|
||||
/* advertise and publish */
|
||||
_geofence_result_pub = orb_advertise(ORB_ID(geofence_result), &_geofence_result);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
|
||||
@ -65,7 +65,7 @@ NavigatorMode::run(bool active) {
|
||||
_first_run = false;
|
||||
/* Reset stay in failsafe flag */
|
||||
_navigator->get_mission_result()->stay_in_failsafe = false;
|
||||
_navigator->publish_mission_result();
|
||||
_navigator->set_mission_result_updated();
|
||||
on_activation();
|
||||
|
||||
} else {
|
||||
|
||||
@ -128,7 +128,7 @@ RCLoss::set_rcl_item()
|
||||
case RCL_STATE_TERMINATE: {
|
||||
/* Request flight termination from the commander */
|
||||
_navigator->get_mission_result()->flight_termination = true;
|
||||
_navigator->publish_mission_result();
|
||||
_navigator->set_mission_result_updated();
|
||||
warnx("rc not recovered: request flight termination");
|
||||
pos_sp_triplet->previous.valid = false;
|
||||
pos_sp_triplet->current.valid = false;
|
||||
@ -162,7 +162,7 @@ RCLoss::advance_rcl()
|
||||
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc loss, terminating");
|
||||
_rcl_state = RCL_STATE_TERMINATE;
|
||||
_navigator->get_mission_result()->stay_in_failsafe = true;
|
||||
_navigator->publish_mission_result();
|
||||
_navigator->set_mission_result_updated();
|
||||
reset_mission_item_reached();
|
||||
}
|
||||
break;
|
||||
@ -171,7 +171,7 @@ RCLoss::advance_rcl()
|
||||
warnx("time is up, no RC regain, terminating");
|
||||
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RC not regained, terminating");
|
||||
_navigator->get_mission_result()->stay_in_failsafe = true;
|
||||
_navigator->publish_mission_result();
|
||||
_navigator->set_mission_result_updated();
|
||||
reset_mission_item_reached();
|
||||
break;
|
||||
case RCL_STATE_TERMINATE:
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved.
|
||||
* Copyright (C) 2012-2014 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@ -148,6 +148,9 @@ ORB_DEFINE(onboard_mission, struct mission_s);
|
||||
#include "topics/mission_result.h"
|
||||
ORB_DEFINE(mission_result, struct mission_result_s);
|
||||
|
||||
#include "topics/geofence_result.h"
|
||||
ORB_DEFINE(geofence_result, struct geofence_result_s);
|
||||
|
||||
#include "topics/fence.h"
|
||||
ORB_DEFINE(fence, unsigned);
|
||||
|
||||
|
||||
65
src/modules/uORB/topics/geofence_result.h
Normal file
65
src/modules/uORB/topics/geofence_result.h
Normal file
@ -0,0 +1,65 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2014 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file geofence_result.h
|
||||
* Status of the plance concerning the geofence
|
||||
*
|
||||
* @author Ban Siesta <bansiesta@gmail.com>
|
||||
*/
|
||||
|
||||
#ifndef TOPIC_GEOFENCE_RESULT_H
|
||||
#define TOPIC_GEOFENCE_RESULT_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include "../uORB.h"
|
||||
|
||||
/**
|
||||
* @addtogroup topics
|
||||
* @{
|
||||
*/
|
||||
|
||||
struct geofence_result_s
|
||||
{
|
||||
bool geofence_violated; /**< true if the geofence is violated */
|
||||
};
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
/* register this as object request broker structure */
|
||||
ORB_DECLARE(geofence_result);
|
||||
|
||||
#endif
|
||||
@ -1,9 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* @author Julian Oes <joes@student.ethz.ch>
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
* Copyright (C) 2012-2014 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@ -37,6 +34,11 @@
|
||||
/**
|
||||
* @file mission_result.h
|
||||
* Mission results that navigator needs to pass on to commander and mavlink.
|
||||
*
|
||||
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* @author Julian Oes <joes@student.ethz.ch>
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
* @author Ban Siesta <bansiesta@gmail.com>
|
||||
*/
|
||||
|
||||
#ifndef TOPIC_MISSION_RESULT_H
|
||||
@ -58,8 +60,10 @@ struct mission_result_s
|
||||
bool reached; /**< true if mission has been reached */
|
||||
bool finished; /**< true if mission has been completed */
|
||||
bool stay_in_failsafe; /**< true if the commander should not switch out of the failsafe mode*/
|
||||
bool geofence_violated; /**< true if the geofence is violated */
|
||||
bool flight_termination; /**< true if the navigator demands a flight termination from the commander app */
|
||||
bool item_do_jump_changed; /**< true if the number of do jumps remaining has changed */
|
||||
unsigned item_changed_index; /**< indicate which item has changed */
|
||||
unsigned item_do_jump_remaining;/**< set to the number of do jumps remaining for that item */
|
||||
};
|
||||
|
||||
/**
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user