mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-03-11 17:13:56 -03:00
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
This commit is contained in:
Jochen Tuchbreiter
Andrew Tridgell
parent
fa7839941f
commit
c5f19f5df8
@ -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;
|
||||
|
||||
|
||||
@ -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;
|
||||
|
||||
|
||||
@ -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);
|
||||
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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:
|
||||
|
||||
Loading…
Reference in New Issue
Block a user