Browse Source

Merge pull request #1518 from bansiesta/do_jump_feedback

DO_JUMP feedback in QGC
sbg
Thomas Gubler 10 years ago
parent
commit
6a8ebc8afd
  1. 47
      src/modules/commander/commander.cpp
  2. 11
      src/modules/mavlink/mavlink_mission.cpp
  3. 8
      src/modules/navigator/datalinkloss.cpp
  4. 2
      src/modules/navigator/gpsfailure.cpp
  5. 29
      src/modules/navigator/mission.cpp
  6. 6
      src/modules/navigator/mission.h
  7. 18
      src/modules/navigator/navigator.h
  8. 37
      src/modules/navigator/navigator_main.cpp
  9. 2
      src/modules/navigator/navigator_mode.cpp
  10. 6
      src/modules/navigator/rcloss.cpp
  11. 5
      src/modules/uORB/objects_common.cpp
  12. 65
      src/modules/uORB/topics/geofence_result.h
  13. 14
      src/modules/uORB/topics/mission_result.h

47
src/modules/commander/commander.cpp

@ -81,6 +81,7 @@ @@ -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[]) @@ -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[]) @@ -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;
}
/* start geofence result check */
orb_check(geofence_result_sub, &updated);
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
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)) {

11
src/modules/mavlink/mavlink_mission.cpp

@ -292,9 +292,6 @@ MavlinkMissionManager::send_mission_item_reached(uint16_t seq) @@ -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) @@ -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 * @@ -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:

8
src/modules/navigator/datalinkloss.cpp

@ -155,7 +155,7 @@ DataLinkLoss::set_dll_item() @@ -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() @@ -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() @@ -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() @@ -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:

2
src/modules/navigator/gpsfailure.cpp

@ -141,7 +141,7 @@ GpsFailure::set_gpsf_item() @@ -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:

29
src/modules/navigator/mission.cpp

@ -38,6 +38,7 @@ @@ -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() @@ -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() @@ -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 @@ -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() @@ -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 @@ -731,5 +736,5 @@ void
Mission::set_mission_finished()
{
_navigator->get_mission_result()->finished = true;
_navigator->publish_mission_result();
_navigator->set_mission_result_updated();
}

6
src/modules/navigator/mission.h

@ -38,6 +38,7 @@ @@ -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: @@ -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
*/

18
src/modules/navigator/navigator.h

@ -54,6 +54,7 @@ @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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: @@ -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.
*/

37
src/modules/navigator/navigator_main.cpp

@ -110,6 +110,7 @@ Navigator::Navigator() : @@ -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() : @@ -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() @@ -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() @@ -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() @@ -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() @@ -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

2
src/modules/navigator/navigator_mode.cpp

@ -65,7 +65,7 @@ NavigatorMode::run(bool active) { @@ -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 {

6
src/modules/navigator/rcloss.cpp

@ -128,7 +128,7 @@ RCLoss::set_rcl_item() @@ -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() @@ -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() @@ -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:

5
src/modules/uORB/objects_common.cpp

@ -1,6 +1,6 @@ @@ -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); @@ -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

@ -0,0 +1,65 @@ @@ -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

14
src/modules/uORB/topics/mission_result.h

@ -1,9 +1,6 @@ @@ -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 @@ @@ -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 @@ -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…
Cancel
Save