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

Added throttle_integrator

Browse Source 
Removed cos for nav_rate testing.

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1934 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-04-30 16:01:57 +00:00
parent 56fd1acc4a
commit dc45d3e8c4
3 changed files with 16 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ArduCopterMega/ArduCopterMega.pde
View File

@ -342,6 +342,7 @@ long nav_lon; // for error calcs
int nav_throttle; // 0-1000 for throttle control
int nav_throttle_old; // for filtering
long throttle_integrator; // used to control when we calculate nav_throttle
bool invalid_throttle; // used to control when we calculate nav_throttle
bool invalid_nav; // used to control when we calculate nav_throttle
bool set_throttle_cruise_flag = false; // used to track the throttle crouse value
@ -521,6 +522,10 @@ void fast_loop()
// ------------------------------
set_servos_4();
// record throttle output
// ------------------------------
throttle_integrator += g.rc_3.servo_out;
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK
// HIL for a copter needs very fast update of the servo values
gcs.send_message(MSG_RADIO_OUT);

8
ArduCopterMega/Log.pde
View File

@ -496,18 +496,24 @@ void Log_Write_Current()
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_CURRENT_MSG);
DataFlash.WriteInt(g.rc_3.control_in);
DataFlash.WriteLong(throttle_integrator);
DataFlash.WriteInt((int)(current_voltage * 100.0));
DataFlash.WriteInt((int)(current_amps * 100.0));
DataFlash.WriteInt((int)current_total);
DataFlash.WriteByte(END_BYTE);
}
// Read a Current packet
void Log_Read_Current()
{
Serial.printf_P(PSTR("CURR: %d, %4.4f, %4.4f, %d\n"),
Serial.printf_P(PSTR("CURR: %d, %ld, %4.4f, %4.4f, %d\n"),
DataFlash.ReadInt(),
DataFlash.ReadLong(),
((float)DataFlash.ReadInt() / 100.f),
((float)DataFlash.ReadInt() / 100.f),
DataFlash.ReadInt());

9
ArduCopterMega/navigation.pde
View File

@ -34,7 +34,6 @@ void navigate()
// nav_bearing will includes xtrac correction
// ------------------------------------------
nav_bearing = target_bearing;
}
bool check_missed_wp()
@ -122,6 +121,7 @@ void calc_nav_output()
nav_roll = (float)nav_lat * cos_nav_x;
nav_pitch = -(float)nav_lat * sin_nav_y;
}
#define WAYPOINT_SPEED 450
#if NAV_TEST == 0
@ -144,7 +144,6 @@ void calc_rate_nav()
// dampen our response
nav_lat -= constrain(dampening, -1800, 1800); // +- 20m max error
}
#else
@ -161,7 +160,7 @@ void calc_rate_nav()
target_error = wrap_180(target_error);
// calc the cos of the error to tell how fast we are moving towards the target
error = (float)error * cos(radians((float)target_error/100));
//error = (float)error * cos(radians((float)target_error/100));
// Scale response by kP
long nav_lat = g.pid_nav_wp.kP() * error;
@ -171,10 +170,10 @@ void calc_rate_nav()
last_ground_speed = g_gps->ground_speed;
// dampen our response
nav_lat -= constrain(dampening, -1800, 1800); // +- 20m max error
nav_lat -= constrain(dampening, -1800, 1800); // +- max error
// limit our output
nav_lat = constrain(nav_lat, -3800, 3800); // +- 20m max error
nav_lat = constrain(nav_lat, -2500, 2500); // +- max error
}
#endif