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 8d1f9c9fd9
commit 51d918dda7
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; // 0-1000 for throttle control
int nav_throttle_old; // for filtering 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_throttle; // used to control when we calculate nav_throttle
bool invalid_nav; // 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 bool set_throttle_cruise_flag = false; // used to track the throttle crouse value
@ -521,6 +522,10 @@ void fast_loop()
// ------------------------------ // ------------------------------
set_servos_4(); set_servos_4();
// record throttle output
// ------------------------------
throttle_integrator += g.rc_3.servo_out;
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK #if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK
// HIL for a copter needs very fast update of the servo values // HIL for a copter needs very fast update of the servo values
gcs.send_message(MSG_RADIO_OUT); 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_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_CURRENT_MSG); DataFlash.WriteByte(LOG_CURRENT_MSG);
DataFlash.WriteInt(g.rc_3.control_in); DataFlash.WriteInt(g.rc_3.control_in);
DataFlash.WriteLong(throttle_integrator);
DataFlash.WriteInt((int)(current_voltage * 100.0)); DataFlash.WriteInt((int)(current_voltage * 100.0));
DataFlash.WriteInt((int)(current_amps * 100.0)); DataFlash.WriteInt((int)(current_amps * 100.0));
DataFlash.WriteInt((int)current_total); DataFlash.WriteInt((int)current_total);
DataFlash.WriteByte(END_BYTE); DataFlash.WriteByte(END_BYTE);
} }
// Read a Current packet // Read a Current packet
void Log_Read_Current() 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.ReadInt(),
DataFlash.ReadLong(),
((float)DataFlash.ReadInt() / 100.f), ((float)DataFlash.ReadInt() / 100.f),
((float)DataFlash.ReadInt() / 100.f), ((float)DataFlash.ReadInt() / 100.f),
DataFlash.ReadInt()); DataFlash.ReadInt());

9
ArduCopterMega/navigation.pde
View File

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