git-svn-id: https://arducopter.googlecode.com/svn/trunk@2433 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-05-29 04:02:01 +00:00
parent 28f91400e4
commit c67601dacb
7 changed files with 61 additions and 56 deletions

View File

@ -8,7 +8,7 @@
#define NAV_TEST 1 // 0 = traditional, 1 = rate controlled
#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.
#define AUTO_RESET_LOITER 0 // enables Loiter to reset it's current location based on stick input.
#define FRAME_CONFIG QUAD_FRAME
/*

View File

@ -393,7 +393,7 @@ struct Location prev_WP; // last waypoint
struct Location current_loc; // current location
struct Location next_WP; // next waypoint
struct Location target_WP; // where do we want to you towards?
struct Location simple_WP; // command for telemetry
struct Location simple_WP; //
struct Location next_command; // command preloaded
long target_altitude; // used for
boolean home_is_set; // Flag for if we have g_gps lock and have set the home location
@ -1172,7 +1172,7 @@ void update_navigation()
update_nav_wp();
break;
#if YAW_HACK == 1
/*#if YAW_HACK == 1
case SIMPLE:
// calculates desired Yaw
// exprimental_hack
@ -1183,7 +1183,7 @@ void update_navigation()
}
break;
#endif
*/
}
}

View File

@ -209,7 +209,6 @@ output_yaw_with_hold(boolean hold)
// rate control
long rate = degrees(omega.z) * 100; // 3rad = 17188 , 6rad = 34377
rate = constrain(rate, -36000, 36000); // limit to something fun!
int dampener = rate * g.pid_yaw.kD(); // 34377 * .175 = 6000
if(hold){
// look to see if we have exited rate control properly - ie stopped turning
@ -218,11 +217,8 @@ output_yaw_with_hold(boolean hold)
if(fabs(omega.z) < .2){
clear_yaw_control();
hold = true; // just to be explicit
//Serial.print("C");
}else{
hold = false; // return to rate control until we slow down.
//Serial.print("D");
}
}
@ -247,7 +243,7 @@ output_yaw_with_hold(boolean hold)
g.rc_4.servo_out = g.pid_yaw.get_pi(yaw_error, delta_ms_fast_loop, 1.0); // .4 * 4000 = 1600
// add in yaw dampener
g.rc_4.servo_out -= dampener;
g.rc_4.servo_out -= rate * g.pid_yaw.kD();
yaw_debug = YAW_HOLD; //0
@ -256,7 +252,7 @@ output_yaw_with_hold(boolean hold)
if(g.rc_4.control_in == 0){
// adaptive braking
g.rc_4.servo_out = (int)(-800.0 * omega.z);
g.rc_4.servo_out = (int)(-1000.0 * omega.z);
yaw_debug = YAW_BRAKE; // 1
@ -266,12 +262,13 @@ output_yaw_with_hold(boolean hold)
long error = ((long)g.rc_4.control_in * 6) - (rate * 2); // control is += 6000 * 6 = 36000
g.rc_4.servo_out = g.pid_acro_rate_yaw.get_pid(error, delta_ms_fast_loop, 1.0); // kP .07 * 36000 = 2520
yaw_debug = YAW_RATE; // 2
nav_yaw = dcm.yaw_sensor; //
//nav_yaw = dcm.yaw_sensor; // I think this caused the free rotation, dont know why.
}
}
// Limit Output
g.rc_4.servo_out = constrain(g.rc_4.servo_out, -2400, 2400); // limit to 24°
g.rc_4.servo_out = constrain(g.rc_4.servo_out, -2500, 2500); // limit to 24°
//Serial.printf("%d\n",g.rc_4.servo_out);
}
@ -303,6 +300,6 @@ output_yaw_with_hold(boolean hold)
// add in yaw dampener
g.rc_4.servo_out -= degrees(omega.z) * 100 * g.pid_yaw.kD();
yaw_error = constrain(yaw_error, -2500, 2500); // limit error to 60 degees
g.rc_4.servo_out = constrain(g.rc_4.servo_out, -2500, 2500); // limit error to 60 degees
}
#endif

View File

@ -354,13 +354,13 @@
// YAW Control
//
#ifndef YAW_P
# define YAW_P 0.6 // increase for more aggressive Yaw Hold, decrease if it's bouncy
# define YAW_P 0.4 // increase for more aggressive Yaw Hold, decrease if it's bouncy
#endif
#ifndef YAW_I
# define YAW_I 0.01 // set to .0001 to try and get over user's steady state error caused by poor balance
#endif
#ifndef YAW_D
# define YAW_D 0.1 // Trying a lower value to prevent odd behavior
# define YAW_D 0.05 // Trying a lower value to prevent odd behavior
#endif
#ifndef YAW_IMAX
# define YAW_IMAX 1 // degrees * 100

View File

@ -22,11 +22,14 @@ void arm_motors()
} else{
arming_counter++;
}
// full left
}else if (g.rc_4.control_in < -4000) {
if (arming_counter >= DISARM_DELAY){
motor_armed = false;
arming_counter = DISARM_DELAY;
compass.save_offsets();
}else{
arming_counter++;
}

View File

@ -405,58 +405,63 @@ setup_sonar(uint8_t argc, const Menu::arg *argv)
static int8_t
setup_mag_offset(uint8_t argc, const Menu::arg *argv)
{
Vector3f _offsets;
if (!strcmp_P(argv[1].str, PSTR("c"))) {
compass.set_offsets(_offsets);
report_compass();
return (0);
}
print_hit_enter();
init_compass();
float _min[3], _max[3];
Vector3f _offsets;
Vector3f compass_mag;
while(1){
static float min[3], _max[3], offset[3];
if (millis() - fast_loopTimer > 100) {
delta_ms_fast_loop = millis() - fast_loopTimer;
fast_loopTimer = millis();
G_Dt = (float)delta_ms_fast_loop / 1000.f;
int _min[3] = {0,0,0};
int _max[3] = {0,0,0};
compass.read();
compass.calculate(0, 0); // roll = 0, pitch = 0 for this example
compass_mag = compass.get_offsets();
compass.calculate(0,0); // roll = 0, pitch = 0
// capture min
_min[0] = min(_min[0], compass_mag.x);
_min[1] = min(_min[1], compass_mag.y);
_min[2] = min(_min[2], compass_mag.z);
while(1){
delay(50);
compass.read();
compass.calculate(0,0); // roll = 0, pitch = 0
if(compass.mag_x < _min[0]) _min[0] = compass.mag_x;
if(compass.mag_y < _min[1]) _min[1] = compass.mag_y;
if(compass.mag_z < _min[2]) _min[2] = compass.mag_z;
// capture max
_max[0] = max(_max[0], compass_mag.x);
_max[1] = max(_max[1], compass_mag.y);
_max[2] = max(_max[2], compass_mag.z);
if(compass.mag_x > _max[0]) _max[0] = compass.mag_x;
if(compass.mag_y > _max[1]) _max[1] = compass.mag_y;
if(compass.mag_z > _max[2]) _max[2] = compass.mag_z;
// calculate offsets
_offsets.x = -(_max[0] + _min[0]) / 2;
_offsets.y = -(_max[1] + _min[1]) / 2;
_offsets.z = -(_max[2] + _min[2]) / 2;
_offsets.x = (float)(_max[0] + _min[0]) / -2;
_offsets.y = (float)(_max[1] + _min[1]) / -2;
_offsets.z = (float)(_max[2] + _min[2]) / -2;
// display all to user
Serial.printf_P(PSTR("Heading: %u, \t (%4.4f, %4.4f, %4.4f) (%4.4f, %4.4f, %4.4f)\n"),
(uint16_t)wrap_360(ToDeg(compass.heading)),
compass_mag.x,
compass_mag.y,
compass_mag.z,
Serial.printf_P(PSTR("Heading: %u, \t (%d, %d, %d), (%4.4f, %4.4f, %4.4f)\n"),
(uint16_t)(wrap_360(ToDeg(compass.heading) * 100)) /100,
compass.mag_x,
compass.mag_y,
compass.mag_z,
_offsets.x,
_offsets.y,
_offsets.z);
if(Serial.available() > 0){
if(Serial.available() > 1){
compass.set_offsets(_offsets);
//compass.set_offsets(mag_offset_x, mag_offset_y, mag_offset_z);
report_compass();
break;
}
}
}
}

View File

@ -37,7 +37,7 @@ const struct Menu::command main_menu_commands[] PROGMEM = {
};
// Create the top-level menu object.
MENU(main_menu, "AC 2.0.17 Beta", main_menu_commands);
MENU(main_menu, "AC 2.0.18 Beta", main_menu_commands);
void init_ardupilot()
{