zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

updates to get alt hold from 2.0.7. 

Cleanups, better logs for Alt hold, yaw debugging
I tuned up Hein's Hexa Yaw. I think we may need to adjust Yaw gains based on Frame type.
Crossing fingers, the next rev is the final.

git-svn-id: https://arducopter.googlecode.com/svn/trunk@2381 f9c3cf11-9bcb-44bc-f272-b75c42450872
mission-4.1.18
jasonshort 14 years ago
parent
c2ff95371c
commit
78f02b51a0
6 changed files with 134 additions and 168 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 125
      ArduCopterMega/ArduCopterMega.pde
  2. 6
      ArduCopterMega/Attitude.pde
  3. 165
      ArduCopterMega/Log.pde
  4. 3
      ArduCopterMega/commands.pde
  5. 2
      ArduCopterMega/system.pde
  6. 1
      ArduCopterMega/test.pde

125
ArduCopterMega/ArduCopterMega.pde
Unescape View File

@ -272,7 +272,7 @@ float cos_roll_x = 1; @@ -272,7 +272,7 @@ float cos_roll_x = 1;
float cos_pitch_x = 1;
float cos_yaw_x = 1;
float sin_pitch_y, sin_yaw_y, sin_roll_y;
float sin_nav_y, cos_nav_x; // used in calc_waypoint_nav
float sin_nav_y, cos_nav_x; // used in calc_nav_output XXX move to local funciton
bool simple_bearing_is_set = false;
long initial_simple_bearing; // used for Simple mode
@ -922,20 +922,11 @@ void update_current_flight_mode(void) @@ -922,20 +922,11 @@ void update_current_flight_mode(void)
}
switch(command_must_ID){
//case MAV_CMD_NAV_TAKEOFF:
// break;
//case MAV_CMD_NAV_LAND:
// break;
default:
// Output Pitch, Roll, Yaw and Throttle
// ------------------------------------
auto_yaw();
//if(invalid_nav)
//calc_waypoint_nav();
// mix in user control
control_nav_mixer();
@ -1107,6 +1098,13 @@ void update_current_flight_mode(void) @@ -1107,6 +1098,13 @@ void update_current_flight_mode(void)
adjust_altitude();
#if AUTO_RESET_LOITER == 1
if((g.rc_2.control_in + g.rc_1.control_in) != 0){
// reset LOITER to current position
next_WP = current_loc;
}
#endif
// Output Pitch, Roll, Yaw and Throttle
// ------------------------------------
@ -1136,66 +1134,51 @@ void update_current_flight_mode(void) @@ -1136,66 +1134,51 @@ void update_current_flight_mode(void)
}
}
void update_nav_wp()
{
if(wp_control == LOITER_MODE){
// calc a pitch to the target
calc_loiter_nav();
// rotate pitch and roll to the copter frame of reference
calc_loiter_output();
} else {
// how far are we from the ideal trajectory?
// this pushes us back on course
update_crosstrack();
// calc a rate dampened pitch to the target
calc_rate_nav();
// rotate that pitch to the copter frame of reference
calc_nav_output();
}
}
// called after a GPS read
void update_navigation()
{
// wp_distance is in ACTUAL meters, not the *100 meters we get from the GPS
// ------------------------------------------------------------------------
switch(control_mode){
case AUTO:
verify_commands();
// distance and bearing calcs only
if(control_mode == AUTO){
verify_commands();
// note: wp_control is handled by commands_logic
update_nav_wp();
// calculates desired Yaw
update_nav_yaw();
// this tracks a location so the copter is always pointing towards it.
if(yaw_tracking == MAV_ROI_LOCATION){
nav_yaw = get_bearing(&current_loc, &target_WP);
// calculates the desired Roll and Pitch
update_nav_wp();
break;
}else if(yaw_tracking == MAV_ROI_WPNEXT){
nav_yaw = target_bearing;
}
case LOITER:
case RTL:
// are we Traversing or Loitering?
wp_control = (wp_distance < 10) ? LOITER_MODE : WP_MODE;
}else{
// calculates desired Yaw
update_nav_yaw();
switch(control_mode){
case LOITER:
wp_control = LOITER_MODE;
update_nav_wp();
break;
// calculates the desired Roll and Pitch
update_nav_wp();
break;
#if YAW_HACK == 1
case SIMPLE:
// calculates desired Yaw
// exprimental_hack
if(g.rc_6.control_in > 900)
update_nav_yaw();
if(g.rc_6.control_in < 100){
nav_yaw = dcm.yaw_sensor;
}
break;
#endif
case RTL:
wp_control = (wp_distance < 700) ? LOITER_MODE : WP_MODE;
update_nav_wp();
break;
}
}
}
void read_AHRS(void)
{
// Perform IMU calculations and get attitude info
@ -1337,3 +1320,35 @@ void tuning(){ @@ -1337,3 +1320,35 @@ void tuning(){
#endif
}
void update_nav_wp()
{
if(wp_control == LOITER_MODE){
// calc a pitch to the target
calc_loiter_nav();
// rotate pitch and roll to the copter frame of reference
calc_loiter_output();
} else {
// how far are we from the ideal trajectory?
// this pushes us back on course
update_crosstrack();
// calc a rate dampened pitch to the target
calc_rate_nav();
// rotate that pitch to the copter frame of reference
calc_nav_output();
}
}
void update_nav_yaw()
{
// this tracks a location so the copter is always pointing towards it.
if(yaw_tracking == MAV_ROI_LOCATION){
nav_yaw = get_bearing(&current_loc, &target_WP);
}else if(yaw_tracking == MAV_ROI_WPNEXT){
nav_yaw = target_bearing;
}
}

6
ArduCopterMega/Attitude.pde
Unescape View File

@ -132,12 +132,12 @@ void calc_nav_throttle() @@ -132,12 +132,12 @@ void calc_nav_throttle()
if(error < 0){
// try and prevent rapid fall
scaler = (altitude_sensor == BARO) ? .5 : .9;
//scaler = (altitude_sensor == BARO) ? 1 : 1;
}
if(altitude_sensor == BARO){
nav_throttle = g.pid_baro_throttle.get_pid(error, dTnav2, scaler); // .25
nav_throttle = g.throttle_cruise + constrain(nav_throttle, -50, 100);
nav_throttle = g.throttle_cruise + constrain(nav_throttle, -70, 140);
}else{
nav_throttle = g.pid_sonar_throttle.get_pid(error, dTnav2, scaler); // .5
nav_throttle = g.throttle_cruise + constrain(nav_throttle, -70, 150);
@ -260,7 +260,7 @@ output_yaw_with_hold(boolean hold) @@ -260,7 +260,7 @@ output_yaw_with_hold(boolean hold)
}else{
// RATE control
// Hein, 5/21/11
long error = ((long)g.rc_4.control_in * 6) - (rate * 3); // control is += 6000 * 6 = 36000
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
}

165
ArduCopterMega/Log.pde
Unescape View File

@ -52,26 +52,21 @@ print_log_menu(void) @@ -52,26 +52,21 @@ print_log_menu(void)
byte last_log_num = get_num_logs();
Serial.printf_P(PSTR("logs enabled: "));
if (0 == g.log_bitmask) {
Serial.printf_P(PSTR("none"));
}else{
// Macro to make the following code a bit easier on the eye.
// Pass it the capitalised name of the log option, as defined
// in defines.h but without the LOG_ prefix. It will check for
// the bit being set and print the name of the log option to suit.
#define PLOG(_s) if (g.log_bitmask & MASK_LOG_ ## _s) Serial.printf_P(PSTR(" %S"), PSTR(#_s))
PLOG(ATTITUDE_FAST);
PLOG(ATTITUDE_MED);
PLOG(GPS);
PLOG(PM);
PLOG(CTUN);
PLOG(NTUN);
PLOG(MODE);
PLOG(RAW);
PLOG(CMD);
PLOG(CURRENT);
#undef PLOG
if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST) Serial.printf_P(PSTR(" ATTITUDE_FAST"));
if (g.log_bitmask & MASK_LOG_ATTITUDE_MED) Serial.printf_P(PSTR(" ATTITUDE_MED"));
if (g.log_bitmask & MASK_LOG_GPS) Serial.printf_P(PSTR(" GPS"));
if (g.log_bitmask & MASK_LOG_PM) Serial.printf_P(PSTR(" PM"));
if (g.log_bitmask & MASK_LOG_CTUN) Serial.printf_P(PSTR(" CTUN"));
if (g.log_bitmask & MASK_LOG_NTUN) Serial.printf_P(PSTR(" NTUN"));
if (g.log_bitmask & MASK_LOG_RAW) Serial.printf_P(PSTR(" RAW"));
if (g.log_bitmask & MASK_LOG_CMD) Serial.printf_P(PSTR(" CMD"));
if (g.log_bitmask & MASK_LOG_CURRENT) Serial.printf_P(PSTR(" CURRENT"));
}
Serial.println();
if (last_log_num == 0) {
@ -108,13 +103,13 @@ dump_log(uint8_t argc, const Menu::arg *argv) @@ -108,13 +103,13 @@ dump_log(uint8_t argc, const Menu::arg *argv)
}
get_log_boundaries(last_log_num, dump_log, dump_log_start, dump_log_end);
Serial.printf_P(PSTR("Dumping Log number %d, start %d, end %d\n"),
/*Serial.printf_P(PSTR("Dumping Log number %d, start %d, end %d\n"),
dump_log,
dump_log_start,
dump_log_end);
*/
Log_Read(dump_log_start, dump_log_end);
Serial.printf_P(PSTR("Done\n"));
//Serial.printf_P(PSTR("Done\n"));
}
static int8_t
@ -354,74 +349,6 @@ int find_last_log_page(int bottom_page) @@ -354,74 +349,6 @@ int find_last_log_page(int bottom_page)
return top_page;
}
void Log_Write_Nav_Tuning()
{
Matrix3f tempmat = dcm.get_dcm_matrix();
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_NAV_TUNING_MSG);
DataFlash.WriteInt((int)(dcm.yaw_sensor/100)); // 1
DataFlash.WriteInt((int)wp_distance); // 2
DataFlash.WriteByte(wp_verify_byte); // 3
DataFlash.WriteInt((int)(target_bearing/100)); // 4
DataFlash.WriteInt((int)(nav_bearing/100)); // 5
DataFlash.WriteInt((int)(g.rc_3.servo_out)); // 6
DataFlash.WriteByte(altitude_sensor); // 7
DataFlash.WriteInt((int)sonar_alt); // 8
DataFlash.WriteInt((int)baro_alt); // 9
DataFlash.WriteInt((int)home.alt); // 10
DataFlash.WriteInt((int)next_WP.alt); // 11
DataFlash.WriteInt((int)altitude_error); // 12
DataFlash.WriteInt((int)(wrap_360(ToDeg(compass.heading)*100)/100)); // Just a temp hack
DataFlash.WriteLong(compass.last_update); // Just a temp hack
DataFlash.WriteInt((int)(tempmat.b.x*1000)); // Just a temp hack
DataFlash.WriteInt((int)(compass.heading_x*1000)); // Just a temp hack
DataFlash.WriteInt((int)(tempmat.a.x*1000)); // Just a temp hack
DataFlash.WriteInt((int)(compass.heading_y*1000)); // Just a temp hack
DataFlash.WriteByte(END_BYTE);
}
// 1 2 3 4 5 6 7 8 9 10 11
//NTUN, 236, 0, 132, 10, 0, 0, 29, 2963, 16545, 16682, 108
void Log_Read_Nav_Tuning()
{
// 1 2 3 4 5 6 7 8 9 10 11
Serial.printf_P(PSTR( "NTUN, %d, %d, %d, "
"%d, %d, "
"%d, %d, %d, %d, "
"%d, %d, %d, "
"%d, %ld, %4.4f, %4.4f, %4.4f, %4.4f\n"),
DataFlash.ReadInt(), //yaw sensor
DataFlash.ReadInt(), //distance
DataFlash.ReadByte(), //bitmask
DataFlash.ReadInt(), //target bearing
DataFlash.ReadInt(), //nav bearing
DataFlash.ReadInt(), //throttle
DataFlash.ReadByte(), //Alt sensor
DataFlash.ReadInt(), //Sonar
DataFlash.ReadInt(), //Baro
DataFlash.ReadInt(), //home.alt
DataFlash.ReadInt(), //Next_alt
DataFlash.ReadInt(), //Alt Error
DataFlash.ReadInt(),
DataFlash.ReadLong(),
(float)DataFlash.ReadInt()/1000,
(float)DataFlash.ReadInt()/1000,
(float)DataFlash.ReadInt()/1000,
(float)DataFlash.ReadInt()/1000);
}
// Write an GPS packet. Total length : 30 bytes
void Log_Write_GPS()
{
@ -516,6 +443,31 @@ void Log_Read_Current() @@ -516,6 +443,31 @@ void Log_Read_Current()
DataFlash.ReadInt());
}
void Log_Write_Nav_Tuning()
{
Matrix3f tempmat = dcm.get_dcm_matrix();
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_NAV_TUNING_MSG);
DataFlash.WriteInt((int)wp_distance); // 1
DataFlash.WriteByte(wp_verify_byte); // 2
DataFlash.WriteInt((int)(target_bearing/100)); // 3
DataFlash.WriteInt((int)(nav_bearing/100)); // 4
DataFlash.WriteByte(END_BYTE);
}
void Log_Read_Nav_Tuning()
{
// 1 2 3 4
Serial.printf_P(PSTR( "NTUN, %d, %d, %d, %d\n"),
DataFlash.ReadInt(), //distance
DataFlash.ReadByte(), //bitmask
DataFlash.ReadInt(), //target bearing
DataFlash.ReadInt()); //nav bearing
}
// Write a control tuning packet. Total length : 22 bytes
#if HIL_MODE != HIL_MODE_ATTITUDE
@ -526,29 +478,24 @@ void Log_Write_Control_Tuning() @@ -526,29 +478,24 @@ void Log_Write_Control_Tuning()
DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG);
// Control
DataFlash.WriteInt((int)(g.rc_3.control_in));
DataFlash.WriteInt((int)(g.rc_3.servo_out));
DataFlash.WriteInt((int)(g.rc_4.control_in));
DataFlash.WriteInt((int)(g.rc_4.servo_out));
// yaw
DataFlash.WriteByte(yaw_debug);
DataFlash.WriteInt((int)yaw_error);
DataFlash.WriteInt((int)(dcm.yaw_sensor/100));
DataFlash.WriteInt((int)(nav_yaw/100));
DataFlash.WriteInt((int)yaw_error);
DataFlash.WriteInt((int)(omega.z * 1000));
// Pitch/roll
DataFlash.WriteInt((int)(dcm.pitch_sensor/100));
DataFlash.WriteInt((int)(dcm.roll_sensor/100));
// Alt hold
DataFlash.WriteInt((int)(g.rc_3.servo_out));
DataFlash.WriteInt((int)sonar_alt); //
DataFlash.WriteInt((int)baro_alt); //
DataFlash.WriteInt((int)g.throttle_cruise);
DataFlash.WriteInt((int)next_WP.alt); //
DataFlash.WriteInt((int)altitude_error); //
DataFlash.WriteInt((int)g.pid_baro_throttle.get_integrator());
DataFlash.WriteInt((int)g.pid_sonar_throttle.get_integrator());
// Position
//DataFlash.WriteInt((int)dcm.pitch_sensor);
//DataFlash.WriteInt((int)dcm.roll_sensor);
//DataFlash.WriteInt((int)(dcm.yaw_sensor/10));
DataFlash.WriteByte(END_BYTE);
}
@ -557,28 +504,26 @@ void Log_Write_Control_Tuning() @@ -557,28 +504,26 @@ void Log_Write_Control_Tuning()
// Read an control tuning packet
void Log_Read_Control_Tuning()
{
Serial.printf_P(PSTR( "CTUN, %d, %d, %d, %d, "
"%d, %d, %d, %1.4f, "
"%d, %d, "
"%d, %d, %d\n"),
Serial.printf_P(PSTR( "CTUN, %d, %d, "
"%d, %d, %d, %d, %1.4f, "
"%d, %d, %d, %d, %d, %d\n"),
// Control
DataFlash.ReadInt(),
DataFlash.ReadInt(),
DataFlash.ReadInt(),
DataFlash.ReadInt(),
// yaw
(int)DataFlash.ReadByte(),
DataFlash.ReadInt(),
DataFlash.ReadInt(),
DataFlash.ReadInt(),
(float)DataFlash.ReadInt() / 1000.0,// Gyro Rate
// Pitch/roll
// Alt Hold
DataFlash.ReadInt(),
DataFlash.ReadInt(),
DataFlash.ReadInt(),
// Alt Hold
DataFlash.ReadInt(),
DataFlash.ReadInt(),
DataFlash.ReadInt());
@ -700,6 +645,7 @@ void Log_Write_Mode(byte mode) @@ -700,6 +645,7 @@ void Log_Write_Mode(byte mode)
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_MODE_MSG);
DataFlash.WriteByte(mode);
DataFlash.WriteInt((int)g.throttle_cruise);
DataFlash.WriteByte(END_BYTE);
}
@ -707,7 +653,8 @@ void Log_Write_Mode(byte mode) @@ -707,7 +653,8 @@ void Log_Write_Mode(byte mode)
void Log_Read_Mode()
{
Serial.printf_P(PSTR("MOD:"));
Serial.println(flight_mode_strings[DataFlash.ReadByte()]);
Serial.print(flight_mode_strings[DataFlash.ReadByte()]);
Serial.printf_P(PSTR(", %d\n"),DataFlash.ReadInt());
}
// Read a raw accel/gyro packet

3
ArduCopterMega/commands.pde
Unescape View File

@ -227,6 +227,9 @@ void init_home() @@ -227,6 +227,9 @@ void init_home()
home.alt = max(g_gps->altitude, 0); // we sometimes get negatives from GPS, not valid
home_is_set = true;
// to point yaw towards home until we set it with Mavlink
target_WP = home;
//Serial.printf_P(PSTR("gps alt: %ld\n"), home.alt);
// Save Home to EEPROM

2
ArduCopterMega/system.pde
Unescape View File

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

1
ArduCopterMega/test.pde
Unescape View File

@ -518,6 +518,7 @@ test_gps(uint8_t argc, const Menu::arg *argv) @@ -518,6 +518,7 @@ test_gps(uint8_t argc, const Menu::arg *argv)
g_gps->longitude,
g_gps->altitude/100,
g_gps->num_sats);
g_gps->new_data = false;
}else{
Serial.print(".");
}

Write Preview
Loading…
Cancel
Save