From 39ab4f62ef8775c57786ca9422be4810a69331f4 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sun, 4 Aug 2013 15:56:35 +0900 Subject: [PATCH] Copter: add SPORT mode --- ArduCopter/ArduCopter.pde | 13 +++++++ ArduCopter/Attitude.pde | 82 +++++++++++++++++++++++++++++++++++++++ ArduCopter/Parameters.pde | 12 +++--- ArduCopter/config.h | 13 +++++++ ArduCopter/defines.h | 9 +++-- ArduCopter/events.pde | 3 ++ ArduCopter/fence.pde | 2 +- ArduCopter/motors.pde | 6 +-- ArduCopter/system.pde | 33 +++++++++++++++- 9 files changed, 157 insertions(+), 16 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 44c0b07590..dead53cdd5 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1637,6 +1637,7 @@ bool set_roll_pitch_mode(uint8_t new_roll_pitch_mode) case ROLL_PITCH_AUTO: case ROLL_PITCH_STABLE_OF: case ROLL_PITCH_TOY: + case ROLL_PITCH_SPORT: roll_pitch_initialised = true; break; @@ -1761,6 +1762,18 @@ void update_roll_pitch_mode(void) get_stabilize_roll(nav_roll); get_stabilize_pitch(nav_pitch); break; + + case ROLL_PITCH_SPORT: + // apply SIMPLE mode transform + if(ap.simple_mode && ap_system.new_radio_frame) { + update_simple_mode(); + } + // copy user input for reporting purposes + control_roll = g.rc_1.control_in; + control_pitch = g.rc_2.control_in; + get_roll_rate_stabilized_ef(g.rc_1.control_in); + get_pitch_rate_stabilized_ef(g.rc_2.control_in); + break; } #if FRAME_CONFIG != HELI_FRAME diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 1599163749..c3f2c4886e 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -233,6 +233,88 @@ get_yaw_rate_stabilized_bf(int32_t stick_angle) set_yaw_rate_target(0, BODY_EARTH_FRAME); } +// Roll with rate input and stabilized in the earth frame +static void +get_roll_rate_stabilized_ef(int32_t stick_angle) +{ + int32_t angle_error = 0; + + // convert the input to the desired roll rate + int32_t target_rate = stick_angle * g.acro_p - (roll_axis * g.acro_balance_roll)/100; + + // convert the input to the desired roll rate + roll_axis += target_rate * G_Dt; + roll_axis = wrap_180_cd(roll_axis); + + // ensure that we don't reach gimbal lock + if (labs(roll_axis) > 4500 && g.acro_trainer_enabled) { + roll_axis = constrain_int32(roll_axis, -4500, 4500); + angle_error = wrap_180_cd(roll_axis - ahrs.roll_sensor); + } else { + // angle error with maximum of +- max_angle_overshoot + angle_error = wrap_180_cd(roll_axis - ahrs.roll_sensor); + angle_error = constrain_int32(angle_error, -MAX_ROLL_OVERSHOOT, MAX_ROLL_OVERSHOOT); + } + +#if FRAME_CONFIG == HELI_FRAME + if (!motors.motor_runup_complete) { + angle_error = 0; + } +#else + // reset target angle to current angle if motors not spinning + if (!motors.armed() || g.rc_3.servo_out == 0) { + angle_error = 0; + } +#endif // HELI_FRAME + + // update roll_axis to be within max_angle_overshoot of our current heading + roll_axis = wrap_180_cd(angle_error + ahrs.roll_sensor); + + // set earth frame targets for rate controller + set_roll_rate_target(g.pi_stabilize_roll.get_p(angle_error) + target_rate, EARTH_FRAME); +} + +// Pitch with rate input and stabilized in the earth frame +static void +get_pitch_rate_stabilized_ef(int32_t stick_angle) +{ + int32_t angle_error = 0; + + // convert the input to the desired pitch rate + int32_t target_rate = stick_angle * g.acro_p - (pitch_axis * g.acro_balance_pitch)/100; + + // convert the input to the desired pitch rate + pitch_axis += target_rate * G_Dt; + pitch_axis = wrap_180_cd(pitch_axis); + + // ensure that we don't reach gimbal lock + if (labs(pitch_axis) > 4500) { + pitch_axis = constrain_int32(pitch_axis, -4500, 4500); + angle_error = wrap_180_cd(pitch_axis - ahrs.pitch_sensor); + } else { + // angle error with maximum of +- max_angle_overshoot + angle_error = wrap_180_cd(pitch_axis - ahrs.pitch_sensor); + angle_error = constrain_int32(angle_error, -MAX_PITCH_OVERSHOOT, MAX_PITCH_OVERSHOOT); + } + +#if FRAME_CONFIG == HELI_FRAME + if (!motors.motor_runup_complete) { + angle_error = 0; + } +#else + // reset target angle to current angle if motors not spinning + if (!motors.armed() || g.rc_3.servo_out == 0) { + angle_error = 0; + } +#endif // HELI_FRAME + + // update pitch_axis to be within max_angle_overshoot of our current heading + pitch_axis = wrap_180_cd(angle_error + ahrs.pitch_sensor); + + // set earth frame targets for rate controller + set_pitch_rate_target(g.pi_stabilize_pitch.get_p(angle_error) + target_rate, EARTH_FRAME); +} + // Yaw with rate input and stabilized in the earth frame static void get_yaw_rate_stabilized_ef(int32_t stick_angle) diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index 18925816a1..b4e76d7af1 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -308,42 +308,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:ToyA,12:ToyM + // @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:ToyA,12:ToyM,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:ToyA,12:ToyM + // @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:ToyA,12:ToyM,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:ToyA,12:ToyM + // @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:ToyA,12:ToyM,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:ToyA,12:ToyM + // @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:ToyA,12:ToyM,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:ToyA,12:ToyM + // @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:ToyA,12:ToyM,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:ToyA,12:ToyM + // @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:ToyA,12:ToyM,13:Sport // @User: Standard GSCALAR(flight_mode6, "FLTMODE6", FLIGHT_MODE_6), diff --git a/ArduCopter/config.h b/ArduCopter/config.h index b9438f931b..6ce63faa23 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -585,6 +585,19 @@ # define ACRO_THR THROTTLE_MANUAL #endif +// Sport Mode +#ifndef SPORT_YAW + # define SPORT_YAW YAW_ACRO +#endif + +#ifndef SPORT_RP + # define SPORT_RP ROLL_PITCH_SPORT +#endif + +#ifndef SPORT_THR + # define SPORT_THR THROTTLE_MANUAL +#endif + // Alt Hold Mode #ifndef ALT_HOLD_YAW # define ALT_HOLD_YAW YAW_HOLD diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index bda9f56f9e..ebe834fdbf 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -27,11 +27,12 @@ #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 +#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_LOITER 5 // pilot inputs the desired horizontal velocities +#define ROLL_PITCH_SPORT 6 // pilot inputs roll, pitch rotation rates in earth frame #define THROTTLE_MANUAL 0 // manual throttle mode - pilot input goes directly to motors #define THROTTLE_MANUAL_TILT_COMPENSATED 1 // mostly manual throttle but with some tilt compensation @@ -135,11 +136,11 @@ #define CIRCLE 7 // AUTO control #define POSITION 8 // 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_A 11 // THOR Enum for Toy mode #define TOY_M 12 // THOR Enum for Toy mode -#define NUM_MODES 13 +#define SPORT 13 // earth frame rate control +#define NUM_MODES 14 // CH_6 Tuning // ----------- diff --git a/ArduCopter/events.pde b/ArduCopter/events.pde index 1a9e005966..bc87fc97de 100644 --- a/ArduCopter/events.pde +++ b/ArduCopter/events.pde @@ -15,6 +15,7 @@ static void failsafe_radio_on_event() switch(control_mode) { case STABILIZE: case ACRO: + case SPORT: // if throttle is zero disarm motors if (g.rc_3.control_in == 0) { init_disarm_motors(); @@ -89,6 +90,7 @@ static void low_battery_event(void) switch(control_mode) { case STABILIZE: case ACRO: + case SPORT: // if throttle is zero disarm motors if (g.rc_3.control_in == 0) { init_disarm_motors(); @@ -217,6 +219,7 @@ static void failsafe_gcs_check() switch(control_mode) { case STABILIZE: case ACRO: + case SPORT: // if throttle is zero disarm motors if (g.rc_3.control_in == 0) { init_disarm_motors(); diff --git a/ArduCopter/fence.pde b/ArduCopter/fence.pde index ac90126ae5..6ecf2021e5 100644 --- a/ArduCopter/fence.pde +++ b/ArduCopter/fence.pde @@ -32,7 +32,7 @@ void fence_check() // disarm immediately if we think we are on the ground // don't disarm if the high-altitude fence has been broken because it's likely the user has pulled their throttle to zero to bring it down - if(control_mode <= ACRO && g.rc_3.control_in == 0 && !ap.failsafe_radio && ((fence.get_breaches() & AC_FENCE_TYPE_ALT_MAX)== 0)){ + if(manual_flight_mode(control_mode) && g.rc_3.control_in == 0 && !ap.failsafe_radio && ((fence.get_breaches() & AC_FENCE_TYPE_ALT_MAX)== 0)){ init_disarm_motors(); }else{ // if we have a GPS diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 07c1f4fd57..8260dffa4d 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -18,8 +18,8 @@ static void arm_motors_check() return; } - // allow arming/disarming in ACRO, STABILIZE and TOY flight modes - if (control_mode == ACRO || control_mode == STABILIZE || control_mode == TOY_A || control_mode == TOY_M) { + // allow arming/disarming in fully manual flight modes ACRO, STABILIZE, SPORT and TOY + if (manual_flight_mode(control_mode)) { allow_arming = true; } @@ -97,7 +97,7 @@ static void auto_disarm_check() { static uint8_t auto_disarming_counter; - if((control_mode <= ACRO) && (g.rc_3.control_in == 0) && motors.armed()) { + if(manual_flight_mode(control_mode) && (g.rc_3.control_in == 0) && motors.armed()) { auto_disarming_counter++; if(auto_disarming_counter == AUTO_DISARMING_DELAY) { diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 1d5ac2c94e..699426540c 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -342,9 +342,25 @@ static bool mode_requires_GPS(uint8_t mode) { return false; } +// manual_flight_mode - returns true if flight mode is completely manual (i.e. roll, pitch and yaw controlled by pilot) +static bool manual_flight_mode(uint8_t mode) { + switch(mode) { + case ACRO: + case STABILIZE: + case TOY_A: + case TOY_M: + case SPORT: + return true; + default: + return false; + } + + return false; +} + // set_mode - change flight mode and perform any necessary initialisation // returns true if mode was succesfully set -// STABILIZE, ACRO and LAND can always be set successfully but the return state of other flight modes should be checked and the caller should deal with failures appropriately +// STABILIZE, ACRO, SPORT and LAND can always be set successfully but the return state of other flight modes should be checked and the caller should deal with failures appropriately static bool set_mode(uint8_t mode) { // boolean to record if flight mode could be set @@ -505,6 +521,16 @@ static bool set_mode(uint8_t mode) set_throttle_mode(THROTTLE_HOLD); break; + case SPORT: + success = true; + ap.manual_throttle = true; + ap.manual_attitude = true; + set_yaw_mode(SPORT_YAW); + set_roll_pitch_mode(SPORT_RP); + set_throttle_mode(SPORT_THR); + set_nav_mode(NAV_NONE); + break; + default: success = false; break; @@ -547,7 +573,7 @@ static void update_auto_armed() return; } // if in stabilize or acro flight mode and throttle is zero, auto-armed should become false - if(control_mode <= ACRO && g.rc_3.control_in == 0 && !ap.failsafe_radio) { + if(manual_flight_mode(control_mode) && g.rc_3.control_in == 0 && !ap.failsafe_radio) { set_auto_armed(false); } }else{ @@ -668,6 +694,9 @@ print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode) case TOY_A: port->print_P(PSTR("TOY_A")); break; + case SPORT: + port->print_P(PSTR("SPORT")); + break; default: port->printf_P(PSTR("Mode(%u)"), (unsigned)mode); break;