From c5f19f5df883383a8cfb810ebaf723f2ad0795e2 Mon Sep 17 00:00:00 2001 From: Jochen Tuchbreiter Date: Sun, 13 Jan 2013 14:29:53 +0100 Subject: [PATCH] Plane: Implement issue 80: counterclockwise loiter - libraries/AP_Common/AP_Common.h: Use bit 2 of Location.options to store loiter direction - ArduPlane/defines.h: New bitmask MASK_OPTIONS_LOITER_DIRECTION for struct Location bit 2 - ArduPlane/ArduPlane.pde: New variable loiter_direction - ArduPlane/GCS_Mavlink.pde: For mavlink loiter-commands use sign of param3 to detemine direction. Set Location.option flag accordingly - ArduPlane/commands.pde: Make sure loiter-directions get saved into EEPROM correctly - ArduPlane/commands_logic.pde: Set loiter_direction on all loiter-actions as well as RTL/instant loiter - ArduPlane/navigation.pde: Yield loiter_direction in update_loiter --- ArduPlane/ArduPlane.pde | 3 +++ ArduPlane/GCS_Mavlink.pde | 24 +++++++++++++++++++++--- ArduPlane/commands.pde | 11 +++++------ ArduPlane/commands_logic.pde | 14 ++++++++++++++ ArduPlane/defines.h | 3 +++ ArduPlane/navigation.pde | 4 ++-- libraries/AP_Common/AP_Common.h | 2 +- 7 files changed, 49 insertions(+), 12 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 3ed291518e..af8eb0dbf5 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -472,6 +472,9 @@ static int32_t old_target_bearing_cd; // Total desired rotation in a loiter. Used for Loiter Turns commands. Degrees static int32_t loiter_total; +// Direction for loiter. 1 for clockwise, -1 for counter-clockwise +static int8_t loiter_direction = 1; + // The amount in degrees we have turned since recording old_target_bearing static int16_t loiter_delta; diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index e0d78eec42..d30052c608 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -1206,13 +1206,25 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields + case MAV_CMD_NAV_LOITER_TIME: case MAV_CMD_NAV_LOITER_TURNS: + if (tell_command.options & MASK_OPTIONS_LOITER_DIRECTION) { + param3 = -g.loiter_radius; + } else { + param3 = g.loiter_radius; + } case MAV_CMD_NAV_TAKEOFF: case MAV_CMD_DO_SET_HOME: - case MAV_CMD_NAV_LOITER_TIME: param1 = tell_command.p1; break; + case MAV_CMD_NAV_LOITER_UNLIM: + if (tell_command.options & MASK_OPTIONS_LOITER_DIRECTION) { + param3 = -g.loiter_radius;; + } else { + param3 = g.loiter_radius; + } + break; case MAV_CMD_CONDITION_CHANGE_ALT: x=0; // Clear fields loaded above that we don't want sent for this command y=0; @@ -1481,16 +1493,22 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // Switch to map APM command fields into MAVLink command fields switch (tell_command.id) { - case MAV_CMD_NAV_WAYPOINT: case MAV_CMD_NAV_LOITER_UNLIM: + if (packet.param3 < 0) { + tell_command.options |= MASK_OPTIONS_LOITER_DIRECTION; + } + case MAV_CMD_NAV_WAYPOINT: case MAV_CMD_NAV_RETURN_TO_LAUNCH: case MAV_CMD_NAV_LAND: break; case MAV_CMD_NAV_LOITER_TURNS: + case MAV_CMD_NAV_LOITER_TIME: + if (packet.param3 < 0) { + tell_command.options |= MASK_OPTIONS_LOITER_DIRECTION; + } case MAV_CMD_NAV_TAKEOFF: case MAV_CMD_DO_SET_HOME: - case MAV_CMD_NAV_LOITER_TIME: tell_command.p1 = packet.param1; break; diff --git a/ArduPlane/commands.pde b/ArduPlane/commands.pde index b67941b81c..fac5a5defa 100644 --- a/ArduPlane/commands.pde +++ b/ArduPlane/commands.pde @@ -102,13 +102,12 @@ static void set_cmd_with_index(struct Location temp, int16_t i) i = constrain_int16(i, 0, g.command_total.get()); uint16_t mem = WP_START_BYTE + (i * WP_SIZE); - // Set altitude options bitmask - // XXX What is this trying to do? - if ((temp.options & MASK_OPTIONS_RELATIVE_ALT) && i != 0) { - temp.options = MASK_OPTIONS_RELATIVE_ALT; - } else { - temp.options = 0; + // force home wp to absolute height + if (i == 0) { + temp.options &= ~(MASK_OPTIONS_RELATIVE_ALT); } + // zero unused bits + temp.options &= (MASK_OPTIONS_RELATIVE_ALT + MASK_OPTIONS_LOITER_DIRECTION); hal.storage->write_byte(mem, temp.id); diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde index b1c5b90259..2bc5c45e5d 100644 --- a/ArduPlane/commands_logic.pde +++ b/ArduPlane/commands_logic.pde @@ -229,6 +229,7 @@ static void do_RTL(void) control_mode = RTL; crash_timer = 0; next_WP = home; + loiter_direction = 1; // Altitude to hold over home // Set by configuration tool @@ -261,15 +262,26 @@ static void do_land() set_next_WP(&next_nav_command); } +static void loiter_set_direction_wp(struct Location *nav_command) +{ + if (nav_command->options & MASK_OPTIONS_LOITER_DIRECTION) { + loiter_direction = -1; + } else { + loiter_direction=1; + } +} + static void do_loiter_unlimited() { set_next_WP(&next_nav_command); + loiter_set_direction_wp(&next_nav_command); } static void do_loiter_turns() { set_next_WP(&next_nav_command); loiter_total = next_nav_command.p1 * 360; + loiter_set_direction_wp(&next_nav_command); } static void do_loiter_time() @@ -277,6 +289,7 @@ static void do_loiter_time() set_next_WP(&next_nav_command); loiter_time_ms = millis(); loiter_time_max_ms = next_nav_command.p1 * (uint32_t)1000; // units are seconds + loiter_set_direction_wp(&next_nav_command); } /********************************************************************************/ @@ -490,6 +503,7 @@ static bool verify_within_distance() static void do_loiter_at_location() { + loiter_direction = 1; next_WP = current_loc; } diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index 4359676aec..0514373693 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -85,6 +85,9 @@ enum FlightMode { //-------------------- #define MASK_OPTIONS_RELATIVE_ALT (1<<0) // 1 = Relative // altitude +#define MASK_OPTIONS_LOITER_DIRECTION (1<<2) // 0 = CW + // 1 = CCW + //repeating events #define NO_REPEAT 0 diff --git a/ArduPlane/navigation.pde b/ArduPlane/navigation.pde index db984cc3dd..347f9461e6 100644 --- a/ArduPlane/navigation.pde +++ b/ArduPlane/navigation.pde @@ -151,11 +151,11 @@ static void update_loiter() if(wp_distance <= (uint32_t)max(g.loiter_radius,0)) { power = float(wp_distance) / float(g.loiter_radius); power = constrain(power, 0.5, 1); - nav_bearing_cd += 9000.0 * (2.0 + power); + nav_bearing_cd += 9000.0 * (2.0 + power) * loiter_direction; } else if(wp_distance < (uint32_t)max((g.loiter_radius + LOITER_RANGE),0)) { power = -((float)(wp_distance - g.loiter_radius - LOITER_RANGE) / LOITER_RANGE); power = constrain(power, 0.5, 1); //power = constrain(power, 0, 1); - nav_bearing_cd -= power * 9000; + nav_bearing_cd -= power * 9000 * loiter_direction; } else{ update_crosstrack(); loiter_time_ms = millis(); // keep start time for loiter updating till we get within LOITER_RANGE of orbit diff --git a/libraries/AP_Common/AP_Common.h b/libraries/AP_Common/AP_Common.h index 2394bafff3..b50ab3d2a4 100644 --- a/libraries/AP_Common/AP_Common.h +++ b/libraries/AP_Common/AP_Common.h @@ -55,7 +55,7 @@ /// Data structures and types used throughout the libraries and applications. 0 = default /// bit 0: Altitude is stored 0: Absolute, 1: Relative /// bit 1: Chnage Alt between WP 0: Gradually, 1: ASAP -/// bit 2: +/// bit 2: Direction of loiter command 0: Clockwise 1: Counter-Clockwise /// bit 3: Req.to hit WP.alt to continue 0: No, 1: Yes /// bit 4: Relative to Home 0: No, 1: Yes /// bit 5: