diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 31d427146a..423c1cfd7f 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -296,6 +296,7 @@ static boolean motor_auto_armed; // if true, //int max_stabilize_dampener; // //int max_yaw_dampener; // static Vector3f omega; +float tuning_value; // LED output // ---------- @@ -507,7 +508,7 @@ static byte simple_timer; // for limiting the execution of flight mode thi static unsigned long dTnav; // Delta Time in milliseconds for navigation computations static unsigned long nav_loopTimer; // used to track the elapsed ime for GPS nav -static float load; // % MCU cycles used +//static float load; // % MCU cycles used static byte counter_one_herz; static bool GPS_enabled = false; @@ -530,7 +531,7 @@ void loop() //PORTK |= B00010000; delta_ms_fast_loop = millis() - fast_loopTimer; fast_loopTimer = millis(); - load = float(fast_loopTimeStamp - fast_loopTimer) / delta_ms_fast_loop; + //load = float(fast_loopTimeStamp - fast_loopTimer) / delta_ms_fast_loop; G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by DCM integrator mainLoop_count++; @@ -907,16 +908,15 @@ static void slow_loop() // between 1 and 5 Hz #else gcs.send_message(MSG_LOCATION); - gcs.send_message(MSG_CPU_LOAD, load*100); + //gcs.send_message(MSG_CPU_LOAD, load*100); #endif #if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0) hil.data_stream_send(1,5); #endif - #if CHANNEL_6_TUNING != CH6_NONE + if(g.radio_tuning > 0) tuning(); - #endif // filter out the baro offset. //if(baro_alt_offset > 0) baro_alt_offset--; @@ -1301,7 +1301,7 @@ static void read_AHRS(void) hil.update(); #endif - dcm.update_DCM(G_Dt); + dcm.update_DCM(G_Dt);//, _tog); omega = dcm.get_gyro(); } @@ -1396,63 +1396,65 @@ adjust_altitude() } static void tuning(){ + tuning_value = (float)g.rc_6.control_in / 1000.0; - //Outer Loop : Attitude - #if CHANNEL_6_TUNING == CH6_STABILIZE_KP - g.pi_stabilize_roll.kP((float)g.rc_6.control_in / 1000.0); - g.pi_stabilize_pitch.kP((float)g.rc_6.control_in / 1000.0); + switch(g.radio_tuning){ + case CH6_STABILIZE_KP: + g.rc_6.set_range(0,8000); // 0 to 8 + g.pi_stabilize_roll.kP(tuning_value); + g.pi_stabilize_pitch.kP(tuning_value); + break; - #elif CHANNEL_6_TUNING == CH6_STABILIZE_KI - g.pi_stabilize_roll.kI((float)g.rc_6.control_in / 1000.0); - g.pi_stabilize_pitch.kI((float)g.rc_6.control_in / 1000.0); + case CH6_STABILIZE_KI: + g.rc_6.set_range(0,300); // 0 to .3 + tuning_value = (float)g.rc_6.control_in / 1000.0; + g.pi_stabilize_roll.kI(tuning_value); + g.pi_stabilize_pitch.kI(tuning_value); + break; - #elif CHANNEL_6_TUNING == CH6_YAW_KP - g.pi_stabilize_yaw.kP((float)g.rc_6.control_in / 1000.0); // range from 0.0 ~ 5.0 + case CH6_RATE_KP: + g.rc_6.set_range(0,300); // 0 to .3 + g.pi_rate_roll.kP(tuning_value); + g.pi_rate_pitch.kP(tuning_value); + break; - #elif CHANNEL_6_TUNING == CH6_YAW_KI - g.pi_stabilize_yaw.kI((float)g.rc_6.control_in / 1000.0); + case CH6_RATE_KI: + g.rc_6.set_range(0,300); // 0 to .3 + g.pi_rate_roll.kI(tuning_value); + g.pi_rate_pitch.kI(tuning_value); + break; + case CH6_YAW_KP: + g.rc_6.set_range(0,1000); + g.pi_stabilize_yaw.kP(tuning_value); + break; - //Inner Loop : Rate - #elif CHANNEL_6_TUNING == CH6_RATE_KP - g.pi_rate_roll.kP((float)g.rc_6.control_in / 1000.0); - g.pi_rate_pitch.kP((float)g.rc_6.control_in / 1000.0); + case CH6_YAW_RATE_KP: + g.rc_6.set_range(0,1000); + g.pi_rate_yaw.kP(tuning_value); + break; - #elif CHANNEL_6_TUNING == CH6_RATE_KI - g.pi_rate_roll.kI((float)g.rc_6.control_in / 1000.0); - g.pi_rate_pitch.kI((float)g.rc_6.control_in / 1000.0); + case CH6_THROTTLE_KP: + g.rc_6.set_range(0,1000); + g.pi_throttle.kP(tuning_value); + break; - #elif CHANNEL_6_TUNING == CH6_YAW_RATE_KP - g.pi_rate_yaw.kP((float)g.rc_6.control_in / 1000.0); + case CH6_TOP_BOTTOM_RATIO: + g.rc_6.set_range(800,1000); // .8 to 1 + g.top_bottom_ratio = tuning_value; + break; - #elif CHANNEL_6_TUNING == CH6_YAW_RATE_KI - g.pi_rate_yaw.kI((float)g.rc_6.control_in / 1000.0); + case CH6_RELAY: + g.rc_6.set_range(0,1000); + if (g.rc_6.control_in <= 600) relay_on(); + if (g.rc_6.control_in >= 400) relay_off(); + break; - - //Altitude Hold - #elif CHANNEL_6_TUNING == CH6_THROTTLE_KP - g.pi_throttle.kP((float)g.rc_6.control_in / 1000.0); // 0 to 1 - - #elif CHANNEL_6_TUNING == CH6_THROTTLE_KD - g.pi_throttle.kD((float)g.rc_6.control_in / 1000.0); // 0 to 1 - - //Extras - #elif CHANNEL_6_TUNING == CH6_TOP_BOTTOM_RATIO - g.top_bottom_ratio = (float)g.rc_6.control_in / 1000.0; - - #elif CHANNEL_6_TUNING == CH6_TRAVERSE_SPEED - g.waypoint_speed_max = (float)g.rc_6.control_in / 1000.0; - - - #elif CHANNEL_6_TUNING == CH6_PMAX - g.pitch_max.set(g.rc_6.control_in * 2); // 0 to 2000 - - // Simple relay control - #elif CHANNEL_6_TUNING == CH6_RELAY - if(g.rc_6.control_in <= 600) relay_on(); - if(g.rc_6.control_in >= 400) relay_off(); - - #endif + case CH6_TRAVERSE_SPEED: + g.rc_6.set_range(0,1000); + g.waypoint_speed_max = g.rc_6.control_in; + break; + } } static void update_nav_wp() diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index effcf76bc2..8a9512664d 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -102,7 +102,7 @@ get_nav_throttle(long z_error, int target_speed) rate_error = constrain(rate_error, -110, 110); throttle = g.pi_throttle.get_pi(rate_error, delta_ms_medium_loop); - return g.throttle_cruise + rate_error; + return g.throttle_cruise + throttle; } diff --git a/ArduCopter/Mavlink_Common.h b/ArduCopter/Mavlink_Common.h index f401d048ef..7d54f4ef80 100644 --- a/ArduCopter/Mavlink_Common.h +++ b/ArduCopter/Mavlink_Common.h @@ -87,7 +87,7 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ mode, nav_mode, status, - load * 1000, + 0, battery_voltage * 1000, battery_remaining, packet_drops); @@ -294,14 +294,14 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS); Vector3f mag_offsets = compass.get_offsets(); - mavlink_msg_sensor_offsets_send(chan, + mavlink_msg_sensor_offsets_send(chan, mag_offsets.x, mag_offsets.y, mag_offsets.z, compass.get_declination(), - barometer.RawPress, + barometer.RawPress, barometer.RawTemp, - imu.gx(), imu.gy(), imu.gz(), + imu.gx(), imu.gy(), imu.gz(), imu.ax(), imu.ay(), imu.az()); break; } @@ -353,8 +353,8 @@ static void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint16_t pa // see if we can send the deferred messages, if any while (q->num_deferred_messages != 0) { - if (!mavlink_try_send_message(chan, - q->deferred_messages[q->next_deferred_message], + if (!mavlink_try_send_message(chan, + q->deferred_messages[q->next_deferred_message], packet_drops)) { break; } diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index c8b8037991..9013557147 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -105,6 +105,7 @@ public: k_param_throttle_fs_value, k_param_throttle_cruise, k_param_esc_calibrate, + k_param_radio_tuning, #if FRAME_CONFIG == HELI_FRAME @@ -231,6 +232,8 @@ public: AP_Int16 pack_capacity; // Battery pack capacity less reserve AP_Int8 compass_enabled; AP_Int8 esc_calibrate; + AP_Int8 radio_tuning; + AP_Int8 frame_orientation; AP_Float top_bottom_ratio; AP_Int8 optflow_enabled; @@ -324,6 +327,7 @@ public: log_bitmask (DEFAULT_LOG_BITMASK, k_param_log_bitmask, PSTR("LOG_BITMASK")), RTL_altitude (ALT_HOLD_HOME * 100, k_param_RTL_altitude, PSTR("ALT_HOLD_RTL")), esc_calibrate (0, k_param_esc_calibrate, PSTR("ESC")), + radio_tuning (0, k_param_radio_tuning, PSTR("TUNE")), frame_orientation (FRAME_ORIENTATION, k_param_frame_orientation, PSTR("FRAME")), top_bottom_ratio (TOP_BOTTOM_RATIO, k_param_top_bottom_ratio, PSTR("TB_RATIO")), diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 664ab801c9..1b3423e4ca 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -513,7 +513,7 @@ // Throttle control gains // #ifndef THROTTLE_P -# define THROTTLE_P 0.35 // +# define THROTTLE_P 0.6 // #endif #ifndef THROTTLE_I # define THROTTLE_I 0.10 // with 4m error, 12.5s windup @@ -522,7 +522,7 @@ //# define THROTTLE_D 0.6 // upped with filter //#endif #ifndef THROTTLE_IMAX -# define THROTTLE_IMAX 150 +# define THROTTLE_IMAX 300 #endif diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 6b485b146a..62339e3b79 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -138,13 +138,6 @@ #define CIRCLE 8 // AUTO control #define NUM_MODES 9 -// YAW debug -// --------- -#define YAW_HOLD 0 -#define YAW_BRAKE 1 -#define YAW_RATE 2 - - // CH_6 Tuning // ----------- #define CH6_NONE 0 @@ -152,20 +145,16 @@ #define CH6_STABILIZE_KP 1 #define CH6_STABILIZE_KI 2 #define CH6_YAW_KP 3 -#define CH6_YAW_KD 4 // Rate #define CH6_RATE_KP 5 #define CH6_RATE_KI 6 #define CH6_YAW_RATE_KP 7 -#define CH6_YAW_RATE_KD 8 // Altitude -#define CH6_THROTTLE_KP 9 -#define CH6_THROTTLE_KD 10 +#define CH6_THROTTLE_KP 8 // Extras -#define CH6_TOP_BOTTOM_RATIO 11 -#define CH6_PMAX 12 -#define CH6_RELAY 13 -#define CH6_TRAVERSE_SPEED 14 +#define CH6_TOP_BOTTOM_RATIO 9 +#define CH6_RELAY 10 +#define CH6_TRAVERSE_SPEED 11 // nav byte mask // ------------- diff --git a/ArduCopter/radio.pde b/ArduCopter/radio.pde index 21e6758962..4025730e8c 100644 --- a/ArduCopter/radio.pde +++ b/ArduCopter/radio.pde @@ -35,30 +35,6 @@ static void init_rc_in() g.rc_7.set_range(0,1000); g.rc_8.set_range(0,1000); - #if CHANNEL_6_TUNING == CH6_RATE_KP - g.rc_6.set_range(0,300); // 0 t0 .3 - - #elif CHANNEL_6_TUNING == CH6_RATE_KI - g.rc_6.set_range(0,300); // 0 t0 .3 - - #elif CHANNEL_6_TUNING == CH6_STABILIZE_KP - g.rc_6.set_range(0,8000); // 0 t0 .3 - - #elif CHANNEL_6_TUNING == CH6_STABILIZE_KI - g.rc_6.set_range(0,300); // 0 t0 .3 - - #elif CHANNEL_6_TUNING == CH6_THROTTLE_KP - g.rc_6.set_range(0,800); // 0 to .8 - - #elif CHANNEL_6_TUNING == CH6_THROTTLE_KD - g.rc_6.set_range(0,500); // 0 to .5 - - #elif CHANNEL_6_TUNING == CH6_TOP_BOTTOM_RATIO - g.rc_6.set_range(800,1000); // .8 to 1 - -/* #elif CHANNEL_6_TUNING == CH6_RELAY - g.rc_6.set_range(0,1000); // 0 to 1 */ - #endif } static void init_rc_out() diff --git a/ArduCopter/setup.pde b/ArduCopter/setup.pde index 9b618dcadb..529297aa2d 100644 --- a/ArduCopter/setup.pde +++ b/ArduCopter/setup.pde @@ -13,6 +13,7 @@ static int8_t setup_flightmodes (uint8_t argc, const Menu::arg *argv); static int8_t setup_batt_monitor (uint8_t argc, const Menu::arg *argv); static int8_t setup_sonar (uint8_t argc, const Menu::arg *argv); static int8_t setup_compass (uint8_t argc, const Menu::arg *argv); +static int8_t setup_tune (uint8_t argc, const Menu::arg *argv); //static int8_t setup_mag_offset (uint8_t argc, const Menu::arg *argv); static int8_t setup_declination (uint8_t argc, const Menu::arg *argv); static int8_t setup_esc (uint8_t argc, const Menu::arg *argv); @@ -41,6 +42,7 @@ const struct Menu::command setup_menu_commands[] PROGMEM = { {"battery", setup_batt_monitor}, {"sonar", setup_sonar}, {"compass", setup_compass}, + {"tune", setup_tune}, // {"offsets", setup_mag_offset}, {"declination", setup_declination}, #ifdef OPTFLOW_ENABLED @@ -354,6 +356,15 @@ setup_declination(uint8_t argc, const Menu::arg *argv) return 0; } +static int8_t +setup_tune(uint8_t argc, const Menu::arg *argv) +{ + g.radio_tuning.set_and_save(argv[1].i); + report_tuning(); + return 0; +} + + static int8_t setup_erase(uint8_t argc, const Menu::arg *argv) @@ -1131,3 +1142,15 @@ static void report_version() print_blanks(2); } + +static void report_tuning() +{ + Serial.printf_P(PSTR("\nTUNE:\n")); + print_divider(); + if (g.radio_tuning == 0){ + print_enabled(g.radio_tuning.get()); + }else{ + Serial.printf_P(PSTR(" %d\n"),(int)g.radio_tuning.get()); + } + print_blanks(2); +} diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde index 3baaf3cc08..6dcd5614f4 100644 --- a/ArduCopter/test.pde +++ b/ArduCopter/test.pde @@ -636,59 +636,8 @@ test_tuning(uint8_t argc, const Menu::arg *argv) while(1){ delay(200); read_radio(); - - //Outer Loop : Attitude - #if CHANNEL_6_TUNING == CH6_NONE - Serial.printf_P(PSTR("disabled\n")); - - #elif CHANNEL_6_TUNING == CH6_STABILIZE_KP - Serial.printf_P(PSTR("stab kP: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0)); - - #elif CHANNEL_6_TUNING == CH6_STABILIZE_KI - Serial.printf_P(PSTR("stab kI: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0)); - - #elif CHANNEL_6_TUNING == CH6_YAW_KP - Serial.printf_P(PSTR("yaw Hold kP: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0)); // range from 0 ~ 5.0 - - #elif CHANNEL_6_TUNING == CH6_YAW_KI - Serial.printf_P(PSTR("yaw Hold kI: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0)); - - //Inner Loop : Rate - #elif CHANNEL_6_TUNING == CH6_RATE_KP - Serial.printf_P(PSTR("rate kD: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0)); - - #elif CHANNEL_6_TUNING == CH6_RATE_KI - Serial.printf_P(PSTR("rate kI: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0)); - - #elif CHANNEL_6_TUNING == CH6_YAW_RATE_KP - Serial.printf_P(PSTR("yaw rate kP: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0)); - - #elif CHANNEL_6_TUNING == CH6_YAW_RATE_KI - Serial.printf_P(PSTR("yaw rate kI: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0)); - - - //Altitude Hold - #elif CHANNEL_6_TUNING == CH6_THROTTLE_KP - Serial.printf_P(PSTR("throttle kP: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0)); - - #elif CHANNEL_6_TUNING == CH6_THROTTLE_KD - Serial.printf_P(PSTR("baro kD: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0)); - - #elif CHANNEL_6_TUNING == CH6_TRAVERSE_SPEED - Serial.printf_P(PSTR("traverse: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0)); - - - //Extras - #elif CHANNEL_6_TUNING == CH6_TOP_BOTTOM_RATIO - Serial.printf_P(PSTR("Y6: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0)); - - #elif CHANNEL_6_TUNING == CH6_PMAX - Serial.printf_P(PSTR("Y6: %d\n"), (g.rc_6.control_in * 2)); - - #elif CHANNEL_6_TUNING == CH6_RELAY - Serial.printf_P(PSTR(" %d\n"), (g.rc_6.control_in )); - - #endif + tuning(); + Serial.printf_P(PSTR("tune: %1.3f\n"), tuning_value); if(Serial.available() > 0){ return (0); @@ -1009,10 +958,12 @@ test_mission(uint8_t argc, const Menu::arg *argv) return (0); } */ + static void print_hit_enter() { Serial.printf_P(PSTR("Hit Enter to exit.\n\n")); } + /* static void fake_out_gps() {