diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 301bf8cf5f..455930dcf7 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -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: diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index 84bfcc630e..72ece20c71 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -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 diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index d6c0046255..aef7efdc1c 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -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 diff --git a/ArduCopter/drift.pde b/ArduCopter/drift.pde new file mode 100644 index 0000000000..0d0bf2ed97 --- /dev/null +++ b/ArduCopter/drift.pde @@ -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); + } +} + diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index d3b4cdacc9..3ad043aacf 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -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; } diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 74f7b1d495..d493146baf 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -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")); diff --git a/ArduCopter/toy.pde b/ArduCopter/toy.pde deleted file mode 100644 index 550989de02..0000000000 --- a/ArduCopter/toy.pde +++ /dev/null @@ -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); -} -