Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 18:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

updates to get alt hold from 2.0.7.

Browse Source 
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
This commit is contained in:
jasonshort 2011-05-23 05:53:00 +00:00
parent 4017bb39d1
commit 9e01cf9eb9
6 changed files with 134 additions and 168 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

125
ArduCopterMega/ArduCopterMega.pde
View File

@ -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)
}
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)
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)
}
}
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(){
#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
View File

@ -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)
}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
View File

@ -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)
}
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)
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()
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()
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()
// 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)
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)
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
View File

@ -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
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.8 Beta", main_menu_commands);
MENU(main_menu, "AC 2.0.9 Beta", main_menu_commands);
void init_ardupilot()
{

1
ArduCopterMega/test.pde
View File

@ -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(".");
}