From 9abd8c63364012dc5f2d16f6d5ac711502da7d94 Mon Sep 17 00:00:00 2001 From: jasonshort Date: Tue, 31 May 2011 05:29:06 +0000 Subject: [PATCH] Big update from Field testing. Includes updated motor tests. These need to be verified. Nav loiter D was added (.03) - verify this helps. Nav_lat is now filtered for Rate control. This is to keep jerky pitching to a minimum. git-svn-id: https://arducopter.googlecode.com/svn/trunk@2448 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- ArduCopterMega/APM_Config.h | 7 ++- ArduCopterMega/ArduCopterMega.pde | 61 ++++++++++++------------- ArduCopterMega/Attitude.pde | 25 ++++------ ArduCopterMega/Camera.pde | 3 ++ ArduCopterMega/GCS_Mavlink.pde | 1 + ArduCopterMega/Log.pde | 6 +++ ArduCopterMega/Parameters.h | 2 +- ArduCopterMega/command description.txt | 2 +- ArduCopterMega/commands.pde | 4 +- ArduCopterMega/commands_logic.pde | 8 +++- ArduCopterMega/commands_process.pde | 52 --------------------- ArduCopterMega/config.h | 12 +++-- ArduCopterMega/motors_hexa.pde | 63 ++++++++++++++++++++++++++ ArduCopterMega/motors_quad.pde | 59 +++++++++++++++++++----- ArduCopterMega/motors_tri.pde | 27 +++++++---- ArduCopterMega/motors_y6.pde | 42 +++++++++++------ ArduCopterMega/navigation.pde | 7 +-- ArduCopterMega/setup.pde | 2 + ArduCopterMega/system.pde | 13 ++++-- 19 files changed, 239 insertions(+), 157 deletions(-) diff --git a/ArduCopterMega/APM_Config.h b/ArduCopterMega/APM_Config.h index 2eba1a6e50..3c540a101f 100644 --- a/ArduCopterMega/APM_Config.h +++ b/ArduCopterMega/APM_Config.h @@ -7,8 +7,11 @@ //#define MAG_ORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD #define NAV_TEST 1 // 0 = traditional, 1 = rate controlled -#define YAW_OPTION 1 // 0 = hybrid rate approach, 1 = offset Yaw approach -#define AUTO_RESET_LOITER 0 // enables Loiter to reset it's current location based on stick input. +#define YAW_OPTION 0 // 0 = hybrid rate approach, 1 = offset Yaw approach +#define AUTO_RESET_LOITER 1 // enables Loiter to reset it's current location based on stick input. + +// do we want to have camera stabilization? +#define CAMERA_STABILIZER ENABLED #define FRAME_CONFIG QUAD_FRAME /* diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index 58c9712c9d..8fec7b3ea1 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -594,6 +594,9 @@ void medium_loop() // control mode specific updates to nav_bearing // -------------------------------------------- update_navigation(); + + if (g.log_bitmask & MASK_LOG_NTUN) + Log_Write_Nav_Tuning(); } break; @@ -632,19 +635,13 @@ void medium_loop() medium_loopCounter++; #if HIL_MODE != HIL_MODE_ATTITUDE - if (g.log_bitmask & MASK_LOG_ATTITUDE_MED && !(g.log_bitmask & MASK_LOG_ATTITUDE_FAST)) + if (g.log_bitmask & MASK_LOG_ATTITUDE_MED) Log_Write_Attitude(); if (g.log_bitmask & MASK_LOG_CTUN) Log_Write_Control_Tuning(); #endif - if (g.log_bitmask & MASK_LOG_NTUN) - Log_Write_Nav_Tuning(); - - if (GPS_enabled && g.log_bitmask & MASK_LOG_GPS){ - Log_Write_GPS(); - } // XXX this should be a "GCS medium loop" interface #if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK @@ -711,9 +708,9 @@ void fifty_hz_loop() Log_Write_Raw(); #endif - //#if ENABLE_CAM - camera_stabilization(); - //#endif + #if CAMERA_STABILIZER == ENABLED + camera_stabilization(); + #endif // XXX is it appropriate to be doing the comms below on the fast loop? @@ -904,6 +901,10 @@ void update_GPS(void) current_loc.lng = g_gps->longitude; // Lon * 10 * *7 current_loc.lat = g_gps->latitude; // Lat * 10 * *7 + + if (g.log_bitmask & MASK_LOG_GPS){ + Log_Write_GPS(); + } } } @@ -978,16 +979,13 @@ void update_current_flight_mode(void) nav_pitch = 0; nav_roll = 0; - // Output Pitch, Roll, Yaw and Throttle - // ------------------------------------ - // are we at rest? reset nav_yaw if(g.rc_3.control_in == 0){ - nav_yaw = dcm.yaw_sensor; + clear_yaw_control(); + }else{ + // Yaw control + output_manual_yaw(); } - // Yaw control - output_manual_yaw(); - // apply throttle control output_manual_throttle(); @@ -1033,14 +1031,12 @@ void update_current_flight_mode(void) // are we at rest? reset nav_yaw if(g.rc_3.control_in == 0){ - nav_yaw = dcm.yaw_sensor; + clear_yaw_control(); + }else{ + // Yaw control + output_manual_yaw(); } - // Output Pitch, Roll, Yaw and Throttle - // ------------------------------------ - // Yaw control - output_manual_yaw(); - // apply throttle control output_manual_throttle(); @@ -1165,8 +1161,8 @@ void update_navigation() update_nav_yaw(); case LOITER: // are we Traversing or Loitering? - //wp_control = (wp_distance < 10) ? LOITER_MODE : WP_MODE; - wp_control = LOITER_MODE; + wp_control = (wp_distance < 50) ? LOITER_MODE : WP_MODE; + //wp_control = LOITER_MODE; // calculates the desired Roll and Pitch update_nav_wp(); @@ -1241,8 +1237,8 @@ void update_alt() //sonar_alt = sonar.read(); // decide if we're using sonar - if ((baro_alt < 1200) || sonar_alt < 300){ - + //if ((baro_alt < 1200) || sonar_alt < 300){ + if (baro_alt < 700){ // correct alt for angle of the sonar float temp = cos_pitch_x * cos_roll_x; temp = max(temp, 0.707); @@ -1280,15 +1276,14 @@ adjust_altitude() flight_timer = 0; if(g.rc_3.control_in <= 200){ - next_WP.alt -= 3; // 1 meter per second + next_WP.alt -= 1; // 1 meter per second + next_WP.alt = max(next_WP.alt, (current_loc.alt - 100)); // don't go more than 4 meters below current location next_WP.alt = max(next_WP.alt, 100); // no lower than 1 meter? - next_WP.alt = max((current_loc.alt - 400), next_WP.alt); // don't go more than 4 meters below current location + }else if (g.rc_3.control_in > 700){ - next_WP.alt += 3; // 1 meter per second + next_WP.alt += 2; // 1 meter per second //next_WP.alt = min((current_loc.alt + 400), next_WP.alt); // don't go more than 4 meters below current location - if(next_WP.alt > (current_loc.alt + 400)){ - next_WP.alt = current_loc.alt + 400; - } + next_WP.alt = min(next_WP.alt, (current_loc.alt + 200)); // don't go more than 4 meters below current location } } } diff --git a/ArduCopterMega/Attitude.pde b/ArduCopterMega/Attitude.pde index ac8dad4c83..40e1f52d85 100644 --- a/ArduCopterMega/Attitude.pde +++ b/ArduCopterMega/Attitude.pde @@ -144,6 +144,9 @@ void calc_nav_throttle() } // simple filtering + if(nav_throttle_old == 0) + nav_throttle_old = nav_throttle; + nav_throttle = (nav_throttle + nav_throttle_old) >> 1; nav_throttle_old = nav_throttle; @@ -166,23 +169,11 @@ yaw control void output_manual_yaw() { - if(g.rc_3.control_in == 0){ - // we want to only call this once - if(did_clear_yaw_control == false){ - clear_yaw_control(); - did_clear_yaw_control = true; - } - - }else{ // motors running - - // Yaw control - if(g.rc_4.control_in == 0){ - output_yaw_with_hold(true); // hold yaw - }else{ - output_yaw_with_hold(false); // rate control yaw - } - - did_clear_yaw_control = false; + // Yaw control + if(g.rc_4.control_in == 0){ + output_yaw_with_hold(true); // hold yaw + }else{ + output_yaw_with_hold(false); // rate control yaw } } diff --git a/ArduCopterMega/Camera.pde b/ArduCopterMega/Camera.pde index 0452e8e5bc..3937bfb065 100644 --- a/ArduCopterMega/Camera.pde +++ b/ArduCopterMega/Camera.pde @@ -1,5 +1,7 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#if CAMERA_STABILIZER == ENABLED + void init_camera() { g.rc_camera_pitch.set_angle(4500); @@ -38,3 +40,4 @@ camera_stabilization() //rc_camera_roll.calc_pwm(); } +#endif diff --git a/ArduCopterMega/GCS_Mavlink.pde b/ArduCopterMega/GCS_Mavlink.pde index 13ede51a2c..4d2c32ec9d 100644 --- a/ArduCopterMega/GCS_Mavlink.pde +++ b/ArduCopterMega/GCS_Mavlink.pde @@ -637,6 +637,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields case MAV_CMD_NAV_LOITER_TURNS: case MAV_CMD_DO_SET_HOME: + case MAV_CMD_DO_SET_ROI: tell_command.p1 = packet.param1; break; diff --git a/ArduCopterMega/Log.pde b/ArduCopterMega/Log.pde index 19169f6b20..13780322db 100644 --- a/ArduCopterMega/Log.pde +++ b/ArduCopterMega/Log.pde @@ -97,6 +97,10 @@ dump_log(uint8_t argc, const Menu::arg *argv) // check that the requested log number can be read dump_log = argv[1].i; + if(dump_log == 99){ + Log_Read(1, 4096); + } + if (/*(argc != 2) || */(dump_log < 1) || (dump_log > last_log_num)) { Serial.printf_P(PSTR("bad log # %d\n"), dump_log); return(-1); @@ -175,9 +179,11 @@ select_logs(uint8_t argc, const Menu::arg *argv) if (!strcasecmp_P(argv[0].str, PSTR("enable"))) { g.log_bitmask.set_and_save(g.log_bitmask | bits); + }else{ g.log_bitmask.set_and_save(g.log_bitmask & ~bits); } + return(0); } diff --git a/ArduCopterMega/Parameters.h b/ArduCopterMega/Parameters.h index 81f03971a3..446ff9f653 100644 --- a/ArduCopterMega/Parameters.h +++ b/ArduCopterMega/Parameters.h @@ -237,7 +237,7 @@ public: throttle_fs_value (THROTTLE_FS_VALUE, k_param_throttle_fs_value, PSTR("THR_FS_VALUE")), throttle_cruise (100, k_param_throttle_cruise, PSTR("TRIM_THROTTLE")), - flight_modes (k_param_flight_modes, PSTR("FLIGHT_MODES")), + flight_modes (k_param_flight_modes, PSTR("FLTMODE")), pitch_max (PITCH_MAX * 100, k_param_pitch_max, PSTR("PITCH_MAX")), diff --git a/ArduCopterMega/command description.txt b/ArduCopterMega/command description.txt index 5eb25caaa7..f2f316a898 100644 --- a/ArduCopterMega/command description.txt +++ b/ArduCopterMega/command description.txt @@ -71,4 +71,4 @@ Command ID Name Parameter 1 Parameter 2 Parameter 3 Parameter 4 Note: Max cycle time = 60 sec, A repeat relay or repeat servo command will cancel any current repeating event // need to add command -0xB7 / 185 MAV_CMD_DO_SET_ROI Yaw_Mode altitude lat lon +0xB7 / 201 MAV_CMD_DO_SET_ROI Yaw_Mode altitude lat lon diff --git a/ArduCopterMega/commands.pde b/ArduCopterMega/commands.pde index 8d9c71b4a1..5bf2035d2e 100644 --- a/ArduCopterMega/commands.pde +++ b/ArduCopterMega/commands.pde @@ -51,7 +51,7 @@ struct Location get_command_with_index(int i) return temp; }else{ - Serial.println("LD"); + //Serial.println("LD"); // we can load a command, we don't process it yet // read WP position long mem = (WP_START_BYTE) + (i * WP_SIZE); @@ -78,7 +78,7 @@ struct Location get_command_with_index(int i) if((temp.id < MAV_CMD_NAV_LAST || temp.id == MAV_CMD_CONDITION_CHANGE_ALT) && temp.options & WP_OPTION_ALT_RELATIVE){ //temp.alt += home.alt; } - Serial.println("ADD ALT"); + //Serial.println("ADD ALT"); if(temp.options & WP_OPTION_RELATIVE){ // If were relative, just offset from home diff --git a/ArduCopterMega/commands_logic.pde b/ArduCopterMega/commands_logic.pde index 9689d02aa2..eb399b96f3 100644 --- a/ArduCopterMega/commands_logic.pde +++ b/ArduCopterMega/commands_logic.pde @@ -5,6 +5,9 @@ /********************************************************************************/ void handle_process_must() { + // clear nav_lat, this is how we pitch towards the target based on speed + nav_lat = 0; + switch(next_command.id){ case MAV_CMD_NAV_TAKEOFF: @@ -38,6 +41,7 @@ void handle_process_must() default: break; } + } void handle_process_may() @@ -494,9 +498,9 @@ void do_yaw() command_yaw_speed = next_command.lat * 100; // ms * 100 - // if unspecified go 60° a second + // if unspecified go 30° a second if(command_yaw_speed == 0) - command_yaw_speed = 6000; + command_yaw_speed = 3000; // if unspecified go counterclockwise if(command_yaw_dir == 0) diff --git a/ArduCopterMega/commands_process.pde b/ArduCopterMega/commands_process.pde index ed468c2221..116bb68914 100644 --- a/ArduCopterMega/commands_process.pde +++ b/ArduCopterMega/commands_process.pde @@ -16,62 +16,10 @@ void change_command(uint8_t index) } } - -// called by 10 Hz Medium loop -// --------------------------- -void update_commands2(void) -{ - //Serial.println("Upd CMDs"); - - // This function loads commands into three buffers - // when a new command is loaded, it is processed with process_XXX() - - // If we have a command in the queue already - if(next_command.id != NO_COMMAND){ - return; - } - - // fetch next command if the next command queue is empty - // ----------------------------------------------------- - next_command = get_command_with_index(g.waypoint_index + 1); - Serial.printf("next CMD %d\n", next_command.id); - - if(next_command.id == NO_COMMAND){ - // if no commands were available from EEPROM - // And we have no nav commands - // -------------------------------------------- - if (command_must_ID == NO_COMMAND){ - gcs.send_text_P(SEVERITY_LOW,PSTR("out of commands!")); - handle_no_commands(); - } - - } else { - // A command was loaded from EEPROM - // -------------------------------------------- - // Set our current mission index + 1; - increment_WP_index(); - - Serial.printf("loaded cmd %d\n" , g.waypoint_index.get()); - - // debug by outputing the Waypoint loaded - print_wp(&next_command, g.waypoint_index); - - - // act on our new command - if (process_next_command()){ - // invalidate command queue so a new one is loaded - // ----------------------------------------------- - clear_command_queue(); - } - } -} - // called by 10 Hz Medium loop // --------------------------- void update_commands(void) { - //Serial.println("Upd CMDs"); - // fill command queue with a new command if available if(next_command.id == NO_COMMAND){ diff --git a/ArduCopterMega/config.h b/ArduCopterMega/config.h index bc1238974d..8958fcf7b4 100644 --- a/ArduCopterMega/config.h +++ b/ArduCopterMega/config.h @@ -371,7 +371,7 @@ // // how much to we pitch towards the target #ifndef PITCH_MAX -# define PITCH_MAX 18 // degrees +# define PITCH_MAX 24 // degrees #endif @@ -382,10 +382,10 @@ # define NAV_LOITER_P 2.5 // upped to be a bit more aggressive #endif #ifndef NAV_LOITER_I -# define NAV_LOITER_I 0.05 // upped a bit to deal with wind faster +# define NAV_LOITER_I 0.08 // upped a bit to deal with wind faster #endif #ifndef NAV_LOITER_D -# define NAV_LOITER_D 0.00 +# define NAV_LOITER_D 0.03 // Added some D 2.20, untested #endif #ifndef NAV_LOITER_IMAX # define NAV_LOITER_IMAX 20 // 20° @@ -515,6 +515,12 @@ # define LOG_CURRENT DISABLED #endif +// if we are using fast, Disable Medium +#if LOG_ATTITUDE_FAST == ENABLED + #undef LOG_ATTITUDE_MED + #define LOG_ATTITUDE_MED DISABLED +#endif + #ifndef DEBUG_PORT # define DEBUG_PORT 0 #endif diff --git a/ArduCopterMega/motors_hexa.pde b/ArduCopterMega/motors_hexa.pde index 32c7a1942b..00d8e7aa07 100644 --- a/ArduCopterMega/motors_hexa.pde +++ b/ArduCopterMega/motors_hexa.pde @@ -111,6 +111,68 @@ void output_motors_disarmed() void output_motor_test() { + motor_out[CH_1] = g.rc_3.radio_min; + motor_out[CH_2] = g.rc_3.radio_min; + motor_out[CH_3] = g.rc_3.radio_min; + motor_out[CH_4] = g.rc_3.radio_min; + motor_out[CH_7] = g.rc_3.radio_min; + motor_out[CH_8] = g.rc_3.radio_min; + + + if(g.frame_orientation == X_FRAME){ +// 31 +// 24 + if(g.rc_1.control_in > 3000){ // right + motor_out[CH_1] += 50; + } + + if(g.rc_1.control_in < -3000){ // left + motor_out[CH_2] += 50; + } + + if(g.rc_2.control_in > 3000){ // back + motor_out[CH_8] += 50; + motor_out[CH_4] += 50; + } + + if(g.rc_2.control_in < -3000){ // front + motor_out[CH_7] += 50; + motor_out[CH_3] += 50; + } + + }else{ +// 3 +// 2 1 +// 4 + if(g.rc_1.control_in > 3000){ // right + motor_out[CH_4] += 50; + motor_out[CH_8] += 50; + } + + if(g.rc_1.control_in < -3000){ // left + motor_out[CH_7] += 50; + motor_out[CH_3] += 50; + } + + if(g.rc_2.control_in > 3000){ // back + motor_out[CH_2] += 50; + } + + if(g.rc_2.control_in < -3000){ // front + motor_out[CH_1] += 50; + } + + } + + APM_RC.OutputCh(CH_1, motor_out[CH_1]); + APM_RC.OutputCh(CH_2, motor_out[CH_2]); + APM_RC.OutputCh(CH_3, motor_out[CH_3]); + APM_RC.OutputCh(CH_4, motor_out[CH_4]); + APM_RC.OutputCh(CH_7, motor_out[CH_7]); + APM_RC.OutputCh(CH_8, motor_out[CH_8]); +} + +/* APM_RC.OutputCh(CH_2, g.rc_3.radio_min); APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 50); delay(1000); @@ -134,6 +196,7 @@ void output_motor_test() APM_RC.OutputCh(CH_8, g.rc_3.radio_min); APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 50); delay(1000); +*/ } #endif \ No newline at end of file diff --git a/ArduCopterMega/motors_quad.pde b/ArduCopterMega/motors_quad.pde index f00711937f..4ac8ae75a3 100644 --- a/ArduCopterMega/motors_quad.pde +++ b/ArduCopterMega/motors_quad.pde @@ -96,21 +96,56 @@ void output_motors_disarmed() void output_motor_test() { - APM_RC.OutputCh(CH_2, g.rc_3.radio_min); - APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 50); - delay(1000); + motor_out[CH_1] = g.rc_3.radio_min; + motor_out[CH_2] = g.rc_3.radio_min; + motor_out[CH_3] = g.rc_3.radio_min; + motor_out[CH_4] = g.rc_3.radio_min; - APM_RC.OutputCh(CH_3, g.rc_3.radio_min); - APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 50); - delay(1000); - APM_RC.OutputCh(CH_1, g.rc_3.radio_min); - APM_RC.OutputCh(CH_4, g.rc_3.radio_min + 50); - delay(1000); + if(g.frame_orientation == X_FRAME){ +// 31 +// 24 + if(g.rc_1.control_in > 3000){ + motor_out[CH_1] += 50; + motor_out[CH_4] += 50; + } - APM_RC.OutputCh(CH_4, g.rc_3.radio_min); - APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 50); - delay(1000); + if(g.rc_1.control_in < -3000){ + motor_out[CH_2] += 50; + motor_out[CH_3] += 50; + } + + if(g.rc_2.control_in > 3000){ + motor_out[CH_2] += 50; + motor_out[CH_4] += 50; + } + + if(g.rc_2.control_in < -3000){ + motor_out[CH_1] += 50; + motor_out[CH_3] += 50; + } + + }else{ +// 3 +// 2 1 +// 4 + if(g.rc_1.control_in > 3000) + motor_out[CH_1] += 50; + + if(g.rc_1.control_in < -3000) + motor_out[CH_2] += 50; + + if(g.rc_2.control_in > 3000) + motor_out[CH_4] += 50; + + if(g.rc_2.control_in < -3000) + motor_out[CH_3] += 50; + } + + APM_RC.OutputCh(CH_1, motor_out[CH_1]); + APM_RC.OutputCh(CH_2, motor_out[CH_2]); + APM_RC.OutputCh(CH_3, motor_out[CH_3]); + APM_RC.OutputCh(CH_4, motor_out[CH_4]); } #endif \ No newline at end of file diff --git a/ArduCopterMega/motors_tri.pde b/ArduCopterMega/motors_tri.pde index a02dfc6059..ecb3d3d65a 100644 --- a/ArduCopterMega/motors_tri.pde +++ b/ArduCopterMega/motors_tri.pde @@ -69,17 +69,26 @@ void output_motors_disarmed() void output_motor_test() { - APM_RC.OutputCh(CH_2, g.rc_3.radio_min); - APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 50); - delay(1000); + motor_out[CH_1] = g.rc_3.radio_min; + motor_out[CH_2] = g.rc_3.radio_min; + motor_out[CH_4] = g.rc_3.radio_min; - APM_RC.OutputCh(CH_1, g.rc_3.radio_min); - APM_RC.OutputCh(CH_4, g.rc_3.radio_min + 50); - delay(1000); - APM_RC.OutputCh(CH_4, g.rc_3.radio_min); - APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 50); - delay(1000); + if(g.rc_1.control_in > 3000){ // right + motor_out[CH_1] += 50; + } + + if(g.rc_1.control_in < -3000){ // left + motor_out[CH_2] += 50; + } + + if(g.rc_2.control_in > 3000){ // back + motor_out[CH_4] += 50; + } + + APM_RC.OutputCh(CH_1, motor_out[CH_1]); + APM_RC.OutputCh(CH_2, motor_out[CH_2]); + APM_RC.OutputCh(CH_4, motor_out[CH_4]); } #endif \ No newline at end of file diff --git a/ArduCopterMega/motors_y6.pde b/ArduCopterMega/motors_y6.pde index 8b95e69da4..48e8cbb0d7 100644 --- a/ArduCopterMega/motors_y6.pde +++ b/ArduCopterMega/motors_y6.pde @@ -88,23 +88,35 @@ void output_motors_disarmed() void output_motor_test() { - APM_RC.OutputCh(CH_2, g.rc_3.radio_min); - APM_RC.OutputCh(CH_3, g.rc_3.radio_min); - APM_RC.OutputCh(CH_7, g.rc_3.radio_min + 50); - APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 50); - delay(1000); + motor_out[CH_1] = g.rc_3.radio_min; + motor_out[CH_2] = g.rc_3.radio_min; + motor_out[CH_3] = g.rc_3.radio_min; + motor_out[CH_4] = g.rc_3.radio_min; + motor_out[CH_7] = g.rc_3.radio_min; + motor_out[CH_8] = g.rc_3.radio_min; - APM_RC.OutputCh(CH_7, g.rc_3.radio_min); - APM_RC.OutputCh(CH_1, g.rc_3.radio_min); - APM_RC.OutputCh(CH_8, g.rc_3.radio_min + 50); - APM_RC.OutputCh(CH_4, g.rc_3.radio_min + 50); - delay(1000); - APM_RC.OutputCh(CH_8, g.rc_3.radio_min); - APM_RC.OutputCh(CH_4, g.rc_3.radio_min); - APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 50); - APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 50); - delay(1000); + if(g.rc_1.control_in > 3000){ // right + motor_out[CH_1] += 50; + motor_out[CH_7] += 50; + } + + if(g.rc_1.control_in < -3000){ // left + motor_out[CH_2] += 50; + motor_out[CH_3] += 50; + } + + if(g.rc_2.control_in > 3000){ // back + motor_out[CH_8] += 50; + motor_out[CH_4] += 50; + } + + APM_RC.OutputCh(CH_1, motor_out[CH_1]); + APM_RC.OutputCh(CH_2, motor_out[CH_2]); + APM_RC.OutputCh(CH_3, motor_out[CH_4]); + APM_RC.OutputCh(CH_4, motor_out[CH_4]); + APM_RC.OutputCh(CH_7, motor_out[CH_7]); + APM_RC.OutputCh(CH_8, motor_out[CH_8]); } #endif \ No newline at end of file diff --git a/ArduCopterMega/navigation.pde b/ArduCopterMega/navigation.pde index 72d3f7886a..4185d1fb1a 100644 --- a/ArduCopterMega/navigation.pde +++ b/ArduCopterMega/navigation.pde @@ -165,9 +165,10 @@ void calc_rate_nav() // we want to be going 450cm/s int error = constrain(WAYPOINT_SPEED - groundspeed, -1000, 1000); // Scale response by kP - nav_lat = g.pid_nav_wp.get_pid(error, dTnav, 1.0); + nav_lat = nav_lat + g.pid_nav_wp.get_pid(error, dTnav, 1.0); + nav_lat >>= 1; // divide by two - Serial.printf("dTnav: %ld, gs: %d, err: %d, int: %d, pitch: %ld", dTnav, groundspeed, error, (int)g.pid_nav_wp.get_integrator(), (long)nav_lat); + //Serial.printf("dTnav: %ld, gs: %d, err: %d, int: %d, pitch: %ld", dTnav, groundspeed, error, (int)g.pid_nav_wp.get_integrator(), (long)nav_lat); // limit our output nav_lat = constrain(nav_lat, -4000, 4000); // +- max error @@ -219,7 +220,7 @@ void update_crosstrack(void) { // Crosstrack Error // ---------------- - if (cross_track_test() < 5000) { // If we are too far off or too close we don't do track following + if (cross_track_test() < 9000) { // If we are too far off or too close we don't do track following crosstrack_error = sin(radians((target_bearing - crosstrack_bearing) / 100)) * wp_distance; // Meters we are off track line nav_bearing += constrain(crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get()); nav_bearing = wrap_360(nav_bearing); diff --git a/ArduCopterMega/setup.pde b/ArduCopterMega/setup.pde index ca8be4a96b..fab8f327b4 100644 --- a/ArduCopterMega/setup.pde +++ b/ArduCopterMega/setup.pde @@ -247,6 +247,8 @@ static int8_t setup_motors(uint8_t argc, const Menu::arg *argv) { while(1){ + delay(20); + read_radio(); output_motor_test(); if(Serial.available() > 0){ g.esc_calibrate.set_and_save(0); diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde index 4cfa16059f..f8574dda78 100644 --- a/ArduCopterMega/system.pde +++ b/ArduCopterMega/system.pde @@ -37,7 +37,7 @@ const struct Menu::command main_menu_commands[] PROGMEM = { }; // Create the top-level menu object. -MENU(main_menu, "AC 2.0.19 Beta", main_menu_commands); +MENU(main_menu, "AC 2.0.20 Beta", main_menu_commands); void init_ardupilot() { @@ -137,7 +137,10 @@ void init_ardupilot() init_rc_in(); // sets up rc channels from radio init_rc_out(); // sets up the timer libs - init_camera(); + + #if CAMERA_STABILIZER == ENABLED + init_camera(); + #endif #if HIL_MODE != HIL_MODE_ATTITUDE adc.Init(); // APM ADC library initialization @@ -331,8 +334,11 @@ void set_mode(byte mode) case ACRO: break; + case SIMPLE: case STABILIZE: do_loiter_at_location(); + g.pid_baro_throttle.reset_I(); + g.pid_sonar_throttle.reset_I(); break; case ALT_HOLD: @@ -345,9 +351,6 @@ void set_mode(byte mode) init_auto(); break; - case SIMPLE: - break; - case LOITER: init_throttle_cruise(); do_loiter_at_location();