Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
This commit is contained in:
jasonshort 2011-05-13 19:29:45 +00:00
parent 142443a9c7
commit 9780ee69d9
6 changed files with 39 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopterMega/APM_Config.h
View File

@ -8,7 +8,7 @@
#define NAV_TEST 0 // 0 = traditional, 1 = rate controlled #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 //#define CHANNEL_6_TUNING CH6_PMAX
/* /*

5
ArduCopterMega/ArduCopterMega.pde
View File

@ -459,7 +459,7 @@ void loop()
{ {
// We want this to execute fast // We want this to execute fast
// ---------------------------- // ----------------------------
if (millis() - fast_loopTimer > 6) { if (millis() - fast_loopTimer >= 5) {
delta_ms_fast_loop = millis() - fast_loopTimer; delta_ms_fast_loop = millis() - fast_loopTimer;
fast_loopTimer = millis(); fast_loopTimer = millis();
load = float(fast_loopTimeStamp - fast_loopTimer) / delta_ms_fast_loop; load = float(fast_loopTimeStamp - fast_loopTimer) / delta_ms_fast_loop;
@ -528,7 +528,7 @@ void fast_loop()
// record throttle output // 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 #if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && HIL_MODE != HIL_MODE_DISABLED
// HIL for a copter needs very fast update of the servo values // 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 // kick the GCS to process uplink data
gcs.update(); gcs.update();
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK #if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
gcs.data_stream_send(45,1000); gcs.data_stream_send(45,1000);
#endif #endif

30
ArduCopterMega/Attitude.pde
View File

@ -49,13 +49,13 @@ output_stabilize_roll()
error = constrain(error, -2500, 2500); error = constrain(error, -2500, 2500);
// only buildup I if we are trying to roll hard // 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? // smoother alternative to reset?
//if(g.pid_stabilize_roll.kI() != 0){ //if(g.pid_stabilize_roll.kI() != 0){
// g.pid_stabilize_roll.kI(g.pid_stabilize_roll.kI() * .8); // 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 // 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 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 // omega is the raw gyro reading
// Limit dampening to be equal to propotional term for symmetry // Limit dampening to be equal to propotional term for symmetry
rate = degrees(omega.x) * 100.0; // 6rad = 34377 rate = degrees(omega.x) * 100.0; // 6rad = 34377
dampener = rate * g.stabilize_dampener; // 34377 * .175 = 6000 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 -= 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 void
@ -82,9 +86,9 @@ output_stabilize_pitch()
error = constrain(error, -2500, 2500); error = constrain(error, -2500, 2500);
// only buildup I if we are trying to roll hard // only buildup I if we are trying to roll hard
if(abs(g.rc_2.servo_out) < 1500){ //if(abs(g.rc_2.servo_out) < 1500){
g.pid_stabilize_pitch.reset_I(); // g.pid_stabilize_pitch.reset_I();
} //}
// write out angles back to servo out - this will be converted to PWM by RC_Channel // 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); 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 // Limit dampening to be equal to propotional term for symmetry
rate = degrees(omega.y) * 100.0; // 6rad = 34377 rate = degrees(omega.y) * 100.0; // 6rad = 34377
dampener = rate * g.stabilize_dampener; // 34377 * .175 = 6000 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 void
@ -286,7 +294,7 @@ output_yaw_with_hold(boolean hold)
if(g.rc_4.control_in == 0){ if(g.rc_4.control_in == 0){
// adaptive braking // 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 yaw_debug = YAW_BRAKE; // 1

7
ArduCopterMega/GCS_Mavlink.pde
View File

@ -498,14 +498,15 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// decode // decode
mavlink_waypoint_clear_all_t packet; mavlink_waypoint_clear_all_t packet;
mavlink_msg_waypoint_clear_all_decode(msg, &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 // clear all waypoints
uint8_t type = 0; // ok (0), error(1) uint8_t type = 0; // ok (0), error(1)
g.waypoint_total.set_and_save(0); g.waypoint_total.set_and_save(0);
// send acknowledgement 3 times to makes sure it is received // 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; break;
} }
@ -540,6 +541,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
packet.count = MAX_WAYPOINTS; packet.count = MAX_WAYPOINTS;
} }
g.waypoint_total.set_and_save(packet.count - 1); g.waypoint_total.set_and_save(packet.count - 1);
waypoint_timelast_receive = millis(); waypoint_timelast_receive = millis();
waypoint_receiving = true; waypoint_receiving = true;
waypoint_sending = false; waypoint_sending = false;
@ -934,6 +936,7 @@ GCS_MAVLINK::_queued_send()
(requested_interface == chan) && (requested_interface == chan) &&
waypoint_request_i <= g.waypoint_total && waypoint_request_i <= g.waypoint_total &&
mavdelay > 15) { // limits to 3.33 hz mavdelay > 15) { // limits to 3.33 hz
mavlink_msg_waypoint_request_send( mavlink_msg_waypoint_request_send(
chan, chan,
waypoint_dest_sysid, waypoint_dest_sysid,

16
ArduCopterMega/config.h
View File

@ -323,29 +323,29 @@
// STABILZE Angle Control // STABILZE Angle Control
// //
#ifndef STABILIZE_ROLL_P #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 #endif
#ifndef STABILIZE_ROLL_I #ifndef STABILIZE_ROLL_I
# define STABILIZE_ROLL_I 0.1 // # define STABILIZE_ROLL_I 0.025 //
#endif #endif
#ifndef STABILIZE_ROLL_D #ifndef STABILIZE_ROLL_D
# define STABILIZE_ROLL_D 0.13 # define STABILIZE_ROLL_D 0.12
#endif #endif
#ifndef STABILIZE_ROLL_IMAX #ifndef STABILIZE_ROLL_IMAX
# define STABILIZE_ROLL_IMAX 10 // 10 degrees # define STABILIZE_ROLL_IMAX .5 // degrees * 100
#endif #endif
#ifndef STABILIZE_PITCH_P #ifndef STABILIZE_PITCH_P
# define STABILIZE_PITCH_P 0.6 # define STABILIZE_PITCH_P 0.48
#endif #endif
#ifndef STABILIZE_PITCH_I #ifndef STABILIZE_PITCH_I
# define STABILIZE_PITCH_I 0.1 # define STABILIZE_PITCH_I 0.025
#endif #endif
#ifndef STABILIZE_PITCH_D #ifndef STABILIZE_PITCH_D
# define STABILIZE_PITCH_D 0.13 # define STABILIZE_PITCH_D 0.12
#endif #endif
#ifndef STABILIZE_PITCH_IMAX #ifndef STABILIZE_PITCH_IMAX
# define STABILIZE_PITCH_IMAX 10 # define STABILIZE_PITCH_IMAX .5 // degrees * 100
#endif #endif
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////

6
ArduCopterMega/setup.pde
View File

@ -313,15 +313,15 @@ setup_motors(uint8_t argc, const Menu::arg *argv)
} }
g.rc_4.calc_pwm(); 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){ if(g.rc_3.control_in > 0){
APM_RC.OutputCh(CH_1, g.rc_3.radio_in); APM_RC.OutputCh(CH_1, g.rc_3.radio_in);
APM_RC.OutputCh(CH_2, 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); 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{ }else{
APM_RC.OutputCh(CH_1, motor_out[CH_1]); APM_RC.OutputCh(CH_1, motor_out[CH_1]);
APM_RC.OutputCh(CH_2, motor_out[CH_2]); APM_RC.OutputCh(CH_2, motor_out[CH_2]);