Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-07 00:13:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: Drift Mode

Browse Source 
Changes Toy mode declarations to Drift mode.
Requires GPS, Mode 2 transmitter
Drift mode mixes Roll, Pitch and Yaw into a single stick on mode two transmitters.
This commit is contained in:
Jason Short 2013-11-13 09:57:48 -08:00 committed by Randy Mackay
parent 57b291b1c5
commit 371dc8c616
7 changed files with 96 additions and 81 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
ArduCopter/ArduCopter.pde
View File

@ -1374,7 +1374,7 @@ bool set_yaw_mode(uint8_t new_yaw_mode)
yaw_initialised = true; yaw_initialised = true;
} }
break; break;
case YAW_TOY: case YAW_DRIFT:
yaw_initialised = true; yaw_initialised = true;
break; break;
case YAW_RESETTOARMEDYAW: case YAW_RESETTOARMEDYAW:
@ -1493,12 +1493,12 @@ void update_yaw_mode(void)
get_look_ahead_yaw(g.rc_4.control_in); get_look_ahead_yaw(g.rc_4.control_in);
break; break;
case YAW_TOY: case YAW_DRIFT:
// if we are landed reset yaw target to current heading // if we have landed reset yaw target to current heading
if (ap.land_complete) { if (ap.land_complete) {
nav_yaw = ahrs.yaw_sensor; nav_yaw = ahrs.yaw_sensor;
} }
get_yaw_toy(); get_yaw_drift();
break; break;
case YAW_RESETTOARMEDYAW: case YAW_RESETTOARMEDYAW:
@ -1570,7 +1570,7 @@ bool set_roll_pitch_mode(uint8_t new_roll_pitch_mode)
break; break;
case ROLL_PITCH_AUTO: case ROLL_PITCH_AUTO:
case ROLL_PITCH_STABLE_OF: case ROLL_PITCH_STABLE_OF:
case ROLL_PITCH_TOY: case ROLL_PITCH_DRIFT:
case ROLL_PITCH_SPORT: case ROLL_PITCH_SPORT:
roll_pitch_initialised = true; roll_pitch_initialised = true;
break; break;
@ -1680,8 +1680,8 @@ void update_roll_pitch_mode(void)
get_stabilize_pitch(get_of_pitch(control_pitch)); get_stabilize_pitch(get_of_pitch(control_pitch));
break; break;
case ROLL_PITCH_TOY: case ROLL_PITCH_DRIFT:
get_roll_pitch_toy(); get_roll_pitch_drift();
break; break;
case ROLL_PITCH_LOITER: case ROLL_PITCH_LOITER:

14
ArduCopter/Parameters.pde
View File

@ -294,42 +294,42 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Param: FLTMODE1 // @Param: FLTMODE1
// @DisplayName: Flight Mode 1 // @DisplayName: Flight Mode 1
// @Description: Flight mode when Channel 5 pwm is <= 1230 // @Description: Flight mode when Channel 5 pwm is <= 1230
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:Toy,13:Sport // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:Drift,13:Sport
// @User: Standard // @User: Standard
GSCALAR(flight_mode1, "FLTMODE1", FLIGHT_MODE_1), GSCALAR(flight_mode1, "FLTMODE1", FLIGHT_MODE_1),
// @Param: FLTMODE2 // @Param: FLTMODE2
// @DisplayName: Flight Mode 2 // @DisplayName: Flight Mode 2
// @Description: Flight mode when Channel 5 pwm is >1230, <= 1360 // @Description: Flight mode when Channel 5 pwm is >1230, <= 1360
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:Toy,13:Sport // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:Drift,13:Sport
// @User: Standard // @User: Standard
GSCALAR(flight_mode2, "FLTMODE2", FLIGHT_MODE_2), GSCALAR(flight_mode2, "FLTMODE2", FLIGHT_MODE_2),
// @Param: FLTMODE3 // @Param: FLTMODE3
// @DisplayName: Flight Mode 3 // @DisplayName: Flight Mode 3
// @Description: Flight mode when Channel 5 pwm is >1360, <= 1490 // @Description: Flight mode when Channel 5 pwm is >1360, <= 1490
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:Toy,13:Sport // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:Drift,13:Sport
// @User: Standard // @User: Standard
GSCALAR(flight_mode3, "FLTMODE3", FLIGHT_MODE_3), GSCALAR(flight_mode3, "FLTMODE3", FLIGHT_MODE_3),
// @Param: FLTMODE4 // @Param: FLTMODE4
// @DisplayName: Flight Mode 4 // @DisplayName: Flight Mode 4
// @Description: Flight mode when Channel 5 pwm is >1490, <= 1620 // @Description: Flight mode when Channel 5 pwm is >1490, <= 1620
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:Toy,13:Sport // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:Drift,13:Sport
// @User: Standard // @User: Standard
GSCALAR(flight_mode4, "FLTMODE4", FLIGHT_MODE_4), GSCALAR(flight_mode4, "FLTMODE4", FLIGHT_MODE_4),
// @Param: FLTMODE5 // @Param: FLTMODE5
// @DisplayName: Flight Mode 5 // @DisplayName: Flight Mode 5
// @Description: Flight mode when Channel 5 pwm is >1620, <= 1749 // @Description: Flight mode when Channel 5 pwm is >1620, <= 1749
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:Toy,13:Sport // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:Drift,13:Sport
// @User: Standard // @User: Standard
GSCALAR(flight_mode5, "FLTMODE5", FLIGHT_MODE_5), GSCALAR(flight_mode5, "FLTMODE5", FLIGHT_MODE_5),
// @Param: FLTMODE6 // @Param: FLTMODE6
// @DisplayName: Flight Mode 6 // @DisplayName: Flight Mode 6
// @Description: Flight mode when Channel 5 pwm is >=1750 // @Description: Flight mode when Channel 5 pwm is >=1750
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:Toy,13:Sport // @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:Drift,13:Sport
// @User: Standard // @User: Standard
GSCALAR(flight_mode6, "FLTMODE6", FLIGHT_MODE_6), GSCALAR(flight_mode6, "FLTMODE6", FLIGHT_MODE_6),
@ -464,8 +464,6 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Group: SS4_ // @Group: SS4_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp // @Path: ../libraries/RC_Channel/RC_Channel.cpp
GGROUP(single_servo_4, "SS4_", RC_Channel), GGROUP(single_servo_4, "SS4_", RC_Channel),
#endif #endif

11
ArduCopter/defines.h
View File

@ -23,14 +23,14 @@
#define YAW_LOOK_AT_HOME 5 // point towards home (no pilot input accepted) #define YAW_LOOK_AT_HOME 5 // point towards home (no pilot input accepted)
#define YAW_LOOK_AT_HEADING 6 // point towards a particular angle (not pilot input accepted) #define YAW_LOOK_AT_HEADING 6 // point towards a particular angle (not pilot input accepted)
#define YAW_LOOK_AHEAD 7 // WARNING! CODE IN DEVELOPMENT NOT PROVEN #define YAW_LOOK_AHEAD 7 // WARNING! CODE IN DEVELOPMENT NOT PROVEN
#define YAW_TOY 8 // THOR This is the Yaw mode #define YAW_DRIFT 8 //
#define YAW_RESETTOARMEDYAW 9 // point towards heading at time motors were armed #define YAW_RESETTOARMEDYAW 9 // point towards heading at time motors were armed
#define ROLL_PITCH_STABLE 0 // pilot input roll, pitch angles #define ROLL_PITCH_STABLE 0 // pilot input roll, pitch angles
#define ROLL_PITCH_ACRO 1 // pilot inputs roll, pitch rotation rates in body frame #define ROLL_PITCH_ACRO 1 // pilot inputs roll, pitch rotation rates in body frame
#define ROLL_PITCH_AUTO 2 // no pilot input. autopilot roll, pitch is sent to stabilize controller inputs #define ROLL_PITCH_AUTO 2 // no pilot input. autopilot roll, pitch is sent to stabilize controller inputs
#define ROLL_PITCH_STABLE_OF 3 // pilot inputs roll, pitch angles which are mixed with optical flow based position controller lean anbles #define ROLL_PITCH_STABLE_OF 3 // pilot inputs roll, pitch angles which are mixed with optical flow based position controller lean anbles
#define ROLL_PITCH_TOY 4 // THOR This is the Roll and Pitch mode #define ROLL_PITCH_DRIFT 4 //
#define ROLL_PITCH_LOITER 5 // pilot inputs the desired horizontal velocities #define ROLL_PITCH_LOITER 5 // pilot inputs the desired horizontal velocities
#define ROLL_PITCH_SPORT 6 // pilot inputs roll, pitch rotation rates in earth frame #define ROLL_PITCH_SPORT 6 // pilot inputs roll, pitch rotation rates in earth frame
#define ROLL_PITCH_AUTOTUNE 7 // description of new roll-pitch mode #define ROLL_PITCH_AUTOTUNE 7 // description of new roll-pitch mode
@ -145,7 +145,7 @@
#define POSITION 8 // AUTO control #define POSITION 8 // AUTO control
#define LAND 9 // AUTO control #define LAND 9 // AUTO control
#define OF_LOITER 10 // Hold a single location using optical flow sensor #define OF_LOITER 10 // Hold a single location using optical flow sensor
#define TOY 11 // THOR Enum for Toy mode (Note: 12 is no longer used) #define DRIFT 11 // DRIFT mode (Note: 12 is no longer used)
#define SPORT 13 // earth frame rate control #define SPORT 13 // earth frame rate control
#define NUM_MODES 14 #define NUM_MODES 14
@ -209,11 +209,6 @@
#define WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL 2 // auto pilot will face next waypoint except when doing RTL at which time it will stay in it's last #define WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL 2 // auto pilot will face next waypoint except when doing RTL at which time it will stay in it's last
#define WP_YAW_BEHAVIOR_LOOK_AHEAD 3 // auto pilot will look ahead during missions and rtl (primarily meant for traditional helicotpers) #define WP_YAW_BEHAVIOR_LOOK_AHEAD 3 // auto pilot will look ahead during missions and rtl (primarily meant for traditional helicotpers)
// TOY mixing options
#define TOY_LOOKUP_TABLE 0
#define TOY_LINEAR_MIXER 1
#define TOY_EXTERNAL_MIXER 2
// Waypoint options // Waypoint options
#define MASK_OPTIONS_RELATIVE_ALT 1 #define MASK_OPTIONS_RELATIVE_ALT 1

52
ArduCopter/drift.pde Normal file
View File

@ -0,0 +1,52 @@
////////////////////////////////////////////////////////////////////////////////
// Drift Mode
////////////////////////////////////////////////////////////////////////////////
#define SPEEDGAIN 14.0
// The function call for managing the flight mode drift
static void
get_roll_pitch_drift()
{
}
static void
get_yaw_drift()
{
static float breaker = 0.0;
// convert pilot input to lean angles
// moved to Yaw since it is called before get_roll_pitch_drift();
get_pilot_desired_lean_angles(g.rc_1.control_in, g.rc_2.control_in, control_roll, control_pitch);
// Grab inertial velocity
Vector3f vel = inertial_nav.get_velocity();
// rotate roll, pitch input from north facing to vehicle's perspective
float roll_vel = vel.y * cos_yaw - vel.x * sin_yaw; // body roll vel
float pitch_vel = vel.y * sin_yaw + vel.x * cos_yaw; // body pitch vel
float pitch_vel2 = min(fabs(pitch_vel), 800);
// simple gain scheduling for yaw input
get_yaw_rate_stabilized_ef((float)(control_roll/2) * (1.0 - (pitch_vel2 / 2400.0)));
roll_vel = constrain_float(roll_vel, -322, 322);
pitch_vel = constrain_float(pitch_vel, -322, 322);
// always limit roll
get_stabilize_roll(roll_vel * -SPEEDGAIN);
if(control_pitch == 0){
// .14/ (.03 * 100) = 4.6 seconds till full breaking
breaker+= .03;
breaker = min(breaker, SPEEDGAIN);
// If we let go of sticks, bring us to a stop
get_stabilize_pitch(pitch_vel * breaker);
}else{
breaker = 0.0;
get_stabilize_pitch(control_pitch);
}
}

2
ArduCopter/motors.pde
View File

@ -18,7 +18,7 @@ static void arm_motors_check()
return; return;
} }
// allow arming/disarming in fully manual flight modes ACRO, STABILIZE, SPORT and TOY // allow arming/disarming in fully manual flight modes ACRO, STABILIZE, SPORT and DRIFT
if (manual_flight_mode(control_mode)) { if (manual_flight_mode(control_mode)) {
allow_arming = true; allow_arming = true;
} }

15
ArduCopter/system.pde
View File

@ -307,6 +307,7 @@ static bool mode_requires_GPS(uint8_t mode) {
case RTL: case RTL:
case CIRCLE: case CIRCLE:
case POSITION: case POSITION:
case DRIFT:
return true; return true;
default: default:
return false; return false;
@ -320,7 +321,7 @@ static bool manual_flight_mode(uint8_t mode) {
switch(mode) { switch(mode) {
case ACRO: case ACRO:
case STABILIZE: case STABILIZE:
case TOY: case DRIFT:
case SPORT: case SPORT:
return true; return true;
default: default:
@ -440,12 +441,12 @@ static bool set_mode(uint8_t mode)
} }
break; break;
case TOY: case DRIFT:
success = true; success = true;
set_yaw_mode(YAW_TOY); set_yaw_mode(YAW_DRIFT);
set_roll_pitch_mode(ROLL_PITCH_TOY); set_roll_pitch_mode(ROLL_PITCH_DRIFT);
set_nav_mode(NAV_NONE); set_nav_mode(NAV_NONE);
set_throttle_mode(THROTTLE_HOLD); set_throttle_mode(THROTTLE_MANUAL_TILT_COMPENSATED);
break; break;
case SPORT: case SPORT:
@ -592,8 +593,8 @@ print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
case OF_LOITER: case OF_LOITER:
port->print_P(PSTR("OF_LOITER")); port->print_P(PSTR("OF_LOITER"));
break; break;
case TOY: case DRIFT:
port->print_P(PSTR("TOY")); port->print_P(PSTR("DRIFT"));
break; break;
case SPORT: case SPORT:
port->print_P(PSTR("SPORT")); port->print_P(PSTR("SPORT"));

31
ArduCopter/toy.pde
View File

@ -1,31 +0,0 @@
////////////////////////////////////////////////////////////////////////////////
// Toy Mode
////////////////////////////////////////////////////////////////////////////////
static float toy_gain;
static void
get_yaw_toy()
{
// convert pilot input to lean angles
// moved to Yaw since it is called before get_roll_pitch_toy();
get_pilot_desired_lean_angles(g.rc_1.control_in, g.rc_2.control_in, control_roll, control_pitch);
// Gain scheduling for Yaw input -
// we reduce the yaw input based on the velocity of the copter
// 0 = full control, 600cm/s = 20% control
toy_gain = min(inertial_nav.get_velocity_xy(), 700);
toy_gain = 1.0 - (toy_gain / 800.0);
get_yaw_rate_stabilized_ef((float)control_roll * toy_gain);
}
// The function call for managing the flight mode Toy
static void
get_roll_pitch_toy()
{
// pass desired roll, pitch to stabilize attitude controllers
get_stabilize_roll((float)control_roll * (1.0 - toy_gain));
get_stabilize_pitch(control_pitch);
}