From 9780ee69d9da3aa51bb2c52ba568999fd9a0ccfb Mon Sep 17 00:00:00 2001 From: jasonshort Date: Fri, 13 May 2011 19:29:45 +0000 Subject: [PATCH] Slight increase to loop speed - for Max removed throttle integrator until I have more time to deal with the data tweaked control loops to be more like RC1 for Max to try. They fly great BTW tweaked config to be more better. Mavlink - only cosmetic tweaks git-svn-id: https://arducopter.googlecode.com/svn/trunk@2254 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- ArduCopterMega/APM_Config.h | 2 +- ArduCopterMega/ArduCopterMega.pde | 5 +++-- ArduCopterMega/Attitude.pde | 30 +++++++++++++++++++----------- ArduCopterMega/GCS_Mavlink.pde | 7 +++++-- ArduCopterMega/config.h | 16 ++++++++-------- ArduCopterMega/setup.pde | 6 +++--- 6 files changed, 39 insertions(+), 27 deletions(-) diff --git a/ArduCopterMega/APM_Config.h b/ArduCopterMega/APM_Config.h index 400c72dc27..260b87c1e9 100644 --- a/ArduCopterMega/APM_Config.h +++ b/ArduCopterMega/APM_Config.h @@ -8,7 +8,7 @@ #define NAV_TEST 0 // 0 = traditional, 1 = rate controlled -#define CHANNEL_6_TUNING CH6_NONE +#define CHANNEL_6_TUNING CH6_STABLIZE_KD //#define CHANNEL_6_TUNING CH6_PMAX /* diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index 950dacf667..d9d536aeab 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -459,7 +459,7 @@ void loop() { // We want this to execute fast // ---------------------------- - if (millis() - fast_loopTimer > 6) { + if (millis() - fast_loopTimer >= 5) { delta_ms_fast_loop = millis() - fast_loopTimer; fast_loopTimer = millis(); load = float(fast_loopTimeStamp - fast_loopTimer) / delta_ms_fast_loop; @@ -528,7 +528,7 @@ void fast_loop() // record throttle output // ------------------------------ - throttle_integrator += g.rc_3.servo_out; + //throttle_integrator += g.rc_3.servo_out; #if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && HIL_MODE != HIL_MODE_DISABLED // HIL for a copter needs very fast update of the servo values @@ -738,6 +738,7 @@ void fifty_hz_loop() // kick the GCS to process uplink data gcs.update(); + #if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK gcs.data_stream_send(45,1000); #endif diff --git a/ArduCopterMega/Attitude.pde b/ArduCopterMega/Attitude.pde index 9d3a8a84c0..bab6eb45b2 100644 --- a/ArduCopterMega/Attitude.pde +++ b/ArduCopterMega/Attitude.pde @@ -49,13 +49,13 @@ output_stabilize_roll() error = constrain(error, -2500, 2500); // only buildup I if we are trying to roll hard - if(abs(g.rc_1.servo_out) < 1000){ + //if(abs(g.rc_1.servo_out) < 1000){ // smoother alternative to reset? //if(g.pid_stabilize_roll.kI() != 0){ // g.pid_stabilize_roll.kI(g.pid_stabilize_roll.kI() * .8); //} - g.pid_stabilize_roll.reset_I(); - } + // g.pid_stabilize_roll.reset_I(); + //} // write out angles back to servo out - this will be converted to PWM by RC_Channel g.rc_1.servo_out = g.pid_stabilize_roll.get_pid(error, delta_ms_fast_loop, 1.0); // 2500 * .7 = 1750 @@ -65,9 +65,13 @@ output_stabilize_roll() // omega is the raw gyro reading // Limit dampening to be equal to propotional term for symmetry - rate = degrees(omega.x) * 100.0; // 6rad = 34377 - dampener = rate * g.stabilize_dampener; // 34377 * .175 = 6000 - g.rc_1.servo_out -= constrain(dampener, -max_stabilize_dampener, max_stabilize_dampener); // limit to 1500 based on kP + rate = degrees(omega.x) * 100.0; // 6rad = 34377 + dampener = rate * g.stabilize_dampener; // 34377 * .175 = 6000 + //g.rc_1.servo_out -= constrain(dampener, -max_stabilize_dampener, max_stabilize_dampener); // limit to 1500 based on kP + + g.rc_1.servo_out -= dampener; + g.rc_1.servo_out = min(g.rc_1.servo_out, 2500); + g.rc_1.servo_out = max(g.rc_1.servo_out, -2500); } void @@ -82,9 +86,9 @@ output_stabilize_pitch() error = constrain(error, -2500, 2500); // only buildup I if we are trying to roll hard - if(abs(g.rc_2.servo_out) < 1500){ - g.pid_stabilize_pitch.reset_I(); - } + //if(abs(g.rc_2.servo_out) < 1500){ + // g.pid_stabilize_pitch.reset_I(); + //} // write out angles back to servo out - this will be converted to PWM by RC_Channel g.rc_2.servo_out = g.pid_stabilize_pitch.get_pid(error, delta_ms_fast_loop, 1.0); @@ -96,7 +100,11 @@ output_stabilize_pitch() // Limit dampening to be equal to propotional term for symmetry rate = degrees(omega.y) * 100.0; // 6rad = 34377 dampener = rate * g.stabilize_dampener; // 34377 * .175 = 6000 - g.rc_2.servo_out -= constrain(dampener, -max_stabilize_dampener, max_stabilize_dampener); // limit to 1500 based on kP + //g.rc_2.servo_out -= constrain(dampener, -max_stabilize_dampener, max_stabilize_dampener); // limit to 1500 based on kP + + g.rc_2.servo_out -= dampener; + g.rc_2.servo_out = min(g.rc_2.servo_out, 2500); + g.rc_2.servo_out = max(g.rc_2.servo_out, -2500); } void @@ -286,7 +294,7 @@ output_yaw_with_hold(boolean hold) if(g.rc_4.control_in == 0){ // adaptive braking - g.rc_4.servo_out = (int)((-3000.0 * omega.z) / 1); + g.rc_4.servo_out = (int)(-800.0 * omega.z); yaw_debug = YAW_BRAKE; // 1 diff --git a/ArduCopterMega/GCS_Mavlink.pde b/ArduCopterMega/GCS_Mavlink.pde index af46d507a1..24e62074de 100644 --- a/ArduCopterMega/GCS_Mavlink.pde +++ b/ArduCopterMega/GCS_Mavlink.pde @@ -498,14 +498,15 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // decode mavlink_waypoint_clear_all_t packet; mavlink_msg_waypoint_clear_all_decode(msg, &packet); - if (mavlink_check_target(packet.target_system,packet.target_component)) break; + if (mavlink_check_target(packet.target_system, packet.target_component)) break; // clear all waypoints uint8_t type = 0; // ok (0), error(1) g.waypoint_total.set_and_save(0); // send acknowledgement 3 times to makes sure it is received - for (int i=0;i<3;i++) mavlink_msg_waypoint_ack_send(chan,msg->sysid,msg->compid,type); + for (int i=0;i<3;i++) + mavlink_msg_waypoint_ack_send(chan, msg->sysid, msg->compid, type); break; } @@ -540,6 +541,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) packet.count = MAX_WAYPOINTS; } g.waypoint_total.set_and_save(packet.count - 1); + waypoint_timelast_receive = millis(); waypoint_receiving = true; waypoint_sending = false; @@ -934,6 +936,7 @@ GCS_MAVLINK::_queued_send() (requested_interface == chan) && waypoint_request_i <= g.waypoint_total && mavdelay > 15) { // limits to 3.33 hz + mavlink_msg_waypoint_request_send( chan, waypoint_dest_sysid, diff --git a/ArduCopterMega/config.h b/ArduCopterMega/config.h index 543c115267..c7a34745aa 100644 --- a/ArduCopterMega/config.h +++ b/ArduCopterMega/config.h @@ -323,29 +323,29 @@ // STABILZE Angle Control // #ifndef STABILIZE_ROLL_P -# define STABILIZE_ROLL_P 0.6 +# define STABILIZE_ROLL_P 0.54 // .48 = 4.0 NG, .54 = 4.5 NG #endif #ifndef STABILIZE_ROLL_I -# define STABILIZE_ROLL_I 0.1 // +# define STABILIZE_ROLL_I 0.025 // #endif #ifndef STABILIZE_ROLL_D -# define STABILIZE_ROLL_D 0.13 +# define STABILIZE_ROLL_D 0.12 #endif #ifndef STABILIZE_ROLL_IMAX -# define STABILIZE_ROLL_IMAX 10 // 10 degrees +# define STABILIZE_ROLL_IMAX .5 // degrees * 100 #endif #ifndef STABILIZE_PITCH_P -# define STABILIZE_PITCH_P 0.6 +# define STABILIZE_PITCH_P 0.48 #endif #ifndef STABILIZE_PITCH_I -# define STABILIZE_PITCH_I 0.1 +# define STABILIZE_PITCH_I 0.025 #endif #ifndef STABILIZE_PITCH_D -# define STABILIZE_PITCH_D 0.13 +# define STABILIZE_PITCH_D 0.12 #endif #ifndef STABILIZE_PITCH_IMAX -# define STABILIZE_PITCH_IMAX 10 +# define STABILIZE_PITCH_IMAX .5 // degrees * 100 #endif ////////////////////////////////////////////////////////////////////////////// diff --git a/ArduCopterMega/setup.pde b/ArduCopterMega/setup.pde index 92c7ee9dae..95aeb29ec9 100644 --- a/ArduCopterMega/setup.pde +++ b/ArduCopterMega/setup.pde @@ -313,15 +313,15 @@ setup_motors(uint8_t argc, const Menu::arg *argv) } g.rc_4.calc_pwm(); - motor_out[CH_3] = g.rc_4.radio_out; + // servo Yaw + APM_RC.OutputCh(CH_7, g.rc_4.radio_out); } if(g.rc_3.control_in > 0){ APM_RC.OutputCh(CH_1, g.rc_3.radio_in); APM_RC.OutputCh(CH_2, g.rc_3.radio_in); APM_RC.OutputCh(CH_3, g.rc_3.radio_in); - if(g.frame_type != TRI_FRAME) - APM_RC.OutputCh(CH_4, g.rc_3.radio_in); + APM_RC.OutputCh(CH_4, g.rc_3.radio_in); }else{ APM_RC.OutputCh(CH_1, motor_out[CH_1]); APM_RC.OutputCh(CH_2, motor_out[CH_2]);