Reworking Yaw some more. Yaw is testing really well with this version.

I have moved Pitch_Max to a different part of the code. It was limiting SIMPLE mode which was bad. Now it only limits in Auto modes, RTL, LOITER, etc.


git-svn-id: https://arducopter.googlecode.com/svn/trunk@1899 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-04-17 05:17:42 +00:00
parent ffa33089b8
commit 87812c9eec
8 changed files with 161 additions and 149 deletions

View File

@ -4,13 +4,17 @@
// GPS is auto-selected
#define MAG_ORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_BACK
#define GPS_PROTOCOL GPS_PROTOCOL_MTK
#define GCS_PROTOCOL GCS_PROTOCOL_NONE
//#define GCS_PORT 0
#define SERIAL0_BAUD 38400
//# define STABILIZE_ROLL_P 0.4
//# define STABILIZE_PITCH_P 0.4
# define STABILIZE_ROLL_P 0.75
# define STABILIZE_PITCH_P 0.75
//# define STABILIZE_DAMPENER 0.1
@ -23,4 +27,14 @@
// Logging
//#define LOG_CURRENT ENABLED
# define LOG_ATTITUDE_FAST DISABLED
# define LOG_ATTITUDE_MED DISABLED
# define LOG_GPS DISABLED
# define LOG_PM DISABLED
# define LOG_CTUN ENABLED
# define LOG_NTUN DISABLED
# define LOG_MODE DISABLED
# define LOG_RAW DISABLED
# define LOG_CMD DISABLED
# define LOG_CURRENT DISABLED

View File

@ -231,6 +231,7 @@ int max_stabilize_dampener; //
int max_yaw_dampener; //
boolean rate_yaw_flag; // used to transition yaw control from Rate control to Yaw hold
byte yaw_debug;
bool did_clear_yaw_control;
// LED output
// ----------
@ -583,6 +584,7 @@ void medium_loop()
// we call these regardless of GPS because of the rapid nature of the yaw sensor
// -----------------------------------------------------------------------------
if(wp_distance < 800){ // 8 meters
//if(g.rc_6.control_in > 500){ // 8 meters
calc_loiter_nav();
}else{
calc_waypoint_nav();
@ -795,6 +797,11 @@ void update_GPS(void)
g_gps->update();
update_GPS_light();
//current_loc.lng = 377697000; // Lon * 10 * *7
//current_loc.lat = -1224318000; // Lat * 10 * *7
//current_loc.alt = 100; // alt * 10 * *7
//return;
if (g_gps->new_data && g_gps->fix) {
// XXX We should be sending GPS data off one of the regular loops so that we send
@ -822,9 +829,6 @@ void update_GPS(void)
}else{
//Serial.printf("init Home!");
if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Startup(TYPE_GROUNDSTART_MSG);
// reset our nav loop timer
//nav_loopTimer = millis();
init_home();
@ -980,14 +984,7 @@ void update_current_flight_mode(void)
nav_pitch = 0;
nav_roll = 0;
//if(g.rc_3.control_in)
// get desired height from the throttle
//next_WP.alt = home.alt + (g.rc_3.control_in); // 0 - 1000 (40 meters)
//next_WP.alt = max(next_WP.alt, 30);
adjust_altitude();
// !!! testing
//next_WP.alt -= 500;
// Yaw control
// -----------

View File

@ -13,6 +13,11 @@ init_pids()
void
control_nav_mixer()
{
// limit the nav pitch and roll of the copter
long pmax = g.pitch_max.get();
nav_roll = constrain(nav_roll, -pmax, pmax);
nav_pitch = constrain(nav_pitch, -pmax, pmax);
// control +- 45° is mixed with the navigation request by the Autopilot
// output is in degrees = target pitch and roll of copter
g.rc_1.servo_out = g.rc_1.control_mix(nav_roll);
@ -149,7 +154,9 @@ output_yaw_with_hold(boolean hold)
if(g.rc_4.control_in == 0){
// we are breaking;
//g.rc_4.servo_out = (omega.z > 0) ? -600 : 600;
g.rc_4.servo_out = (int)((float)g.rc_4.servo_out * (omega.z / 6.0));
// adaptive braking
g.rc_4.servo_out = (int)((1800.0 * omega.z) / 6.0);
yaw_debug = YAW_BRAKE;
}else{
@ -160,8 +167,8 @@ output_yaw_with_hold(boolean hold)
}
// Limit dampening to be equal to propotional term for symmetry
//dampener = rate * g.hold_yaw_dampener; // 34377 * .175 = 6000
//g.rc_4.servo_out -= constrain(dampener, -1800, 1800);
dampener = rate * g.hold_yaw_dampener; // 34377 * .175 = 6000
g.rc_4.servo_out -= constrain(dampener, -1800, 1800);
// Limit Output
g.rc_4.servo_out = constrain(g.rc_4.servo_out, -1800, 1800); // limit to 24°
@ -263,7 +270,11 @@ yaw control
void output_manual_yaw()
{
if(g.rc_3.control_in == 0){
clear_yaw_control();
// we want to only call this once
if(did_clear_yaw_control == false){
clear_yaw_control();
did_clear_yaw_control = true;
}
}else{
// Yaw control
if(g.rc_4.control_in == 0){
@ -271,6 +282,8 @@ void output_manual_yaw()
}else{
output_yaw_with_hold(false); // rate control yaw
}
did_clear_yaw_control = false;
}
}

View File

@ -77,9 +77,8 @@ print_log_menu(void)
if (last_log_num == 0) {
Serial.printf_P(PSTR("\nNo logs\n"));
}else{
Serial.printf_P(PSTR("\n%d logs\n"), last_log_num);
for(int i=1;i<last_log_num+1;i++) {
for(int i = 1; i < last_log_num + 1; i++) {
get_log_boundaries(last_log_num, i, log_start, log_end);
Serial.printf_P(PSTR("Log # %d, start %d, end %d\n"),
i, log_start, log_end);
@ -112,7 +111,7 @@ dump_log(uint8_t argc, const Menu::arg *argv)
dump_log_end);
Log_Read(dump_log_start, dump_log_end);
Serial.printf_P(PSTR("Complete\n"));
Serial.printf_P(PSTR("Done\n"));
}
static int8_t
@ -227,31 +226,36 @@ void start_new_log(byte num_existing_logs)
int end_pages[50] = {0,0,0};
byte data;
if(num_existing_logs > 0) {
if (num_existing_logs > 0) {
for(int i=0;i<num_existing_logs;i++) {
get_log_boundaries(num_existing_logs, i+1,start_pages[i],end_pages[i]);
}
end_pages[num_existing_logs - 1] = find_last_log_page(start_pages[num_existing_logs - 1]);
}
if(end_pages[num_existing_logs - 1] < 4095 && num_existing_logs < MAX_NUM_LOGS) {
if (end_pages[num_existing_logs - 1] < 4095 && num_existing_logs < MAX_NUM_LOGS) {
if(num_existing_logs > 0)
start_pages[num_existing_logs] = end_pages[num_existing_logs - 1] + 1;
else
start_pages[0] = 2;
num_existing_logs++;
DataFlash.StartWrite(1);
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_INDEX_MSG);
DataFlash.WriteByte(num_existing_logs);
for(int i=0;i<MAX_NUM_LOGS;i++) {
DataFlash.WriteInt(start_pages[i]);
DataFlash.WriteInt(end_pages[i]);
}
DataFlash.WriteByte(END_BYTE);
DataFlash.FinishWrite();
DataFlash.StartWrite(start_pages[num_existing_logs-1]);
}else{
gcs.send_text_P(SEVERITY_LOW,PSTR("<start_new_log> Logs full"));
}
@ -364,26 +368,6 @@ void Log_Write_Cmd(byte num, struct Location *wp)
DataFlash.WriteByte(END_BYTE);
}
void Log_Write_Startup(byte type)
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_STARTUP_MSG);
DataFlash.WriteByte(type);
DataFlash.WriteByte(g.waypoint_total);
DataFlash.WriteByte(END_BYTE);
// create a location struct to hold the temp Waypoints for printing
struct Location cmd = get_wp_with_index(0);
Log_Write_Cmd(0, &cmd);
for (int i = 1; i <= g.waypoint_total; i++){
cmd = get_wp_with_index(i);
Log_Write_Cmd(i, &cmd);
}
}
// Write a control tuning packet. Total length : 22 bytes
#if HIL_MODE != HIL_MODE_ATTITUDE
void Log_Write_Control_Tuning()
@ -585,20 +569,6 @@ void Log_Read_Cmd()
Serial.println(" ");
}
void Log_Read_Startup()
{
byte logbyte = DataFlash.ReadByte();
if (logbyte == TYPE_AIRSTART_MSG)
Serial.printf_P(PSTR("AIR START - "));
else if (logbyte == TYPE_GROUNDSTART_MSG)
Serial.printf_P(PSTR("GROUND START - "));
else
Serial.printf_P(PSTR("UNKNOWN STARTUP - "));
Serial.printf_P(PSTR(" %d commands in memory\n"),(int)DataFlash.ReadByte());
}
// Read an attitude packet
void Log_Read_Attitude()
{
@ -648,26 +618,30 @@ void Log_Read_Raw()
void Log_Read(int start_page, int end_page)
{
byte data;
byte log_step = 0;
int packet_count = 0;
int page = start_page;
byte log_step = 0;
int packet_count = 0;
int page = start_page;
DataFlash.StartRead(start_page);
while (page < end_page && page != -1){
data = DataFlash.ReadByte();
switch(log_step) // This is a state machine to read the packets
{
// This is a state machine to read the packets
switch(log_step){
case 0:
if(data == HEAD_BYTE1) // Head byte 1
log_step++;
break;
case 1:
if(data == HEAD_BYTE2) // Head byte 2
log_step++;
else
log_step = 0;
break;
case 2:
if(data == LOG_ATTITUDE_MSG){
Log_Read_Attitude();
@ -702,30 +676,32 @@ void Log_Read(int start_page, int end_page)
log_step++;
}else if(data == LOG_STARTUP_MSG){
Log_Read_Startup();
// not implemented
log_step++;
}else {
if(data == LOG_GPS_MSG){
Log_Read_GPS();
log_step++;
}else{
Serial.printf_P(PSTR("Error Reading Packet: %d\n"),packet_count);
log_step = 0; // Restart, we have a problem...
}
}else if(data == LOG_GPS_MSG){
Log_Read_GPS();
log_step++;
}else{
Serial.printf_P(PSTR("Error P: %d\n"),packet_count);
log_step = 0; // Restart, we have a problem...
}
break;
case 3:
if(data == END_BYTE){
packet_count++;
}else{
Serial.printf_P(PSTR("Error Reading END_BYTE: %d\n"),data);
Serial.printf_P(PSTR("Error EB: %d\n"),data);
}
log_step = 0; // Restart sequence: new packet...
break;
}
page = DataFlash.GetPage();
}
Serial.printf_P(PSTR("# of packets read: %d\n"), packet_count);
page = DataFlash.GetPage();
}
//Serial.printf_P(PSTR("# of packets read: %d\n"), packet_count);
}

View File

@ -255,6 +255,14 @@
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// Y6 Support
#ifndef Y6_MOTOR_SCALER
# define Y6_MOTOR_SCALER 0.92
#endif
//////////////////////////////////////////////////////////////////////////////
// ACRO Rate Control
@ -312,7 +320,7 @@
# define STABILIZE_ROLL_I 0.1 //
#endif
#ifndef STABILIZE_ROLL_D
# define STABILIZE_ROLL_D 0.11
# define STABILIZE_ROLL_D 0.13
#endif
#ifndef STABILIZE_ROLL_IMAX
# define STABILIZE_ROLL_IMAX 10 // 10 degrees
@ -325,13 +333,12 @@
# define STABILIZE_PITCH_I 0.1
#endif
#ifndef STABILIZE_PITCH_D
# define STABILIZE_PITCH_D 0.11
# define STABILIZE_PITCH_D 0.13
#endif
#ifndef STABILIZE_PITCH_IMAX
# define STABILIZE_PITCH_IMAX 10
#endif
//////////////////////////////////////////////////////////////////////////////
// YAW Control
//

View File

@ -59,7 +59,7 @@ set_servos_4()
g.rc_3.servo_out = constrain(g.rc_3.servo_out, 0, 1000);
if(g.rc_3.servo_out > 0)
out_min = g.rc_3.radio_min + 60;
out_min = g.rc_3.radio_min + 90;
//Serial.printf("out: %d %d %d %d\t\t", g.rc_1.servo_out, g.rc_2.servo_out, g.rc_3.servo_out, g.rc_4.servo_out);
@ -71,7 +71,8 @@ set_servos_4()
// limit Yaw control so we don't clip and loose altitude
// this is only a partial solution.
//g.rc_4.pwm_out = min(g.rc_4.pwm_out, (g.rc_3.radio_out - out_min));
// g.rc_4.pwm_out = min(g.rc_4.pwm_out, (g.rc_3.radio_out - out_min));
//Serial.printf("out: %d %d %d %d\n", g.rc_1.radio_out, g.rc_2.radio_out, g.rc_3.radio_out, g.rc_4.radio_out);
//Serial.printf("yaw: %d ", g.rc_4.radio_out);
@ -163,15 +164,15 @@ set_servos_4()
int pitch_out = g.rc_2.pwm_out / 2;
//left
motor_out[CH_2] = ((g.rc_3.radio_out * 0.92) + roll_out + pitch_out); // CCW TOP
motor_out[CH_2] = ((g.rc_3.radio_out * Y6_MOTOR_SCALER) + roll_out + pitch_out); // CCW TOP
motor_out[CH_3] = g.rc_3.radio_out + roll_out + pitch_out; // CW
//right
motor_out[CH_7] = ((g.rc_3.radio_out * 0.92) - roll_out + pitch_out); // CCW TOP
motor_out[CH_7] = ((g.rc_3.radio_out * Y6_MOTOR_SCALER) - roll_out + pitch_out); // CCW TOP
motor_out[CH_1] = g.rc_3.radio_out - roll_out + pitch_out; // CW
//back
motor_out[CH_8] = ((g.rc_3.radio_out * 0.92) - g.rc_2.pwm_out); // CCW TOP
motor_out[CH_8] = ((g.rc_3.radio_out * Y6_MOTOR_SCALER) - g.rc_2.pwm_out); // CCW TOP
motor_out[CH_4] = g.rc_3.radio_out - g.rc_2.pwm_out; // CW
//yaw
@ -223,6 +224,13 @@ set_servos_4()
//g.pid_baro_throttle.kD((float)g.rc_6.control_in / 1000); // 0 to 1
//g.pid_baro_throttle.kP((float)g.rc_6.control_in / 4000); // 0 to .25
// uncomment me out to try in flight dampening, 0 = unflyable, .2 = unfun, .13 worked for me.
// use test,radio to get the value to use in your config.
//g.stabilize_dampener.set((float)g.rc_6.control_in / 1000.0);
/*
// ROLL and PITCH
// make sure you init_pids() after changing the kP
@ -239,16 +247,41 @@ set_servos_4()
init_pids();
//*/
/*
Serial.printf("yaw: %d, lat_e: %ld, lng_e: %ld, \tnlat: %ld, nlng: %ld,\tnrll: %ld, nptc: %ld, \tcx: %.2f, sy: %.2f, \ttber: %ld, \tnber: %ld\n",
(int)(dcm.yaw_sensor / 100),
lat_error,
long_error,
nav_lat,
nav_lon,
nav_roll,
nav_pitch,
cos_yaw_x,
sin_yaw_y,
target_bearing,
nav_bearing);
//*/
/*
gcs_simple.write_byte(control_mode);
//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)(dcm.yaw_sensor / 100));
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);
//gcs_simple.write_int((int)(cos_yaw_x * 100));
//gcs_simple.write_int((int)(sin_yaw_y * 100));
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
@ -257,10 +290,9 @@ 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));
gcs_simple.write_int((int)(target_bearing / 100));
gcs_simple.write_int((int)(nav_bearing / 100));
gcs_simple.write_int((int)(nav_yaw / 100));
if(altitude_sensor == BARO){
gcs_simple.write_int((int)g.pid_baro_throttle.get_integrator());
@ -269,17 +301,15 @@ set_servos_4()
}
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));
gcs_simple.write_int(g.throttle_cruise);
//24
gcs_simple.flush(10); // Message ID
//*/
//Serial.printf("\n tb %d\n", (int)(target_bearing / 100));
//Serial.printf("\n nb %d\n", (int)(nav_bearing / 100));
//Serial.printf("\n dcm %d\n", (int)(dcm.yaw_sensor / 100));
/*Serial.printf("a %ld, e %ld, i %d, t %d, b %4.2f\n",
current_loc.alt,

View File

@ -108,10 +108,6 @@ void calc_loiter_nav()
//EAST -1000 * -1 + 1000 * 0 = 1000 // pitch back
//SOUTH -1000 * 0 + 1000 * -1 = -1000 // pitch forward
long pmax = g.pitch_max.get();
nav_roll = constrain(nav_roll, -pmax, pmax);
nav_pitch = constrain(nav_pitch, -pmax, pmax);
}
void calc_waypoint_nav()
@ -129,9 +125,6 @@ void calc_waypoint_nav()
nav_roll = (float)nav_lat * cos_nav_x;
nav_pitch = -(float)nav_lat * sin_nav_y;
long pmax = g.pitch_max.get();
nav_roll = constrain(nav_roll, -pmax, pmax);
nav_pitch = constrain(nav_pitch, -pmax, pmax);
}
void calc_bearing_error()

View File

@ -41,10 +41,6 @@ MENU(main_menu, "ACM", main_menu_commands);
void init_ardupilot()
{
byte last_log_num;
int last_log_start;
int last_log_end;
// Console serial port
//
// The console port buffers are defined to be sufficiently large to support
@ -67,9 +63,9 @@ void init_ardupilot()
// XXX the 128 byte receive buffer may be too small for NMEA, depending
// on the message set configured.
//
#if GPS_PROTOCOL != GPS_PROTOCOL_IMU
#if GPS_PROTOCOL != GPS_PROTOCOL_IMU
Serial1.begin(38400, 128, 16);
#endif
#endif
// Telemetry port.
//
@ -81,8 +77,8 @@ void init_ardupilot()
//
Serial3.begin(SERIAL3_BAUD, 128, 128);
Serial.printf_P(PSTR("\n\nInit ArduCopterMega"
"\n\nFree RAM: %lu\n"),
Serial.printf_P(PSTR("\n\nInit ACM"
"\n\nRAM: %lu\n"),
freeRAM());
@ -131,21 +127,22 @@ void init_ardupilot()
// Serial.printf_P(PSTR("using %u bytes of memory\n"), AP_Var::get_memory_use());
}
#ifdef RADIO_OVERRIDE_DEFAULTS
#ifdef RADIO_OVERRIDE_DEFAULTS
{
int16_t rc_override[8] = RADIO_OVERRIDE_DEFAULTS;
APM_RC.setHIL(rc_override);
}
#endif
#endif
init_rc_in(); // sets up rc channels from radio
init_rc_out(); // sets up the timer libs
init_camera();
#if HIL_MODE != HIL_MODE_ATTITUDE
#if HIL_MODE != HIL_MODE_ATTITUDE
adc.Init(); // APM ADC library initialization
barometer.Init(); // APM Abs Pressure sensor initialization
#endif
DataFlash.Init(); // DataFlash log initialization
#endif
// Do GPS init
//g_gps = &GPS;
@ -159,18 +156,14 @@ void init_ardupilot()
gcs.init(&Serial);
#endif
if (g.log_bitmask & MASK_LOG_MODE)
Log_Write_Mode(control_mode);
if(g.compass_enabled)
init_compass();
#if HIL_MODE != HIL_MODE_ATTITUDE
#if HIL_MODE != HIL_MODE_ATTITUDE
if(g.sonar_enabled){
sonar.init(SONAR_PIN, &adc);
}
#endif
#endif
pinMode(C_LED_PIN, OUTPUT); // GPS status LED
pinMode(A_LED_PIN, OUTPUT); // GPS status LED
@ -198,33 +191,30 @@ void init_ardupilot()
}
}
if(g.log_bitmask > 0){
// TODO - Here we will check on the length of the last log
// We don't want to create a bunch of little logs due to powering on and off
last_log_num = get_num_logs();
start_new_log(last_log_num);
}
// read in the flight switches
update_servo_switches();
// read in the flight switches
//update_servo_switches();
//imu.init_gyro(IMU::WARM_START);
// All of the Gyro calibrations
// ----------------------------
startup_ground();
if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Startup(TYPE_GROUNDSTART_MSG);
if (g.log_bitmask & MASK_LOG_SET_DEFAULTS) {
default_log_bitmask();
}
// set the correct flight mode
// ---------------------------
reset_control_switch();
// Logging:
// --------
DataFlash.Init(); // DataFlash log initialization
// setup the log bitmask
if (g.log_bitmask & MASK_LOG_SET_DEFAULTS)
default_log_bitmask();
if(g.log_bitmask != 0){
// TODO - Here we will check on the length of the last log
// We don't want to create a bunch of little logs due to powering on and off
byte last_log_num = get_num_logs();
start_new_log(last_log_num);
}
if (g.log_bitmask & MASK_LOG_MODE)
Log_Write_Mode(control_mode);
}
//********************************************************************************
@ -257,14 +247,6 @@ void startup_ground(void)
report_imu();
#endif
// read the radio to set trims
// ---------------------------
//trim_radio();
if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Startup(TYPE_GROUNDSTART_MSG);
#if HIL_MODE != HIL_MODE_ATTITUDE
// read Baro pressure at ground
//-----------------------------