From 520a535c2103fad7e225434f202ec997dcaae4dc Mon Sep 17 00:00:00 2001 From: Jason Short Date: Mon, 28 Oct 2013 21:28:27 -0700 Subject: [PATCH] Copter: TOY mode updates --- ArduCopter/ArduCopter.pde | 34 ++----- ArduCopter/Parameters.pde | 12 +-- ArduCopter/config.h | 6 -- ArduCopter/defines.h | 4 +- ArduCopter/motors.pde | 8 +- ArduCopter/navigation.pde | 14 +-- ArduCopter/radio.pde | 6 -- ArduCopter/system.pde | 38 +++----- ArduCopter/toy.pde | 184 +++++--------------------------------- 9 files changed, 51 insertions(+), 255 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 93880883ba..0ce2a5bba6 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -141,7 +141,7 @@ static AP_HAL::BetterStream* cliSerial; // N.B. we need to keep a static declaration which isn't guarded by macros -// at the top to cooperate with the prototype mangler. +// at the top to cooperate with the prototype mangler. //////////////////////////////////////////////////////////////////////////////// // AP_HAL instance @@ -644,8 +644,6 @@ static float target_sonar_alt; // desired altitude in cm above the ground // The altitude as reported by Baro in cm – Values can be quite high static int32_t baro_alt; -static int16_t saved_toy_throttle; - //////////////////////////////////////////////////////////////////////////////// // flight modes @@ -714,7 +712,6 @@ static uint32_t throttle_integrator; //////////////////////////////////////////////////////////////////////////////// // The Commanded Yaw from the autopilot. static int32_t nav_yaw; -static uint8_t yaw_timer; // Yaw will point at this location if yaw_mode is set to YAW_LOOK_AT_LOCATION static Vector3f yaw_look_at_WP; // bearing from current location to the yaw_look_at_WP @@ -863,7 +860,6 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = { { read_aux_switches, 10, 50 }, { arm_motors_check, 10, 10 }, { auto_trim, 10, 140 }, - { update_toy_throttle, 10, 50 }, { update_altitude, 10, 1000 }, { run_nav_updates, 10, 800 }, { three_hz_loop, 33, 90 }, @@ -947,7 +943,7 @@ static void compass_accumulate(void) { if (g.compass_enabled) { compass.accumulate(); - } + } } /* @@ -963,7 +959,7 @@ static void perf_update(void) if (g.log_bitmask & MASK_LOG_PM) Log_Write_Performance(); if (scheduler.debug()) { - cliSerial->printf_P(PSTR("PERF: %u/%u %lu\n"), + cliSerial->printf_P(PSTR("PERF: %u/%u %lu\n"), (unsigned)perf_info_get_num_long_running(), (unsigned)perf_info_get_num_loops(), (unsigned long)perf_info_get_max_time()); @@ -1079,10 +1075,6 @@ static void throttle_loop() // check if we've landed update_land_detector(); -#if TOY_EDF == ENABLED - edf_toy(); -#endif - // check auto_armed status update_auto_armed(); } @@ -1318,7 +1310,7 @@ static void update_GPS(void) if (camera.update_location(current_loc) == true) { do_take_picture(); } -#endif +#endif } // check for loss of gps @@ -1497,19 +1489,13 @@ void update_yaw_mode(void) get_look_ahead_yaw(g.rc_4.control_in); break; -#if TOY_LOOKUP == TOY_EXTERNAL_MIXER case YAW_TOY: // if we are landed reset yaw target to current heading if (ap.land_complete) { nav_yaw = ahrs.yaw_sensor; - }else{ - // update to allow external roll/yaw mixing - // keep heading always pointing at home with no pilot input allowed - nav_yaw = get_yaw_slew(nav_yaw, home_bearing, AUTO_YAW_SLEW_RATE); } - get_stabilize_yaw(nav_yaw); + get_yaw_toy(); break; -#endif case YAW_RESETTOARMEDYAW: // if we are landed reset yaw target to current heading @@ -1543,7 +1529,7 @@ uint8_t get_wp_yaw_mode(bool rtl) if( rtl ) { return YAW_HOLD; }else{ - return YAW_LOOK_AT_NEXT_WP; + return YAW_LOOK_AT_NEXT_WP; } break; @@ -1690,10 +1676,8 @@ void update_roll_pitch_mode(void) get_stabilize_pitch(get_of_pitch(control_pitch)); break; - // THOR - // a call out to the main toy logic case ROLL_PITCH_TOY: - roll_pitch_toy(); + get_roll_pitch_toy(); break; case ROLL_PITCH_LOITER: @@ -1885,7 +1869,7 @@ void update_throttle_mode(void) } else { motors.stab_throttle = false; } - + // allow swash collective to move if we are in manual throttle modes, even if disarmed if( !motors.armed() ) { if ( !(throttle_mode == THROTTLE_MANUAL) && !(throttle_mode == THROTTLE_MANUAL_TILT_COMPENSATED)){ @@ -1903,7 +1887,7 @@ void update_throttle_mode(void) set_target_alt_for_reporting(0); return; } - + #endif // HELI_FRAME switch(throttle_mode) { diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index 8b7b57ab10..9629935b8e 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:ToyA,12:ToyM,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:Toy,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,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:Toy,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,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:Toy,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,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:Toy,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,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:Toy,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,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:Toy,13:Sport // @User: Standard GSCALAR(flight_mode6, "FLTMODE6", FLIGHT_MODE_6), diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 5a5275c688..5edc40d3a6 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -99,12 +99,6 @@ #ifndef FRAME_ORIENTATION # define FRAME_ORIENTATION X_FRAME #endif -#ifndef TOY_EDF - # define TOY_EDF DISABLED -#endif -#ifndef TOY_MIXER - # define TOY_MIXER TOY_LINEAR_MIXER -#endif ///////////////////////////////////////////////////////////////////////////////// // TradHeli defaults diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 6f3c8cd327..01675f3212 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -144,11 +144,11 @@ #define POSITION 8 // AUTO control #define LAND 9 // AUTO control #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 TOY 11 // THOR Enum for Toy mode (Note: 12 is no longer used) #define SPORT 13 // earth frame rate control #define NUM_MODES 14 + // CH_6 Tuning // ----------- #define CH6_NONE 0 // no tuning performed diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 08fefde340..35f4124f83 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -33,7 +33,7 @@ static void arm_motors_check() arming_counter = 0; return; } - + #if FRAME_CONFIG == HELI_FRAME if ((motors.rsc_mode > 0) && (g.rc_8.control_in >= 10)){ arming_counter = 0; @@ -41,11 +41,7 @@ static void arm_motors_check() } #endif // HELI_FRAME -#if TOY_EDF == ENABLED - int16_t tmp = g.rc_1.control_in; -#else int16_t tmp = g.rc_4.control_in; -#endif // full right if (tmp > 4000) { @@ -233,7 +229,7 @@ static void pre_arm_checks(bool display_failure) } return; } - + // pre-arm check to ensure ch7 and ch8 have different functions if ((g.ch7_option != 0 || g.ch8_option != 0) && g.ch7_option == g.ch8_option) { if (display_failure) { diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 443e4f40af..7f8a9f1d9c 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -28,7 +28,7 @@ static void calc_position(){ static void calc_distance_and_bearing() { Vector3f curr = inertial_nav.get_position(); - + // get target from loiter or wpinav controller if( nav_mode == NAV_LOITER || nav_mode == NAV_CIRCLE ) { wp_distance = wp_nav.get_distance_to_target(); @@ -164,16 +164,6 @@ static void update_nav_mode() log_counter = 0; Log_Write_Nav_Tuning(); } - - /* - // To-Do: check that we haven't broken toy mode - case TOY_A: - case TOY_M: - set_nav_mode(NAV_NONE); - update_nav_wp(); - break; - } - */ } // Keeps old data out of our calculation / logs @@ -225,7 +215,7 @@ circle_set_center(const Vector3f current_position, float heading_in_radians) circle_angle = wrap_PI(heading_in_radians-PI); // calculate max velocity based on waypoint speed ensuring we do not use more than half our max acceleration for accelerating towards the center of the circle - max_velocity = min(wp_nav.get_horizontal_velocity(), safe_sqrt(0.5f*wp_nav.get_waypoint_acceleration()*g.circle_radius*100.0f)); + max_velocity = min(wp_nav.get_horizontal_velocity(), safe_sqrt(0.5f*wp_nav.get_waypoint_acceleration()*g.circle_radius*100.0f)); // angular_velocity in radians per second circle_angular_velocity_max = max_velocity/((float)g.circle_radius * 100.0f); diff --git a/ArduCopter/radio.pde b/ArduCopter/radio.pde index 85a6e0eb7d..c26411719e 100644 --- a/ArduCopter/radio.pde +++ b/ArduCopter/radio.pde @@ -92,12 +92,6 @@ static void init_rc_out() if (ap.pre_arm_rc_check) { output_min(); } - -#if TOY_EDF == ENABLED - // add access to CH8 and CH6 - APM_RC.enable_out(CH_8); - APM_RC.enable_out(CH_6); -#endif } // output_min - enable and output lowest possible value to motors diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index bb83324fd5..d3eab2e6c4 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -63,7 +63,7 @@ static void run_cli(AP_HAL::UARTDriver *port) motors.armed(false); motors.output(); } - + while (1) { main_menu.run(); } @@ -120,7 +120,7 @@ static void init_ardupilot() // report_version(); - relay.init(); + relay.init(); #if COPTER_LEDS == ENABLED copter_leds_init(); @@ -142,14 +142,14 @@ static void init_ardupilot() hal.scheduler->register_delay_callback(mavlink_delay_cb, 5); // we start by assuming USB connected, as we initialed the serial - // port with SERIAL0_BAUD. check_usb_mux() fixes this if need be. + // port with SERIAL0_BAUD. check_usb_mux() fixes this if need be. ap.usb_connected = true; check_usb_mux(); #if CONFIG_HAL_BOARD != HAL_BOARD_APM2 // we have a 2nd serial port for telemetry on all boards except // APM2. We actually do have one on APM2 but it isn't necessary as - // a MUX is used + // a MUX is used hal.uartC->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128); gcs3.init(hal.uartC); #endif @@ -303,14 +303,14 @@ static bool mode_requires_GPS(uint8_t mode) { switch(mode) { case AUTO: case GUIDED: - case LOITER: + case LOITER: case RTL: case CIRCLE: case POSITION: return true; default: return false; - } + } return false; } @@ -320,8 +320,7 @@ static bool manual_flight_mode(uint8_t mode) { switch(mode) { case ACRO: case STABILIZE: - case TOY_A: - case TOY_M: + case TOY: case SPORT: return true; default: @@ -441,21 +440,7 @@ static bool set_mode(uint8_t mode) } break; - // THOR - // These are the flight modes for Toy mode - // See the defines for the enumerated values - case TOY_A: - success = true; - set_yaw_mode(YAW_TOY); - set_roll_pitch_mode(ROLL_PITCH_TOY); - set_throttle_mode(THROTTLE_AUTO); - set_nav_mode(NAV_NONE); - - // save throttle for fast exit of Alt hold - saved_toy_throttle = g.rc_3.control_in; - break; - - case TOY_M: + case TOY: success = true; set_yaw_mode(YAW_TOY); set_roll_pitch_mode(ROLL_PITCH_TOY); @@ -607,11 +592,8 @@ print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode) case OF_LOITER: port->print_P(PSTR("OF_LOITER")); break; - case TOY_M: - port->print_P(PSTR("TOY_M")); - break; - case TOY_A: - port->print_P(PSTR("TOY_A")); + case TOY: + port->print_P(PSTR("TOY")); break; case SPORT: port->print_P(PSTR("SPORT")); diff --git a/ArduCopter/toy.pde b/ArduCopter/toy.pde index 38eaee41ef..f4f0187166 100644 --- a/ArduCopter/toy.pde +++ b/ArduCopter/toy.pde @@ -1,175 +1,31 @@ //////////////////////////////////////////////////////////////////////////////// -// Toy Mode - THOR +// Toy Mode //////////////////////////////////////////////////////////////////////////////// -static bool CH7_toy_flag; -#if TOY_MIXER == TOY_LOOKUP_TABLE -static const int16_t toy_lookup[] = { - 186, 373, 558, 745, - 372, 745, 1117, 1490, - 558, 1118, 1675, 2235, - 743, 1490, 2233, 2980, - 929, 1863, 2792, 3725, - 1115, 2235, 3350, 4470, - 1301, 2608, 3908, 4500, - 1487, 2980, 4467, 4500, - 1673, 3353, 4500, 4500 -}; -#endif +static float toy_gain; -//called at 10hz -void update_toy_throttle() +static void +get_yaw_toy() { - if(control_mode == TOY_A) { - // look for a change in throttle position to exit throttle hold - if(abs(g.rc_3.control_in - saved_toy_throttle) > 40) { - throttle_mode = THROTTLE_MANUAL; - } - - if(throttle_mode == THROTTLE_AUTO) { - update_toy_altitude(); - } - } -} - -#define TOY_ALT_SMALL 25 -#define TOY_ALT_LARGE 100 - -//called at 10hz -void update_toy_altitude() -{ - int16_t input = g.rc_3.radio_in; // throttle - float current_target_alt = wp_nav.get_desired_alt(); - //int16_t input = g.rc_7.radio_in; - - // Trigger upward alt change - if(false == CH7_toy_flag && input > 1666) { - CH7_toy_flag = true; - // go up - if(current_target_alt >= 400) { - wp_nav.set_desired_alt(current_target_alt + TOY_ALT_LARGE); - }else{ - wp_nav.set_desired_alt(current_target_alt + TOY_ALT_SMALL); - } - - // Trigger downward alt change - }else if(false == CH7_toy_flag && input < 1333) { - CH7_toy_flag = true; - // go down - if(current_target_alt >= (400 + TOY_ALT_LARGE)) { - wp_nav.set_desired_alt(current_target_alt - TOY_ALT_LARGE); - }else if(current_target_alt >= TOY_ALT_SMALL) { - wp_nav.set_desired_alt(current_target_alt - TOY_ALT_SMALL); - }else if(current_target_alt < TOY_ALT_SMALL) { - wp_nav.set_desired_alt(0); - } - - // clear flag - }else if (CH7_toy_flag && ((input < 1666) && (input > 1333))) { - CH7_toy_flag = false; - } + // 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(), 600); + toy_gain = 1.0 - (toy_gain / 750.0); + get_yaw_rate_stabilized_ef((float)control_roll * toy_gain); } -// called at 50 hz from all flight modes -#if TOY_EDF == ENABLED -void edf_toy() -{ - // EDF control: - g.rc_8.radio_out = 1000 + ((abs(g.rc_2.control_in) << 1) / 9); - if(g.rc_8.radio_out < 1050) - g.rc_8.radio_out = 1000; - - // output throttle to EDF - if(motors.armed()) { - APM_RC.OutputCh(CH_8, g.rc_8.radio_out); - }else{ - APM_RC.OutputCh(CH_8, 1000); - } - - // output Servo direction - if(g.rc_2.control_in > 0) { - APM_RC.OutputCh(CH_6, 1000); // 1000 : 2000 - }else{ - APM_RC.OutputCh(CH_6, 2000); // less than 1450 - } -} -#endif // The function call for managing the flight mode Toy -void roll_pitch_toy() +static void +get_roll_pitch_toy() { -#if TOY_MIXER == TOY_LOOKUP_TABLE || TOY_MIXER == TOY_LINEAR_MIXER - int16_t yaw_rate = g.rc_1.control_in / g.toy_yaw_rate; - - if(g.rc_1.control_in != 0) { // roll - get_acro_yaw(yaw_rate/2); - ap.yaw_stopped = false; - yaw_timer = 150; - - }else if (!ap.yaw_stopped) { - get_acro_yaw(0); - yaw_timer--; - - if((yaw_timer == 0) || (fabsf(omega.z) < 0.17f)) { - ap.yaw_stopped = true; - nav_yaw = ahrs.yaw_sensor; - } - }else{ - if(motors.armed() == false || g.rc_3.control_in == 0) - nav_yaw = ahrs.yaw_sensor; - - get_stabilize_yaw(nav_yaw); - } -#endif - - // roll_rate is the outcome of the linear equation or lookup table - // based on speed and Yaw rate - int16_t roll_rate = 0; - -#if TOY_MIXER == TOY_LOOKUP_TABLE - uint8_t xx, yy; - // Lookup value - //xx = g_gps->ground_speed / 200; - xx = abs(g.rc_2.control_in / 1000); - yy = abs(yaw_rate / 500); - - // constrain to lookup Array range - xx = constrain_int16(xx, 0, 3); - yy = constrain_int16(yy, 0, 8); - - roll_rate = toy_lookup[yy * 4 + xx]; - - if(yaw_rate == 0) { - roll_rate = 0; - }else if(yaw_rate < 0) { - roll_rate = -roll_rate; - } - - int16_t roll_limit = 4500 / g.toy_yaw_rate; - roll_rate = constrain_int16(roll_rate, -roll_limit, roll_limit); - -#elif TOY_MIXER == TOY_LINEAR_MIXER - roll_rate = -((int32_t)g.rc_2.control_in * (yaw_rate/100)) /30; - roll_rate = constrain_int32(roll_rate, -2000, 2000); - -#elif TOY_MIXER == TOY_EXTERNAL_MIXER - // JKR update to allow external roll/yaw mixing - roll_rate = g.rc_1.control_in; -#endif - -#if TOY_EDF == ENABLED - // Output the attitude - //g.rc_1.servo_out = get_stabilize_roll(roll_rate); - //g.rc_2.servo_out = get_stabilize_pitch(g.rc_6.control_in); // use CH6 to trim pitch - get_stabilize_roll(roll_rate); - get_stabilize_pitch(g.rc_6.control_in); // use CH6 to trim pitch - -#else - // Output the attitude - //g.rc_1.servo_out = get_stabilize_roll(roll_rate); - //g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in); - get_stabilize_roll(roll_rate); - get_stabilize_pitch(g.rc_2.control_in); -#endif - + // pass desired roll, pitch to stabilize attitude controllers + get_stabilize_roll((float)control_roll * (1.0 - toy_gain)); + get_stabilize_pitch(control_pitch); } +