Copter: Drift Mode
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:
parent
57b291b1c5
commit
371dc8c616
@ -447,7 +447,7 @@ static struct {
|
||||
#define MOTOR_CLASS AP_MotorsOctaQuad
|
||||
#elif FRAME_CONFIG == HELI_FRAME
|
||||
#define MOTOR_CLASS AP_MotorsHeli
|
||||
#elif FRAME_CONFIG == SINGLE_FRAME
|
||||
#elif FRAME_CONFIG == SINGLE_FRAME
|
||||
#define MOTOR_CLASS AP_MotorsSingle
|
||||
#else
|
||||
#error Unrecognised frame type
|
||||
@ -457,7 +457,7 @@ static struct {
|
||||
static MOTOR_CLASS motors(&g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.rc_8, &g.heli_servo_1, &g.heli_servo_2, &g.heli_servo_3, &g.heli_servo_4);
|
||||
#elif FRAME_CONFIG == TRI_FRAME // tri constructor requires additional rc_7 argument to allow tail servo reversing
|
||||
static MOTOR_CLASS motors(&g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.rc_7);
|
||||
#elif FRAME_CONFIG == SINGLE_FRAME // single constructor requires extra servos for flaps
|
||||
#elif FRAME_CONFIG == SINGLE_FRAME // single constructor requires extra servos for flaps
|
||||
static MOTOR_CLASS motors(&g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.single_servo_1, &g.single_servo_2, &g.single_servo_3, &g.single_servo_4);
|
||||
#else
|
||||
static MOTOR_CLASS motors(&g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4);
|
||||
@ -1374,7 +1374,7 @@ bool set_yaw_mode(uint8_t new_yaw_mode)
|
||||
yaw_initialised = true;
|
||||
}
|
||||
break;
|
||||
case YAW_TOY:
|
||||
case YAW_DRIFT:
|
||||
yaw_initialised = true;
|
||||
break;
|
||||
case YAW_RESETTOARMEDYAW:
|
||||
@ -1493,12 +1493,12 @@ void update_yaw_mode(void)
|
||||
get_look_ahead_yaw(g.rc_4.control_in);
|
||||
break;
|
||||
|
||||
case YAW_TOY:
|
||||
// if we are landed reset yaw target to current heading
|
||||
case YAW_DRIFT:
|
||||
// if we have landed reset yaw target to current heading
|
||||
if (ap.land_complete) {
|
||||
nav_yaw = ahrs.yaw_sensor;
|
||||
}
|
||||
get_yaw_toy();
|
||||
get_yaw_drift();
|
||||
break;
|
||||
|
||||
case YAW_RESETTOARMEDYAW:
|
||||
@ -1570,7 +1570,7 @@ bool set_roll_pitch_mode(uint8_t new_roll_pitch_mode)
|
||||
break;
|
||||
case ROLL_PITCH_AUTO:
|
||||
case ROLL_PITCH_STABLE_OF:
|
||||
case ROLL_PITCH_TOY:
|
||||
case ROLL_PITCH_DRIFT:
|
||||
case ROLL_PITCH_SPORT:
|
||||
roll_pitch_initialised = true;
|
||||
break;
|
||||
@ -1680,8 +1680,8 @@ void update_roll_pitch_mode(void)
|
||||
get_stabilize_pitch(get_of_pitch(control_pitch));
|
||||
break;
|
||||
|
||||
case ROLL_PITCH_TOY:
|
||||
get_roll_pitch_toy();
|
||||
case ROLL_PITCH_DRIFT:
|
||||
get_roll_pitch_drift();
|
||||
break;
|
||||
|
||||
case ROLL_PITCH_LOITER:
|
||||
|
@ -294,42 +294,42 @@ const AP_Param::Info var_info[] PROGMEM = {
|
||||
// @Param: FLTMODE1
|
||||
// @DisplayName: Flight Mode 1
|
||||
// @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
|
||||
GSCALAR(flight_mode1, "FLTMODE1", FLIGHT_MODE_1),
|
||||
|
||||
// @Param: FLTMODE2
|
||||
// @DisplayName: Flight Mode 2
|
||||
// @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
|
||||
GSCALAR(flight_mode2, "FLTMODE2", FLIGHT_MODE_2),
|
||||
|
||||
// @Param: FLTMODE3
|
||||
// @DisplayName: Flight Mode 3
|
||||
// @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
|
||||
GSCALAR(flight_mode3, "FLTMODE3", FLIGHT_MODE_3),
|
||||
|
||||
// @Param: FLTMODE4
|
||||
// @DisplayName: Flight Mode 4
|
||||
// @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
|
||||
GSCALAR(flight_mode4, "FLTMODE4", FLIGHT_MODE_4),
|
||||
|
||||
// @Param: FLTMODE5
|
||||
// @DisplayName: Flight Mode 5
|
||||
// @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
|
||||
GSCALAR(flight_mode5, "FLTMODE5", FLIGHT_MODE_5),
|
||||
|
||||
// @Param: FLTMODE6
|
||||
// @DisplayName: Flight Mode 6
|
||||
// @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
|
||||
GSCALAR(flight_mode6, "FLTMODE6", FLIGHT_MODE_6),
|
||||
|
||||
@ -451,22 +451,20 @@ const AP_Param::Info var_info[] PROGMEM = {
|
||||
// @User: Standard
|
||||
GSCALAR(heli_yaw_ff, "RATE_YAW_FF", HELI_YAW_FF),
|
||||
#endif
|
||||
#if FRAME_CONFIG == SINGLE_FRAME
|
||||
// @Group: SS1_
|
||||
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
|
||||
GGROUP(single_servo_1, "SS1_", RC_Channel),
|
||||
// @Group: SS2_
|
||||
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
|
||||
GGROUP(single_servo_2, "SS2_", RC_Channel),
|
||||
// @Group: SS3_
|
||||
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
|
||||
GGROUP(single_servo_3, "SS3_", RC_Channel),
|
||||
// @Group: SS4_
|
||||
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
|
||||
GGROUP(single_servo_4, "SS4_", RC_Channel),
|
||||
|
||||
|
||||
#endif
|
||||
#if FRAME_CONFIG == SINGLE_FRAME
|
||||
// @Group: SS1_
|
||||
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
|
||||
GGROUP(single_servo_1, "SS1_", RC_Channel),
|
||||
// @Group: SS2_
|
||||
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
|
||||
GGROUP(single_servo_2, "SS2_", RC_Channel),
|
||||
// @Group: SS3_
|
||||
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
|
||||
GGROUP(single_servo_3, "SS3_", RC_Channel),
|
||||
// @Group: SS4_
|
||||
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
|
||||
GGROUP(single_servo_4, "SS4_", RC_Channel),
|
||||
#endif
|
||||
|
||||
|
||||
// RC channel
|
||||
@ -1050,8 +1048,8 @@ const AP_Param::Info var_info[] PROGMEM = {
|
||||
// @Group: SS4_
|
||||
// @Path: ../libraries/RC_Channel/RC_Channel.cpp
|
||||
GGROUP(single_servo_4, "SS4_", RC_Channel),
|
||||
// @Group: H_
|
||||
// @Path: ../libraries/AP_Motors/AP_MotorsHeli.cpp
|
||||
// @Group: H_
|
||||
// @Path: ../libraries/AP_Motors/AP_MotorsHeli.cpp
|
||||
GOBJECT(motors, "MOT_", AP_MotorsSingle),
|
||||
|
||||
#else
|
||||
|
@ -23,14 +23,14 @@
|
||||
#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_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 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_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_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_SPORT 6 // pilot inputs roll, pitch rotation rates in earth frame
|
||||
#define ROLL_PITCH_AUTOTUNE 7 // description of new roll-pitch mode
|
||||
@ -145,7 +145,7 @@
|
||||
#define POSITION 8 // AUTO control
|
||||
#define LAND 9 // AUTO control
|
||||
#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 NUM_MODES 14
|
||||
|
||||
@ -206,14 +206,9 @@
|
||||
// Yaw behaviours during missions - possible values for WP_YAW_BEHAVIOR parameter
|
||||
#define WP_YAW_BEHAVIOR_NONE 0 // auto pilot will never control yaw during missions or rtl (except for DO_CONDITIONAL_YAW command received)
|
||||
#define WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP 1 // auto pilot will face next waypoint or home during rtl
|
||||
#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)
|
||||
|
||||
// TOY mixing options
|
||||
#define TOY_LOOKUP_TABLE 0
|
||||
#define TOY_LINEAR_MIXER 1
|
||||
#define TOY_EXTERNAL_MIXER 2
|
||||
|
||||
|
||||
// Waypoint options
|
||||
#define MASK_OPTIONS_RELATIVE_ALT 1
|
||||
|
52
ArduCopter/drift.pde
Normal file
52
ArduCopter/drift.pde
Normal 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);
|
||||
}
|
||||
}
|
||||
|
@ -18,7 +18,7 @@ static void arm_motors_check()
|
||||
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)) {
|
||||
allow_arming = true;
|
||||
}
|
||||
|
@ -307,6 +307,7 @@ static bool mode_requires_GPS(uint8_t mode) {
|
||||
case RTL:
|
||||
case CIRCLE:
|
||||
case POSITION:
|
||||
case DRIFT:
|
||||
return true;
|
||||
default:
|
||||
return false;
|
||||
@ -320,7 +321,7 @@ static bool manual_flight_mode(uint8_t mode) {
|
||||
switch(mode) {
|
||||
case ACRO:
|
||||
case STABILIZE:
|
||||
case TOY:
|
||||
case DRIFT:
|
||||
case SPORT:
|
||||
return true;
|
||||
default:
|
||||
@ -440,12 +441,12 @@ static bool set_mode(uint8_t mode)
|
||||
}
|
||||
break;
|
||||
|
||||
case TOY:
|
||||
case DRIFT:
|
||||
success = true;
|
||||
set_yaw_mode(YAW_TOY);
|
||||
set_roll_pitch_mode(ROLL_PITCH_TOY);
|
||||
set_yaw_mode(YAW_DRIFT);
|
||||
set_roll_pitch_mode(ROLL_PITCH_DRIFT);
|
||||
set_nav_mode(NAV_NONE);
|
||||
set_throttle_mode(THROTTLE_HOLD);
|
||||
set_throttle_mode(THROTTLE_MANUAL_TILT_COMPENSATED);
|
||||
break;
|
||||
|
||||
case SPORT:
|
||||
@ -592,8 +593,8 @@ print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
|
||||
case OF_LOITER:
|
||||
port->print_P(PSTR("OF_LOITER"));
|
||||
break;
|
||||
case TOY:
|
||||
port->print_P(PSTR("TOY"));
|
||||
case DRIFT:
|
||||
port->print_P(PSTR("DRIFT"));
|
||||
break;
|
||||
case SPORT:
|
||||
port->print_P(PSTR("SPORT"));
|
||||
|
@ -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);
|
||||
}
|
||||
|
Loading…
Reference in New Issue
Block a user