mirror of https://github.com/ArduPilot/ardupilot
Altitude control fix - trying to go higher would make alt hold reset to 0
Yaw fix, improved PID defaults potential logging fix git-svn-id: https://arducopter.googlecode.com/svn/trunk@1877 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort
parent
6769295058
commit
21980216b9
|
|
@ -4,7 +4,7 @@
|
|||
|
||||
// GPS is auto-selected
|
||||
|
||||
//#define GCS_PROTOCOL GCS_PROTOCOL_NONE
|
||||
#define GCS_PROTOCOL GCS_PROTOCOL_NONE
|
||||
//#define GCS_PORT 0
|
||||
|
||||
#define SERIAL0_BAUD 38400
|
||||
|
|
|
|||
|
|
@ -42,6 +42,7 @@ version 2.1 of the License, or (at your option) any later version.
|
|||
|
||||
#define MAVLINK_COMM_NUM_BUFFERS 2
|
||||
#include <GCS_MAVLink.h> // MAVLink GCS definitions
|
||||
|
||||
//#include <GCS_SIMPLE.h>
|
||||
|
||||
|
||||
|
|
@ -767,7 +768,7 @@ void slow_loop()
|
|||
// 1Hz loop
|
||||
void super_slow_loop()
|
||||
{
|
||||
if (g.log_bitmask & MASK_LOG_CUR)
|
||||
if (g.log_bitmask & MASK_LOG_CURRENT)
|
||||
Log_Write_Current();
|
||||
|
||||
gcs.send_message(MSG_HEARTBEAT); // XXX This is running at 3 1/3 Hz instead of 1 Hz
|
||||
|
|
@ -1162,7 +1163,10 @@ adjust_altitude()
|
|||
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 = min((current_loc.alt + 400), next_WP.alt); // don't go more than 4 meters below current location
|
||||
//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;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -41,6 +41,10 @@ output_stabilize_roll()
|
|||
|
||||
// only buildup I if we are trying to roll hard
|
||||
if(abs(g.rc_1.servo_out) < 1500){
|
||||
// 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();
|
||||
}
|
||||
|
||||
|
|
@ -126,18 +130,18 @@ output_yaw_with_hold(boolean hold)
|
|||
|
||||
if(hold){
|
||||
// try and hold the current nav_yaw setting
|
||||
yaw_error = nav_yaw - dcm.yaw_sensor; // +- 60°
|
||||
yaw_error = wrap_180(yaw_error);
|
||||
yaw_error = nav_yaw - dcm.yaw_sensor; // +- 60°
|
||||
yaw_error = wrap_180(yaw_error);
|
||||
|
||||
// limit the error we're feeding to the PID
|
||||
yaw_error = constrain(yaw_error, -6000, 6000); // limit error to 60 degees
|
||||
yaw_error = constrain(yaw_error, -6000, 6000); // limit error to 60 degees
|
||||
|
||||
// Apply PID and save the new angle back to RC_Channel
|
||||
g.rc_4.servo_out = g.pid_yaw.get_pid(yaw_error, delta_ms_fast_loop, 1.0); // .5 * 6000 = 3000
|
||||
|
||||
// We adjust the output by the rate of rotation
|
||||
long rate = degrees(omega.z) * 100.0; // 3rad = 17188 , 6rad = 34377
|
||||
int dampener = ((float)rate * g.hold_yaw_dampener); // 18000 * .17 = 3000
|
||||
long rate = degrees(omega.z) * 100.0; // 3rad = 17188 , 6rad = 34377
|
||||
int dampener = (float)rate * g.hold_yaw_dampener; // 18000 * .17 = 3000
|
||||
|
||||
// Limit dampening to be equal to propotional term for symmetry
|
||||
g.rc_4.servo_out -= constrain(dampener, -max_yaw_dampener, max_yaw_dampener); // -3000
|
||||
|
|
@ -155,15 +159,26 @@ output_yaw_with_hold(boolean hold)
|
|||
// we are breaking;
|
||||
g.rc_4.servo_out = (omega.z > 0) ? -1800 : 1800;
|
||||
|
||||
//g.rc_4.servo_out = (int)((float)g.rc_4.servo_out * (omega.z / 6.0));
|
||||
|
||||
//switch comments to get old behavior.
|
||||
//g.rc_4.servo_out = g.pid_acro_rate_yaw.get_pid(error, delta_ms_fast_loop, 1.0);// kP .07 * 36000 = 2520
|
||||
|
||||
}else{
|
||||
g.rc_4.servo_out = g.pid_acro_rate_yaw.get_pid(error, delta_ms_fast_loop, 1.0);// kP .07 * 36000 = 2520
|
||||
g.rc_4.servo_out = g.pid_acro_rate_yaw.get_pid(error, delta_ms_fast_loop, 1.0); // kP .07 * 36000 = 2520
|
||||
|
||||
// We adjust the output by the rate of rotation
|
||||
//long rate = degrees(omega.z) * 100.0; // 3rad = 17188 , 6rad = 34377
|
||||
//int dampener = (float)rate * g.hold_yaw_dampener; // 18000 * .17 = 3000
|
||||
|
||||
// Limit dampening to be equal to propotional term for symmetry
|
||||
//g.rc_4.servo_out -= constrain(dampener, -max_yaw_dampener, max_yaw_dampener); // -3000
|
||||
g.rc_4.servo_out = constrain(g.rc_4.servo_out, -1800, 1800); // limit to 24°
|
||||
}
|
||||
|
||||
g.rc_4.servo_out = constrain(g.rc_4.servo_out, -1800, 1800); // limit to 24°
|
||||
}
|
||||
|
||||
//Serial.printf("%d\n",g.rc_4.servo_out);
|
||||
}
|
||||
|
||||
// slight left rudder gives right roll.
|
||||
|
|
|
|||
|
|
@ -69,7 +69,7 @@ print_log_menu(void)
|
|||
PLOG(MODE);
|
||||
PLOG(RAW);
|
||||
PLOG(CMD);
|
||||
PLOG(CUR);
|
||||
PLOG(CURRENT);
|
||||
#undef PLOG
|
||||
}
|
||||
Serial.println();
|
||||
|
|
@ -166,7 +166,7 @@ select_logs(uint8_t argc, const Menu::arg *argv)
|
|||
TARG(MODE);
|
||||
TARG(RAW);
|
||||
TARG(CMD);
|
||||
TARG(CUR);
|
||||
TARG(CURRENT);
|
||||
#undef TARG
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -212,8 +212,8 @@ void do_RTL(void)
|
|||
// output control mode to the ground station
|
||||
gcs.send_message(MSG_HEARTBEAT);
|
||||
|
||||
if (g.log_bitmask & MASK_LOG_MODE)
|
||||
Log_Write_Mode(control_mode);
|
||||
//if (g.log_bitmask & MASK_LOG_MODE)
|
||||
// Log_Write_Mode(control_mode);
|
||||
}
|
||||
|
||||
void do_takeoff()
|
||||
|
|
|
|||
|
|
@ -270,7 +270,6 @@
|
|||
#ifndef ACRO_RATE_ROLL_IMAX
|
||||
# define ACRO_RATE_ROLL_IMAX 20
|
||||
#endif
|
||||
# define ACRO_RATE_ROLL_IMAX_CENTIDEGREE ACRO_RATE_ROLL_IMAX * 100
|
||||
|
||||
#ifndef ACRO_RATE_PITCH_P
|
||||
# define ACRO_RATE_PITCH_P .190
|
||||
|
|
@ -284,10 +283,9 @@
|
|||
#ifndef ACRO_RATE_PITCH_IMAX
|
||||
# define ACRO_RATE_PITCH_IMAX 20
|
||||
#endif
|
||||
#define ACRO_RATE_PITCH_IMAX_CENTIDEGREE ACRO_RATE_PITCH_IMAX * 100
|
||||
|
||||
#ifndef ACRO_RATE_YAW_P
|
||||
# define ACRO_RATE_YAW_P .2 // used to control response in turning
|
||||
# define ACRO_RATE_YAW_P .1 // used to control response in turning
|
||||
#endif
|
||||
#ifndef ACRO_RATE_YAW_I
|
||||
# define ACRO_RATE_YAW_I 0.0
|
||||
|
|
@ -298,7 +296,6 @@
|
|||
#ifndef ACRO_RATE_YAW_IMAX
|
||||
# define ACRO_RATE_YAW_IMAX 0
|
||||
#endif
|
||||
# define ACRO_RATE_YAW_IMAX_CENTIDEGREE ACRO_RATE_YAW_IMAX * 100
|
||||
|
||||
#ifndef ACRO_RATE_TRIGGER
|
||||
# define ACRO_RATE_TRIGGER 0
|
||||
|
|
@ -315,7 +312,7 @@
|
|||
# define STABILIZE_ROLL_I 0.1 //
|
||||
#endif
|
||||
#ifndef STABILIZE_ROLL_D
|
||||
# define STABILIZE_ROLL_D 0.135
|
||||
# define STABILIZE_ROLL_D 0.11
|
||||
#endif
|
||||
#ifndef STABILIZE_ROLL_IMAX
|
||||
# define STABILIZE_ROLL_IMAX 10 // 10 degrees
|
||||
|
|
@ -328,7 +325,7 @@
|
|||
# define STABILIZE_PITCH_I 0.1
|
||||
#endif
|
||||
#ifndef STABILIZE_PITCH_D
|
||||
# define STABILIZE_PITCH_D 0.135
|
||||
# define STABILIZE_PITCH_D 0.11
|
||||
#endif
|
||||
#ifndef STABILIZE_PITCH_IMAX
|
||||
# define STABILIZE_PITCH_IMAX 10
|
||||
|
|
@ -384,10 +381,10 @@
|
|||
# define THROTTLE_BARO_P 0.25
|
||||
#endif
|
||||
#ifndef THROTTLE_BARO_I
|
||||
# define THROTTLE_BARO_I 0.01
|
||||
# define THROTTLE_BARO_I 0.04
|
||||
#endif
|
||||
#ifndef THROTTLE_BARO_D
|
||||
# define THROTTLE_BARO_D 0.2
|
||||
# define THROTTLE_BARO_D 0.0 // lowered to 0 to debug effects
|
||||
#endif
|
||||
#ifndef THROTTLE_BARO_IMAX
|
||||
# define THROTTLE_BARO_IMAX 50
|
||||
|
|
@ -398,7 +395,7 @@
|
|||
# define THROTTLE_SONAR_P .3
|
||||
#endif
|
||||
#ifndef THROTTLE_SONAR_I
|
||||
# define THROTTLE_SONAR_I 0.01
|
||||
# define THROTTLE_SONAR_I 0.1
|
||||
#endif
|
||||
#ifndef THROTTLE_SONAR_D
|
||||
# define THROTTLE_SONAR_D 0.03
|
||||
|
|
|
|||
|
|
@ -80,7 +80,7 @@ void read_trim_switch()
|
|||
#endif
|
||||
}else{
|
||||
// set the throttle nominal
|
||||
if(g.rc_3.control_in > 200){
|
||||
if(g.rc_3.control_in > 150){
|
||||
g.throttle_cruise.set_and_save(g.rc_3.control_in);
|
||||
//Serial.printf("tnom %d\n", g.throttle_cruise.get());
|
||||
}
|
||||
|
|
|
|||
|
|
@ -212,7 +212,7 @@
|
|||
#define MASK_LOG_MODE (1<<6)
|
||||
#define MASK_LOG_RAW (1<<7)
|
||||
#define MASK_LOG_CMD (1<<8)
|
||||
#define MASK_LOG_CUR (1<<9)
|
||||
#define MASK_LOG_CURRENT (1<<9)
|
||||
#define MASK_LOG_SET_DEFAULTS (1<<15)
|
||||
|
||||
// Waypoint Modes
|
||||
|
|
|
|||
|
|
@ -9,21 +9,29 @@ void arm_motors()
|
|||
|
||||
// Arm motor output : Throttle down and full yaw right for more than 2 seconds
|
||||
if (g.rc_3.control_in == 0){
|
||||
if (g.rc_4.control_in > 2700) {
|
||||
if (arming_counter > ARM_DELAY) {
|
||||
motor_armed = true;
|
||||
// full right
|
||||
if (g.rc_4.control_in > 4000) {
|
||||
if (arming_counter >= ARM_DELAY) {
|
||||
motor_armed = true;
|
||||
arming_counter = ARM_DELAY;
|
||||
} else{
|
||||
arming_counter++;
|
||||
}
|
||||
}else if (g.rc_4.control_in < -2700) {
|
||||
if (arming_counter > DISARM_DELAY){
|
||||
motor_armed = false;
|
||||
// full left
|
||||
}else if (g.rc_4.control_in < -4000) {
|
||||
if (arming_counter >= DISARM_DELAY){
|
||||
motor_armed = false;
|
||||
arming_counter = DISARM_DELAY;
|
||||
}else{
|
||||
arming_counter++;
|
||||
}
|
||||
// centered
|
||||
}else{
|
||||
arming_counter = 0;
|
||||
}
|
||||
|
||||
}else{
|
||||
arming_counter = 0;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -186,7 +194,7 @@ set_servos_4()
|
|||
}
|
||||
|
||||
num++;
|
||||
if (num > 50){
|
||||
if (num > 25){
|
||||
num = 0;
|
||||
/*
|
||||
Serial.printf("t_alt:%ld, alt:%ld, thr: %d sen: ",
|
||||
|
|
@ -224,24 +232,15 @@ set_servos_4()
|
|||
//*/
|
||||
|
||||
/*
|
||||
gcs_simple.write_int(motor_out[CH_1]);
|
||||
gcs_simple.write_int(motor_out[CH_2]);
|
||||
gcs_simple.write_int(motor_out[CH_3]);
|
||||
gcs_simple.write_int(motor_out[CH_4]);
|
||||
gcs_simple.write_int(g.rc_3.servo_out);
|
||||
|
||||
gcs_simple.write_int((int)(cos_yaw_x * 100));
|
||||
gcs_simple.write_int((int)(sin_yaw_y * 100));
|
||||
gcs_simple.write_int((int)(dcm.yaw_sensor / 100));
|
||||
gcs_simple.write_int((int)(nav_yaw / 100));
|
||||
gcs_simple.write_byte(control_mode);
|
||||
gcs_simple.write_int(g.rc_3.servo_out);
|
||||
|
||||
gcs_simple.write_int((int)nav_lat);
|
||||
gcs_simple.write_int((int)nav_lon);
|
||||
|
||||
gcs_simple.write_int((int)nav_roll);
|
||||
gcs_simple.write_int((int)nav_pitch);
|
||||
|
||||
//24
|
||||
gcs_simple.write_long(current_loc.lat); //28
|
||||
gcs_simple.write_long(current_loc.lng); //32
|
||||
gcs_simple.write_int((int)current_loc.alt); //34
|
||||
|
|
@ -250,6 +249,27 @@ set_servos_4()
|
|||
gcs_simple.write_long(next_WP.lng);
|
||||
gcs_simple.write_int((int)next_WP.alt); //44
|
||||
|
||||
gcs_simple.write_int((int)(target_bearing / 100));
|
||||
gcs_simple.write_int((int)(nav_bearing / 100));
|
||||
gcs_simple.write_int((int)(nav_yaw / 100));
|
||||
gcs_simple.write_int((int)(dcm.yaw_sensor / 100));
|
||||
|
||||
if(altitude_sensor == BARO){
|
||||
gcs_simple.write_int((int)g.pid_baro_throttle.get_integrator());
|
||||
}else{
|
||||
gcs_simple.write_int((int)g.pid_sonar_throttle.get_integrator());
|
||||
}
|
||||
|
||||
gcs_simple.write_int(g.throttle_cruise);
|
||||
|
||||
|
||||
//gcs_simple.write_int((int)(cos_yaw_x * 100));
|
||||
//gcs_simple.write_int((int)(sin_yaw_y * 100));
|
||||
//gcs_simple.write_int((int)(nav_yaw / 100));
|
||||
|
||||
|
||||
//24
|
||||
|
||||
gcs_simple.flush(10); // Message ID
|
||||
//*/
|
||||
|
||||
|
|
|
|||
|
|
@ -107,8 +107,8 @@ setup_factory(uint8_t argc, const Menu::arg *argv)
|
|||
Serial.printf_P(PSTR("\nFACTORY RESET complete - reboot APM"));
|
||||
|
||||
delay(1000);
|
||||
default_log_bitmask();
|
||||
default_gains();
|
||||
//default_log_bitmask();
|
||||
//default_gains();
|
||||
|
||||
for (;;) {
|
||||
}
|
||||
|
|
@ -683,7 +683,7 @@ void default_log_bitmask()
|
|||
{
|
||||
|
||||
// convenience macro for testing LOG_* and setting LOGBIT_*
|
||||
#define LOGBIT(_s) (MASK_LOG_##_s ? MASK_LOG_##_s : 0)
|
||||
#define LOGBIT(_s) (LOG_##_s ? MASK_LOG_##_s : 0)
|
||||
|
||||
g.log_bitmask =
|
||||
LOGBIT(ATTITUDE_FAST) |
|
||||
|
|
@ -695,7 +695,7 @@ void default_log_bitmask()
|
|||
LOGBIT(MODE) |
|
||||
LOGBIT(RAW) |
|
||||
LOGBIT(CMD) |
|
||||
LOGBIT(CUR);
|
||||
LOGBIT(CURRENT);
|
||||
#undef LOGBIT
|
||||
|
||||
g.log_bitmask.save();
|
||||
|
|
|
|||
|
|
@ -292,6 +292,10 @@ void set_mode(byte mode)
|
|||
control_mode = mode;
|
||||
control_mode = constrain(control_mode, 0, NUM_MODES - 1);
|
||||
|
||||
// XXX temporary
|
||||
//if (g.log_bitmask & MASK_LOG_MODE)
|
||||
Log_Write_Mode(control_mode);
|
||||
|
||||
// used to stop fly_aways
|
||||
if(g.rc_1.control_in == 0){
|
||||
// we are on the ground is this is true
|
||||
|
|
@ -481,8 +485,11 @@ init_simple_bearing()
|
|||
void
|
||||
init_throttle_cruise()
|
||||
{
|
||||
if(set_throttle_cruise_flag == false && g.rc_3.control_in > 250){
|
||||
set_throttle_cruise_flag = true;
|
||||
g.throttle_cruise.set_and_save(g.rc_3.control_in);
|
||||
if(set_throttle_cruise_flag == false){
|
||||
if(g.rc_3.control_in > 200){
|
||||
//set_throttle_cruise_flag = true;
|
||||
g.throttle_cruise.set_and_save(g.rc_3.control_in);
|
||||
//Serial.printf("throttle_cruise: %d\n", g.throttle_cruise.get());
|
||||
}
|
||||
}
|
||||
}
|
||||
Loading…
Reference in New Issue