mirror of https://github.com/ArduPilot/ardupilot
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
parent
142443a9c7
commit
9780ee69d9
|
|
@ -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
|
||||
|
||||
/*
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -67,7 +67,11 @@ output_stabilize_roll()
|
|||
// 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
|
||||
//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
|
||||
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
|
|
|
|||
|
|
@ -313,14 +313,14 @@ 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);
|
||||
}else{
|
||||
APM_RC.OutputCh(CH_1, motor_out[CH_1]);
|
||||
|
|
|
|||
Loading…
Reference in New Issue