diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2013-12-28 10:04:13 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2013-12-28 10:04:13 +0400 |
commit | 7b7539fbbd256165603d545e2c4c99daaf719e3e (patch) | |
tree | 209a98f04a4093ec9d827a5ad61c1d4afc336d07 /src/modules/uORB/topics | |
parent | 153114aec8571a9105541b1fef473d36c4099519 (diff) | |
parent | 72d9c80ce954d2289282f5df01aef7e5e8914acc (diff) | |
download | px4-firmware-7b7539fbbd256165603d545e2c4c99daaf719e3e.tar.gz px4-firmware-7b7539fbbd256165603d545e2c4c99daaf719e3e.tar.bz2 px4-firmware-7b7539fbbd256165603d545e2c4c99daaf719e3e.zip |
Merge branch 'navigator_new' into navigator_new_vector, WIP
Diffstat (limited to 'src/modules/uORB/topics')
-rw-r--r-- | src/modules/uORB/topics/fence.h | 80 | ||||
-rw-r--r-- | src/modules/uORB/topics/home_position.h | 20 | ||||
-rw-r--r-- | src/modules/uORB/topics/mission.h | 52 | ||||
-rw-r--r-- | src/modules/uORB/topics/mission_item_triplet.h (renamed from src/modules/uORB/topics/vehicle_global_position_set_triplet.h) | 29 | ||||
-rw-r--r-- | src/modules/uORB/topics/mission_result.h (renamed from src/modules/uORB/topics/vehicle_global_position_setpoint.h) | 35 | ||||
-rw-r--r-- | src/modules/uORB/topics/navigation_capabilities.h | 5 | ||||
-rw-r--r-- | src/modules/uORB/topics/rc_channels.h | 11 | ||||
-rw-r--r-- | src/modules/uORB/topics/vehicle_control_mode.h | 1 | ||||
-rw-r--r-- | src/modules/uORB/topics/vehicle_status.h | 8 |
9 files changed, 165 insertions, 76 deletions
diff --git a/src/modules/uORB/topics/fence.h b/src/modules/uORB/topics/fence.h new file mode 100644 index 000000000..6f16c51cf --- /dev/null +++ b/src/modules/uORB/topics/fence.h @@ -0,0 +1,80 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Jean Cyr <jean.m.cyr@gmail.com> + * + * 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 fence.h + * Definition of geofence. + */ + +#ifndef TOPIC_FENCE_H_ +#define TOPIC_FENCE_H_ + +#include <stdint.h> +#include <stdbool.h> +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +#define GEOFENCE_MAX_VERTICES 16 + +/** + * This is the position of a geofence vertex + * + */ +struct fence_vertex_s { + // Worst case float precision gives us 2 meter resolution at the equator + float lat; /**< latitude in degrees */ + float lon; /**< longitude in degrees */ +}; + +/** + * This is the position of a geofence + * + */ +struct fence_s { + unsigned count; /**< number of actual vertices */ + struct fence_vertex_s vertices[GEOFENCE_MAX_VERTICES]; +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(fence); + +#endif diff --git a/src/modules/uORB/topics/home_position.h b/src/modules/uORB/topics/home_position.h index 7e1b82a0f..1cf37e29c 100644 --- a/src/modules/uORB/topics/home_position.h +++ b/src/modules/uORB/topics/home_position.h @@ -2,6 +2,7 @@ * * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. * Author: Lorenz Meier <lm@inf.ethz.ch> + * Julian Oes <joes@student.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,9 +35,10 @@ /** * @file home_position.h - * Definition of the GPS home position uORB topic. + * Definition of the home position uORB topic. * * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> */ #ifndef TOPIC_HOME_POSITION_H_ @@ -55,16 +57,12 @@ */ struct home_position_s { - uint64_t timestamp; /**< Timestamp (microseconds since system boot) */ - uint64_t time_gps_usec; /**< Timestamp (microseconds in GPS format), this is the timestamp from the gps module */ - - int32_t lat; /**< Latitude in 1E7 degrees */ - int32_t lon; /**< Longitude in 1E7 degrees */ - int32_t alt; /**< Altitude in 1E3 meters (millimeters) above MSL */ - float eph_m; /**< GPS HDOP horizontal dilution of position in m */ - float epv_m; /**< GPS VDOP horizontal dilution of position in m */ - float s_variance_m_s; /**< speed accuracy estimate m/s */ - float p_variance_m; /**< position accuracy estimate m */ + uint64_t timestamp; /**< Timestamp (microseconds since system boot) */ + + bool altitude_is_relative; + double lat; /**< Latitude in degrees */ + double lon; /**< Longitude in degrees */ + float altitude; /**< Altitude in meters */ }; /** diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index 978a3383a..dcdb234fa 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -35,8 +35,8 @@ ****************************************************************************/ /** - * @file mission_item.h - * Definition of one mission item. + * @file mission.h + * Definition of a mission consisting of mission items. */ #ifndef TOPIC_MISSION_H_ @@ -46,14 +46,24 @@ #include <stdbool.h> #include "../uORB.h" +#define NUM_MISSIONS_SUPPORTED 256 + +/* compatible to mavlink MAV_CMD */ enum NAV_CMD { - NAV_CMD_WAYPOINT = 0, - NAV_CMD_LOITER_TURN_COUNT, - NAV_CMD_LOITER_TIME_LIMIT, - NAV_CMD_LOITER_UNLIMITED, - NAV_CMD_RETURN_TO_LAUNCH, - NAV_CMD_LAND, - NAV_CMD_TAKEOFF + NAV_CMD_WAYPOINT=16, + NAV_CMD_LOITER_UNLIMITED=17, + NAV_CMD_LOITER_TURN_COUNT=18, + NAV_CMD_LOITER_TIME_LIMIT=19, + NAV_CMD_RETURN_TO_LAUNCH=20, + NAV_CMD_LAND=21, + NAV_CMD_TAKEOFF=22, + NAV_CMD_ROI=80, + NAV_CMD_PATHPLANNING=81 +}; + +enum ORIGIN { + ORIGIN_MAVLINK = 0, + ORIGIN_ONBOARD }; /** @@ -70,23 +80,26 @@ enum NAV_CMD { struct mission_item_s { bool altitude_is_relative; /**< true if altitude is relative from start point */ - double lat; /**< latitude in degrees * 1E7 */ - double lon; /**< longitude in degrees * 1E7 */ + double lat; /**< latitude in degrees */ + double lon; /**< longitude in degrees */ float altitude; /**< altitude in meters */ float yaw; /**< in radians NED -PI..+PI */ float loiter_radius; /**< loiter radius in meters, 0 for a VTOL to hover */ - uint8_t loiter_direction; /**< 1: positive / clockwise, -1, negative. */ - enum NAV_CMD nav_cmd; /**< true if loitering is enabled */ - float param1; - float param2; - float param3; - float param4; + int8_t loiter_direction; /**< 1: positive / clockwise, -1, negative. */ + enum NAV_CMD nav_cmd; /**< navigation command */ + float radius; /**< radius in which the mission is accepted as reached in meters */ + float time_inside; /**< time that the MAV should stay inside the radius before advancing in seconds */ + float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */ + bool autocontinue; /**< true if next waypoint should follow after this one */ + int index; /**< index matching the mavlink waypoint */ + enum ORIGIN origin; /**< where the waypoint has been generated */ }; struct mission_s { - struct mission_item_s *items; - unsigned count; + int dataman_id; /**< default -1, there are two offboard storage places in the dataman: 0 or 1 */ + unsigned count; /**< count of the missions stored in the datamanager */ + int current_index; /**< default -1, start at the one changed latest */ }; /** @@ -95,5 +108,6 @@ struct mission_s /* register this as object request broker structure */ ORB_DECLARE(mission); +ORB_DECLARE(onboard_mission); #endif diff --git a/src/modules/uORB/topics/vehicle_global_position_set_triplet.h b/src/modules/uORB/topics/mission_item_triplet.h index 8516b263f..b35eae607 100644 --- a/src/modules/uORB/topics/vehicle_global_position_set_triplet.h +++ b/src/modules/uORB/topics/mission_item_triplet.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 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> @@ -35,18 +35,18 @@ ****************************************************************************/ /** - * @file vehicle_global_position_setpoint.h + * @file mission_item_triplet.h * Definition of the global WGS84 position setpoint uORB topic. */ -#ifndef TOPIC_VEHICLE_GLOBAL_POSITION_SET_TRIPLET_H_ -#define TOPIC_VEHICLE_GLOBAL_POSITION_SET_TRIPLET_H_ +#ifndef TOPIC_MISSION_ITEM_TRIPLET_H_ +#define TOPIC_MISSION_ITEM_TRIPLET_H_ #include <stdint.h> #include <stdbool.h> #include "../uORB.h" -#include "vehicle_global_position_setpoint.h" +#include "mission.h" /** * @addtogroup topics @@ -58,14 +58,19 @@ * * This are the three next waypoints (or just the next two or one). */ -struct vehicle_global_position_set_triplet_s +struct mission_item_triplet_s { - bool previous_valid; /**< flag indicating previous position is valid */ - bool next_valid; /**< flag indicating next position is valid */ + bool previous_valid; + bool current_valid; /**< flag indicating previous mission item is valid */ + bool next_valid; /**< flag indicating next mission item is valid */ - struct vehicle_global_position_setpoint_s previous; - struct vehicle_global_position_setpoint_s current; - struct vehicle_global_position_setpoint_s next; + struct mission_item_s previous; + struct mission_item_s current; + struct mission_item_s next; + + int previous_index; + int current_index; + int next_index; }; /** @@ -73,6 +78,6 @@ struct vehicle_global_position_set_triplet_s */ /* register this as object request broker structure */ -ORB_DECLARE(vehicle_global_position_set_triplet); +ORB_DECLARE(mission_item_triplet); #endif diff --git a/src/modules/uORB/topics/vehicle_global_position_setpoint.h b/src/modules/uORB/topics/mission_result.h index a56434d3b..c99706b97 100644 --- a/src/modules/uORB/topics/vehicle_global_position_setpoint.h +++ b/src/modules/uORB/topics/mission_result.h @@ -35,45 +35,26 @@ ****************************************************************************/ /** - * @file vehicle_global_position_setpoint.h - * Definition of the global WGS84 position setpoint uORB topic. + * @file mission_result.h + * Mission results that navigator needs to pass on to commander and mavlink. */ -#ifndef TOPIC_VEHICLE_GLOBAL_POSITION_SETPOINT_H_ -#define TOPIC_VEHICLE_GLOBAL_POSITION_SETPOINT_H_ +#ifndef TOPIC_MISSION_RESULT_H +#define TOPIC_MISSION_RESULT_H #include <stdint.h> #include <stdbool.h> #include "../uORB.h" -#include "mission.h" /** * @addtogroup topics * @{ */ -/** - * Global position setpoint in WGS84 coordinates. - * - * This is the position the MAV is heading towards. If it of type loiter, - * the MAV is circling around it with the given loiter radius in meters. - */ -struct vehicle_global_position_setpoint_s +struct mission_result_s { - bool altitude_is_relative; /**< true if altitude is relative from start point */ - int32_t lat; /**< latitude in degrees * 1E7 */ - int32_t lon; /**< longitude in degrees * 1E7 */ - float altitude; /**< altitude in meters */ - float yaw; /**< in radians NED -PI..+PI */ - float loiter_radius; /**< loiter radius in meters, 0 for a VTOL to hover */ - int8_t loiter_direction; /**< 1: positive / clockwise, -1, negative. */ - enum NAV_CMD nav_cmd; /**< true if loitering is enabled */ - float param1; - float param2; - float param3; - float param4; - float turn_distance_xy; /**< The distance on the plane which will mark this as reached */ - float turn_distance_z; /**< The distance in Z direction which will mark this as reached */ + bool mission_reached; /**< true if mission has been reached */ + unsigned mission_index; /**< index of the mission which has been reached */ }; /** @@ -81,6 +62,6 @@ struct vehicle_global_position_setpoint_s */ /* register this as object request broker structure */ -ORB_DECLARE(vehicle_global_position_setpoint); +ORB_DECLARE(mission_result); #endif diff --git a/src/modules/uORB/topics/navigation_capabilities.h b/src/modules/uORB/topics/navigation_capabilities.h index 6a3e811e3..391756f99 100644 --- a/src/modules/uORB/topics/navigation_capabilities.h +++ b/src/modules/uORB/topics/navigation_capabilities.h @@ -53,6 +53,11 @@ */ struct navigation_capabilities_s { float turn_distance; /**< the optimal distance to a waypoint to switch to the next */ + + /* Landing parameters: see fw_pos_control_l1/landingslope.h */ + float landing_horizontal_slope_displacement; + float landing_slope_angle_rad; + float landing_flare_length; }; /** diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index 5a8580143..086b2ef15 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -1,9 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Nils Wenzler <wenzlern@student.ethz.ch> - * @author Ivan Ovinnikov <oivan@student.ethz.ch> - * @author Lorenz Meier <lm@inf.ethz.ch> + * Copyright (c) 2012, 2013 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 @@ -47,13 +44,13 @@ /** * The number of RC channel inputs supported. - * Current (Q1/2013) radios support up to 18 channels, + * Current (Q4/2013) radios support up to 18 channels, * leaving at a sane value of 15. * This number can be greater then number of RC channels, * because single RC channel can be mapped to multiple * functions, e.g. for various mode switches. */ -#define RC_CHANNELS_MAX 15 +#define RC_CHANNELS_MAPPED_MAX 15 /** * This defines the mapping of the RC functions. @@ -91,7 +88,7 @@ struct rc_channels_s { uint64_t timestamp_last_valid; /**< timestamp of last valid RC signal. */ struct { float scaled; /**< Scaled to -1..1 (throttle: 0..1) */ - } chan[RC_CHANNELS_MAX]; + } chan[RC_CHANNELS_MAPPED_MAX]; uint8_t chan_count; /**< number of valid channels */ /*String array to store the names of the functions*/ diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 38fb74c9b..e26fb97c8 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -78,6 +78,7 @@ struct vehicle_control_mode_s bool flag_control_position_enabled; /**< true if position is controlled */ bool flag_control_altitude_enabled; /**< true if altitude is controlled */ bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */ + bool flag_control_flighttermination_enabled; /**< true if flighttermination is enabled */ bool flag_control_auto_enabled; // TEMP uint8_t auto_state; // TEMP navigation state for AUTO modes diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 1c184d3a7..1c3763924 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -95,6 +95,12 @@ typedef enum { HIL_STATE_ON } hil_state_t; + +typedef enum { + FLIGHTTERMINATION_STATE_OFF = 0, + FLIGHTTERMINATION_STATE_ON +} flighttermination_state_t; + typedef enum { MODE_SWITCH_MANUAL = 0, MODE_SWITCH_ASSISTED, @@ -229,6 +235,8 @@ struct vehicle_status_s uint16_t errors_count2; uint16_t errors_count3; uint16_t errors_count4; + + flighttermination_state_t flighttermination_state; }; /** |