diff options
| -rw-r--r-- | src/drivers/drv_rc_input.h | 2 | ||||
| -rw-r--r-- | src/drivers/px4io/px4io.cpp | 10 | ||||
| -rw-r--r-- | src/drivers/stm32/drv_hrt.c | 39 | ||||
| -rw-r--r-- | src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 38 | ||||
| -rw-r--r-- | src/modules/fw_pos_control_l1/landingslope.cpp | 14 | ||||
| -rw-r--r-- | src/modules/fw_pos_control_l1/landingslope.h | 33 | ||||
| -rw-r--r-- | src/modules/mavlink/waypoints.c | 2 | ||||
| -rw-r--r-- | src/modules/navigator/mission_feasibility_checker.cpp | 184 | ||||
| -rw-r--r-- | src/modules/navigator/mission_feasibility_checker.h | 82 | ||||
| -rw-r--r-- | src/modules/navigator/module.mk | 3 | ||||
| -rw-r--r-- | src/modules/navigator/navigator_main.cpp | 26 | ||||
| -rw-r--r-- | src/modules/px4iofirmware/controls.c | 16 | ||||
| -rw-r--r-- | src/modules/px4iofirmware/protocol.h | 3 | ||||
| -rw-r--r-- | src/modules/px4iofirmware/registers.c | 4 | ||||
| -rw-r--r-- | src/modules/px4iofirmware/sbus.c | 2 | ||||
| -rw-r--r-- | src/modules/systemlib/ppm_decode.h | 1 | ||||
| -rw-r--r-- | src/modules/uORB/topics/navigation_capabilities.h | 5 |
17 files changed, 415 insertions, 49 deletions
diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h index 6f773b82a..03f1dfbe5 100644 --- a/src/drivers/drv_rc_input.h +++ b/src/drivers/drv_rc_input.h @@ -89,7 +89,7 @@ struct rc_input_values { /** number of channels actually being seen */ uint32_t channel_count; - /** receive signal strength indicator (RSSI): < 0: Undefined, 0: no signal, 1000: full reception */ + /** receive signal strength indicator (RSSI): < 0: Undefined, 0: no signal, 255: full reception */ int32_t rssi; /** Input source */ diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 4e9daf910..9812ea497 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1801,6 +1801,16 @@ PX4IO::print_status() printf(" %u", io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + i)); printf("\n"); + + if (raw_inputs > 0) { + int frame_len = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_RC_DATA); + printf("RC data (PPM frame len) %u us\n", frame_len); + + if ((frame_len - raw_inputs * 2000 - 3000) < 0) { + printf("WARNING WARNING WARNING! This RC receiver does not allow safe frame detection.\n"); + } + } + uint16_t mapped_inputs = io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_VALID); printf("mapped R/C inputs 0x%04x", mapped_inputs); diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index f105251f0..5bfbe04b8 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -282,6 +282,10 @@ static void hrt_call_invoke(void); * Note that we assume that M3 means STM32F1 (since we don't really care about the F2). */ # ifdef CONFIG_ARCH_CORTEXM3 +# undef GTIM_CCER_CC1NP +# undef GTIM_CCER_CC2NP +# undef GTIM_CCER_CC3NP +# undef GTIM_CCER_CC4NP # define GTIM_CCER_CC1NP 0 # define GTIM_CCER_CC2NP 0 # define GTIM_CCER_CC3NP 0 @@ -332,10 +336,10 @@ static void hrt_call_invoke(void); /* * PPM decoder tuning parameters */ -# define PPM_MAX_PULSE_WIDTH 550 /* maximum width of a valid pulse */ +# define PPM_MAX_PULSE_WIDTH 700 /* maximum width of a valid pulse */ # define PPM_MIN_CHANNEL_VALUE 800 /* shortest valid channel signal */ # define PPM_MAX_CHANNEL_VALUE 2200 /* longest valid channel signal */ -# define PPM_MIN_START 2500 /* shortest valid start gap */ +# define PPM_MIN_START 2400 /* shortest valid start gap (only 2nd part of pulse) */ /* decoded PPM buffer */ #define PPM_MIN_CHANNELS 5 @@ -345,6 +349,7 @@ static void hrt_call_invoke(void); #define PPM_CHANNEL_LOCK 4 /* should be less than the input timeout */ __EXPORT uint16_t ppm_buffer[PPM_MAX_CHANNELS]; +__EXPORT uint16_t ppm_frame_length = 0; __EXPORT unsigned ppm_decoded_channels = 0; __EXPORT uint64_t ppm_last_valid_decode = 0; @@ -362,7 +367,8 @@ static uint16_t ppm_temp_buffer[PPM_MAX_CHANNELS]; struct { uint16_t last_edge; /* last capture time */ uint16_t last_mark; /* last significant edge */ - unsigned next_channel; + uint16_t frame_start; /* the frame width */ + unsigned next_channel; /* next channel index */ enum { UNSYNCH = 0, ARM, @@ -447,7 +453,6 @@ hrt_ppm_decode(uint32_t status) /* how long since the last edge? - this handles counter wrapping implicitely. */ width = count - ppm.last_edge; - ppm.last_edge = count; ppm_edge_history[ppm_edge_next++] = width; @@ -491,6 +496,7 @@ hrt_ppm_decode(uint32_t status) ppm_buffer[i] = ppm_temp_buffer[i]; ppm_last_valid_decode = hrt_absolute_time(); + } } @@ -500,13 +506,14 @@ hrt_ppm_decode(uint32_t status) /* next edge is the reference for the first channel */ ppm.phase = ARM; + ppm.last_edge = count; return; } switch (ppm.phase) { case UNSYNCH: /* we are waiting for a start pulse - nothing useful to do here */ - return; + break; case ARM: @@ -515,14 +522,23 @@ hrt_ppm_decode(uint32_t status) goto error; /* pulse was too long */ /* record the mark timing, expect an inactive edge */ - ppm.last_mark = count; - ppm.phase = INACTIVE; - return; + ppm.last_mark = ppm.last_edge; + + /* frame length is everything including the start gap */ + ppm_frame_length = (uint16_t)(ppm.last_edge - ppm.frame_start); + ppm.frame_start = ppm.last_edge; + ppm.phase = ACTIVE; + break; case INACTIVE: + + /* we expect a short pulse */ + if (width > PPM_MAX_PULSE_WIDTH) + goto error; /* pulse was too long */ + /* this edge is not interesting, but now we are ready for the next mark */ ppm.phase = ACTIVE; - return; + break; case ACTIVE: /* determine the interval from the last mark */ @@ -543,10 +559,13 @@ hrt_ppm_decode(uint32_t status) ppm_temp_buffer[ppm.next_channel++] = interval; ppm.phase = INACTIVE; - return; + break; } + ppm.last_edge = count; + return; + /* the state machine is corrupted; reset it */ error: diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index ae0f8c0ea..3784337ac 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -312,6 +312,11 @@ private: void vehicle_setpoint_poll(); /** + * Publish navigation capabilities + */ + void navigation_capabilities_publish(); + + /** * Control position. */ bool control_position(const math::Vector2f &global_pos, const math::Vector2f &ground_speed, @@ -538,6 +543,12 @@ FixedwingPositionControl::parameters_update() /* Update the landing slope */ landingslope.update(math::radians(_parameters.land_slope_angle), _parameters.land_flare_alt_relative, _parameters.land_thrust_lim_horizontal_distance, _parameters.land_H1_virt); + /* Update and publish the navigation capabilities */ + _nav_capabilities.landing_slope_angle_rad = landingslope.landing_slope_angle_rad(); + _nav_capabilities.landing_horizontal_slope_displacement = landingslope.horizontal_slope_displacement(); + _nav_capabilities.landing_flare_length = landingslope.flare_length(); + navigation_capabilities_publish(); + return OK; } @@ -709,6 +720,15 @@ FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector2f &cu } } +void FixedwingPositionControl::navigation_capabilities_publish() +{ + if (_nav_capabilities_pub > 0) { + orb_publish(ORB_ID(navigation_capabilities), _nav_capabilities_pub, &_nav_capabilities); + } else { + _nav_capabilities_pub = orb_advertise(ORB_ID(navigation_capabilities), &_nav_capabilities); + } +} + bool FixedwingPositionControl::control_position(const math::Vector2f ¤t_position, const math::Vector2f &ground_speed, const struct mission_item_triplet_s &mission_item_triplet) @@ -845,8 +865,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */ // XXX this could make a great param - float flare_angle_rad = -math::radians(5.0f);//math::radians(mission_item_triplet.current.param1) - float land_pitch_min = math::radians(5.0f); + float flare_pitch_angle_rad = -math::radians(5.0f);//math::radians(mission_item_triplet.current.param1) float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; float airspeed_land = 1.3f * _parameters.airspeed_min; float airspeed_approach = 1.3f * _parameters.airspeed_min; @@ -876,7 +895,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } - float flare_curve_alt = _mission_item_triplet.current.altitude + landingslope.H0() * expf(-math::max(0.0f, landingslope.flare_length() - wp_distance)/landingslope.flare_constant()) - landingslope.H1_virt(); + float flare_curve_alt = landingslope.getFlareCurveAltitude(wp_distance, _mission_item_triplet.current.altitude); /* avoid climbout */ if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground) @@ -887,9 +906,9 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, flare_curve_alt, calculate_target_airspeed(airspeed_land), _airspeed.indicated_airspeed_m_s, eas2tas, - false, flare_angle_rad, + false, flare_pitch_angle_rad, 0.0f, throttle_max, throttle_land, - flare_angle_rad, math::radians(15.0f)); + flare_pitch_angle_rad, math::radians(15.0f)); if (!land_noreturn_vertical) { mavlink_log_info(_mavlink_fd, "#audio: Landing, flaring"); @@ -904,7 +923,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* minimize speed to approach speed, stay on landing slope */ _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, landing_slope_alt_desired, calculate_target_airspeed(airspeed_approach), _airspeed.indicated_airspeed_m_s, eas2tas, - false, flare_angle_rad, + false, flare_pitch_angle_rad, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); //warnx("Landing: stay on slope, alt_desired: %.1f (wp_distance: %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f, d1 %.1f, flare_length %.1f", landing_slope_alt_desired, wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement, d1, flare_length); @@ -1215,11 +1234,8 @@ FixedwingPositionControl::task_main() /* set new turn distance */ _nav_capabilities.turn_distance = turn_distance; - if (_nav_capabilities_pub > 0) { - orb_publish(ORB_ID(navigation_capabilities), _nav_capabilities_pub, &_nav_capabilities); - } else { - _nav_capabilities_pub = orb_advertise(ORB_ID(navigation_capabilities), &_nav_capabilities); - } + navigation_capabilities_publish(); + } } diff --git a/src/modules/fw_pos_control_l1/landingslope.cpp b/src/modules/fw_pos_control_l1/landingslope.cpp index ecf51b740..b139a6397 100644 --- a/src/modules/fw_pos_control_l1/landingslope.cpp +++ b/src/modules/fw_pos_control_l1/landingslope.cpp @@ -1,9 +1,7 @@ /**************************************************************************** * * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier <lm@inf.ethz.ch> - * @author Thomas Gubler <thomasgubler@student.ethz.ch> - * @author Julian Oes <joes@student.ethz.ch> + * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -37,7 +35,6 @@ /* * @file: landingslope.cpp * - * @author: Thomas Gubler <thomasgubler@gmail.com> */ #include "landingslope.h" @@ -47,6 +44,7 @@ #include <errno.h> #include <math.h> #include <unistd.h> +#include <mathlib/mathlib.h> void Landingslope::update(float landing_slope_angle_rad, float flare_relative_alt, @@ -73,6 +71,12 @@ void Landingslope::calculateSlopeValues() float Landingslope::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude) { - return (wp_distance - _horizontal_slope_displacement) * tanf(_landing_slope_angle_rad) + wp_altitude; //flare_relative_alt is negative + return Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, wp_altitude, _horizontal_slope_displacement, _landing_slope_angle_rad); +} + +float Landingslope::getFlareCurveAltitude(float wp_landing_distance, float wp_landing_altitude) +{ + return wp_landing_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance)/_flare_constant) - _H1_virt; + } diff --git a/src/modules/fw_pos_control_l1/landingslope.h b/src/modules/fw_pos_control_l1/landingslope.h index f855b796f..8ff431509 100644 --- a/src/modules/fw_pos_control_l1/landingslope.h +++ b/src/modules/fw_pos_control_l1/landingslope.h @@ -1,9 +1,7 @@ /**************************************************************************** * * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier <lm@inf.ethz.ch> - * @author Thomas Gubler <thomasgubler@student.ethz.ch> - * @author Julian Oes <joes@student.ethz.ch> + * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -37,15 +35,18 @@ /* * @file: landingslope.h * - * @author: Thomas Gubler <thomasgubler@gmail.com> */ #ifndef LANDINGSLOPE_H_ #define LANDINGSLOPE_H_ +#include <math.h> +#include <systemlib/err.h> + class Landingslope { private: + //xxx: documentation of landing pending float _landing_slope_angle_rad; float _flare_relative_alt; float _motor_lim_horizontal_distance; @@ -62,7 +63,29 @@ public: Landingslope() {} ~Landingslope() {} - float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude); + float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_landing_altitude); + + /** + * + * @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance + */ + __EXPORT static float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_landing_altitude, float horizontal_slope_displacement, float landing_slope_angle_rad) + { + return (wp_landing_distance - horizontal_slope_displacement) * tanf(landing_slope_angle_rad) + wp_landing_altitude; //flare_relative_alt is negative + } + + /** + * + * @return distance to landing waypoint of point on landing slope at altitude=slope_altitude + */ + __EXPORT static float getLandingSlopeWPDistance(float slope_altitude, float wp_landing_altitude, float horizontal_slope_displacement, float landing_slope_angle_rad) + { + return (slope_altitude - wp_landing_altitude)/tanf(landing_slope_angle_rad) + horizontal_slope_displacement; + + } + + + float getFlareCurveAltitude(float wp_distance, float wp_altitude); void update(float landing_slope_angle_rad, float flare_relative_alt, diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index 741ea8aa4..59db898b9 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -62,7 +62,7 @@ orb_advert_t mission_pub = -1; struct mission_s mission; //#define MAVLINK_WPM_NO_PRINTF -#define MAVLINK_WPM_VERBOSE 1 +#define MAVLINK_WPM_VERBOSE 0 uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp new file mode 100644 index 000000000..25b2636bb --- /dev/null +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -0,0 +1,184 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * + * 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 mission_feasibility_checker.cpp + * Provides checks if mission is feasible given the navigation capabilities + */ + +#include "mission_feasibility_checker.h" + +#include <geo/geo.h> +#include <math.h> +#include <mathlib/mathlib.h> +#include <mavlink/mavlink_log.h> +#include <fw_pos_control_l1/landingslope.h> +#include <systemlib/err.h> +#include <stdio.h> +#include <fcntl.h> +#include <errno.h> + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capabilities_sub(-1), _initDone(false) +{ + _nav_caps = {0}; +} + + +bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nItems) +{ + /* Init if not done yet */ + init(); + + /* Open mavlink fd */ + if (_mavlink_fd < 0) { + /* try to open the mavlink log device every once in a while */ + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + } + + + if (isRotarywing) + return checkMissionFeasibleRotarywing(dm_current, nItems); + else + return checkMissionFeasibleFixedwing(dm_current, nItems); +} + +bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nItems) +{ + + return checkGeofence(dm_current, nItems); +} + +bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nItems) +{ + /* Update fixed wing navigation capabilites */ + 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); + + return (checkFixedWingLanding(dm_current, nItems) && checkGeofence(dm_current, nItems)); +} + +bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nItems) +{ + //xxx: check geofence + return true; +} + +bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nItems) +{ + /* 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 */ + + + for (size_t i = 0; i < nItems; i++) { + static struct mission_item_s missionitem; + const ssize_t len = sizeof(struct mission_item_s); + if (dm_read(dm_current, i, &missionitem, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + return false; + } + + if (missionitem.nav_cmd == NAV_CMD_LAND) { + struct mission_item_s missionitem_previous; + if (i != 0) { + if (dm_read(dm_current, i-1, &missionitem_previous, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + return false; + } + + float wp_distance = get_distance_to_next_waypoint(missionitem_previous.lat , missionitem_previous.lon, missionitem.lat, missionitem.lon); + float slope_alt_req = Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, missionitem.altitude, _nav_caps.landing_horizontal_slope_displacement, _nav_caps.landing_slope_angle_rad); + float wp_distance_req = Landingslope::getLandingSlopeWPDistance(missionitem_previous.altitude, missionitem.altitude, _nav_caps.landing_horizontal_slope_displacement, _nav_caps.landing_slope_angle_rad); + float delta_altitude = missionitem.altitude - missionitem_previous.altitude; +// warnx("wp_distance %.2f, delta_altitude %.2f, missionitem_previous.altitude %.2f, missionitem.altitude %.2f, slope_alt_req %.2f, wp_distance_req %.2f", +// wp_distance, delta_altitude, missionitem_previous.altitude, missionitem.altitude, slope_alt_req, wp_distance_req); +// warnx("_nav_caps.landing_horizontal_slope_displacement %.4f, _nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_flare_length %.4f", +// _nav_caps.landing_horizontal_slope_displacement, _nav_caps.landing_slope_angle_rad, _nav_caps.landing_flare_length); + + if (wp_distance > _nav_caps.landing_flare_length) { + /* Last wp is before flare region */ + + if (delta_altitude < 0) { + if (missionitem_previous.altitude <= slope_alt_req) { + /* Landing waypoint is at or below altitude of slope at the given waypoint distance: this is ok, aircraft will intersect the slope */ + return true; + } else { + /* Landing waypoint is above altitude of slope at the given waypoint distance */ + mavlink_log_info(_mavlink_fd, "#audio: Landing: last waypoint too high/too close"); + mavlink_log_info(_mavlink_fd, "Move down to %.1fm or move further away by %.1fm", + (double)(slope_alt_req), + (double)(wp_distance_req - wp_distance)); + return false; + } + } else { + /* Landing waypoint is above last waypoint */ + mavlink_log_info(_mavlink_fd, "#audio: Landing waypoint above last nav waypoint"); + return false; + } + } else { + /* Last wp is in flare region */ + //xxx give recommendations + mavlink_log_info(_mavlink_fd, "#audio: Warning: Landing: last waypoint in flare region"); + return false; + } + } else { + mavlink_log_info(_mavlink_fd, "#audio: Warning: starting with land waypoint"); + return false; + } + } + } + + +// float slope_alt = wp_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_distance)/_flare_constant) - _H1_virt; +} + +void MissionFeasibilityChecker::updateNavigationCapabilities() +{ + int res = orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); +} + +void MissionFeasibilityChecker::init() +{ + if (!_initDone) { + + _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); + + _initDone = true; + } +} diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h new file mode 100644 index 000000000..7d1cc2f8a --- /dev/null +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -0,0 +1,82 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * + * 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 mission_feasibility_checker.h + * Provides checks if mission is feasible given the navigation capabilities + */ +#ifndef MISSION_FEASIBILITY_CHECKER_H_ +#define MISSION_FEASIBILITY_CHECKER_H_ + +#include <unistd.h> +#include <uORB/topics/mission.h> +#include <uORB/topics/navigation_capabilities.h> +#include <dataman/dataman.h> + + +class MissionFeasibilityChecker +{ +private: + int _mavlink_fd; + + int _capabilities_sub; + struct navigation_capabilities_s _nav_caps; + + bool _initDone; + void init(); + + /* Checks for all airframes */ + bool checkGeofence(dm_item_t dm_current, size_t nItems); + + /* Checks specific to fixedwing airframes */ + bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nItems); + bool checkFixedWingLanding(dm_item_t dm_current, size_t nItems); + void updateNavigationCapabilities(); + + /* Checks specific to rotarywing airframes */ + bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nItems); +public: + + MissionFeasibilityChecker(); + ~MissionFeasibilityChecker() {} + + /* + * Returns true if mission is feasible and false otherwise + */ + bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nItems); + +}; + + +#endif /* MISSION_FEASIBILITY_CHECKER_H_ */ diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index fc59c956a..6be4e87a0 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -39,6 +39,7 @@ MODULE_COMMAND = navigator SRCS = navigator_main.cpp \ navigator_params.c \ - navigator_mission.cpp + navigator_mission.cpp \ + mission_feasibility_checker.cpp INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 48f828ff7..c7ac885b4 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -76,6 +76,7 @@ #include <mavlink/mavlink_log.h> #include "navigator_mission.h" +#include "mission_feasibility_checker.h" /* oddly, ERROR is not defined for c++ */ @@ -165,6 +166,8 @@ private: bool _waypoint_yaw_reached; uint64_t _time_first_inside_orbit; + MissionFeasibilityChecker missionFeasiblityChecker; + struct { float min_altitude; float loiter_radius; @@ -228,7 +231,7 @@ private: /** * Retrieve offboard mission. */ - void offboard_mission_update(); + void offboard_mission_update(bool isrotaryWing); /** * Retrieve onboard mission. @@ -435,11 +438,21 @@ Navigator::navigation_capabilities_update() void -Navigator::offboard_mission_update() +Navigator::offboard_mission_update(bool isrotaryWing) { struct mission_s offboard_mission; if (orb_copy(ORB_ID(mission), _offboard_mission_sub, &offboard_mission) == OK) { + /* Check mission feasibility, for now do not handle the return value, + * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ + dm_item_t dm_current; + if (offboard_mission.dataman_id == 0) { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; + } + missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count); + _mission.set_offboard_dataman_id(offboard_mission.dataman_id); _mission.set_current_offboard_mission_index(offboard_mission.current_index); _mission.set_offboard_mission_count(offboard_mission.count); @@ -503,13 +516,14 @@ Navigator::task_main() /* copy all topics first time */ + vehicle_status_update(); parameters_update(); global_position_update(); home_position_update(); navigation_capabilities_update(); - offboard_mission_update(); + offboard_mission_update(_vstatus.is_rotary_wing); onboard_mission_update(); - vehicle_status_update(); + /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vstatus_sub, 200); @@ -632,7 +646,7 @@ Navigator::task_main() } if (fds[4].revents & POLLIN) { - offboard_mission_update(); + offboard_mission_update(_vstatus.is_rotary_wing); // XXX check if mission really changed dispatch(EVENT_MISSION_CHANGED); } @@ -1311,4 +1325,4 @@ Navigator::add_home_pos_to_rtl(struct mission_item_s *new_mission_item) new_mission_item->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number new_mission_item->radius = 50.0f; // TODO: get rid of magic number } -}
\ No newline at end of file +} diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index f630b6f2a..541eed0e1 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -50,7 +50,7 @@ #define RC_CHANNEL_HIGH_THRESH 5000 #define RC_CHANNEL_LOW_THRESH -5000 -static bool ppm_input(uint16_t *values, uint16_t *num_values); +static bool ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len); static perf_counter_t c_gather_dsm; static perf_counter_t c_gather_sbus; @@ -94,7 +94,7 @@ controls_tick() { * other. Don't do that. */ - /* receive signal strenght indicator (RSSI). 0 = no connection, 1000: perfect connection */ + /* receive signal strenght indicator (RSSI). 0 = no connection, 255: perfect connection */ uint16_t rssi = 0; perf_begin(c_gather_dsm); @@ -108,7 +108,7 @@ controls_tick() { else r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RC_DSM11; - rssi = 1000; + rssi = 255; } perf_end(c_gather_dsm); @@ -125,11 +125,11 @@ controls_tick() { * disable the PPM decoder completely if we have S.bus signal. */ perf_begin(c_gather_ppm); - bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count); + bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_status[PX4IO_P_STATUS_RC_DATA]); if (ppm_updated) { /* XXX sample RSSI properly here */ - rssi = 1000; + rssi = 255; r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM; } @@ -321,7 +321,7 @@ controls_tick() { } static bool -ppm_input(uint16_t *values, uint16_t *num_values) +ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len) { bool result = false; @@ -345,6 +345,10 @@ ppm_input(uint16_t *values, uint16_t *num_values) /* clear validity */ ppm_last_valid_decode = 0; + /* store PPM frame length */ + if (num_values) + *frame_len = ppm_frame_length; + /* good if we got any channels */ result = (*num_values > 0); } diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index c10f0167c..e5bef6eb3 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -128,7 +128,8 @@ #define PX4IO_P_STATUS_VSERVO 6 /* [2] servo rail voltage in mV */ #define PX4IO_P_STATUS_VRSSI 7 /* [2] RSSI voltage */ #define PX4IO_P_STATUS_PRSSI 8 /* [2] RSSI PWM value */ -#define PX4IO_P_STATUS_NRSSI 9 /* [2] Normalized RSSI value, 0: no reception, 1000: perfect reception */ +#define PX4IO_P_STATUS_NRSSI 9 /* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */ +#define PX4IO_P_STATUS_RC_DATA 10 /* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */ /* array of post-mix actuator outputs, -10000..10000 */ #define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */ diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 916b893c4..7ef1aa309 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -89,7 +89,9 @@ uint16_t r_page_status[] = { [PX4IO_P_STATUS_IBATT] = 0, [PX4IO_P_STATUS_VSERVO] = 0, [PX4IO_P_STATUS_VRSSI] = 0, - [PX4IO_P_STATUS_PRSSI] = 0 + [PX4IO_P_STATUS_PRSSI] = 0, + [PX4IO_P_STATUS_NRSSI] = 0, + [PX4IO_P_STATUS_RC_DATA] = 0 }; /** diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 6aca2bd11..11ccd7356 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -280,7 +280,7 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint *rssi = 100; // XXX magic number indicating bad signal, but not a signal loss (yet) } - *rssi = 1000; + *rssi = 255; return true; } diff --git a/src/modules/systemlib/ppm_decode.h b/src/modules/systemlib/ppm_decode.h index 6c5e15345..5a1ad84da 100644 --- a/src/modules/systemlib/ppm_decode.h +++ b/src/modules/systemlib/ppm_decode.h @@ -57,6 +57,7 @@ __BEGIN_DECLS * PPM decoder state */ __EXPORT extern uint16_t ppm_buffer[PPM_MAX_CHANNELS]; /**< decoded PPM channel values */ +__EXPORT extern uint16_t ppm_frame_length; /**< length of the decoded PPM frame (includes gap) */ __EXPORT extern unsigned ppm_decoded_channels; /**< count of decoded channels */ __EXPORT extern hrt_abstime ppm_last_valid_decode; /**< timestamp of the last valid decode */ 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; }; /** |