Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Circle Mode now working 

250Hz loop implemented
This commit is contained in:
Jason Short 2011-09-11 11:22:01 -07:00
parent de04b8c260
commit db331efbae
4 changed files with 12 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
ArduCopter/ArduCopter.pde
View File

@ -527,7 +527,7 @@ void loop()
{ {
// We want this to execute fast // We want this to execute fast
// ---------------------------- // ----------------------------
if (millis() - fast_loopTimer >= 5) { if (millis() - fast_loopTimer >= 4) {
//PORTK |= B00010000; //PORTK |= B00010000;
delta_ms_fast_loop = millis() - fast_loopTimer; delta_ms_fast_loop = millis() - fast_loopTimer;
fast_loopTimer = millis(); fast_loopTimer = millis();
@ -543,6 +543,7 @@ void loop()
fast_loop(); fast_loop();
fast_loopTimeStamp = millis(); fast_loopTimeStamp = millis();
} }
//PORTK &= B11101111;
if (millis() - fiftyhz_loopTimer > 19) { if (millis() - fiftyhz_loopTimer > 19) {
delta_ms_fiftyhz = millis() - fiftyhz_loopTimer; delta_ms_fiftyhz = millis() - fiftyhz_loopTimer;
@ -577,7 +578,6 @@ void loop()
} }
//PORTK &= B10111111; //PORTK &= B10111111;
} }
//PORTK &= B11101111;
} }
// PORTK |= B01000000; // PORTK |= B01000000;
// PORTK &= B10111111; // PORTK &= B10111111;
@ -1268,16 +1268,13 @@ static void update_navigation()
// calculates desired Yaw // calculates desired Yaw
update_auto_yaw(); update_auto_yaw();
{ {
circle_angle += dTnav; //1000 * (dTnav/1000); //circle_angle += dTnav; //1000 * (dTnav/1000);
circle_angle = wrap_360(target_bearing + 2000 + 18000);
if (circle_angle >= 36000)
circle_angle -= 36000;
target_WP.lng = next_WP.lng + g.loiter_radius * cos(radians(90 - circle_angle)); target_WP.lng = next_WP.lng + g.loiter_radius * cos(radians(90 - circle_angle));
target_WP.lat = next_WP.lat + g.loiter_radius * sin(radians(90 - circle_angle)); target_WP.lat = next_WP.lat + g.loiter_radius * sin(radians(90 - circle_angle));
} }
// calc the lat and long error to the target // calc the lat and long error to the target
calc_location_error(&target_WP); calc_location_error(&target_WP);
@ -1301,7 +1298,7 @@ static void read_AHRS(void)
hil.update(); hil.update();
#endif #endif
dcm.update_DCM(G_Dt);//, _tog); dcm.update_DCM_fast(G_Dt);//, _tog);
omega = dcm.get_gyro(); omega = dcm.get_gyro();
} }

7
ArduCopter/motors.pde
View File

@ -35,7 +35,8 @@ static void arm_motors()
// ------------------- // -------------------
#if HIL_MODE != HIL_MODE_ATTITUDE #if HIL_MODE != HIL_MODE_ATTITUDE
dcm.kp_roll_pitch(0.030000); dcm.kp_roll_pitch(0.030000);
dcm.ki_roll_pitch(0.000006); dcm.ki_roll_pitch(0.00001278), // 50 hz I term
//dcm.ki_roll_pitch(0.000006);
#endif #endif
// tune down compass // tune down compass
@ -95,8 +96,8 @@ static void arm_motors()
// Tune down DCM // Tune down DCM
// ------------------- // -------------------
#if HIL_MODE != HIL_MODE_ATTITUDE #if HIL_MODE != HIL_MODE_ATTITUDE
dcm.kp_roll_pitch(0.12); // higher for quads dcm.kp_roll_pitch(0.12); // higher for fast recovery
dcm.ki_roll_pitch(0.00000319); // 1/4 of the normal rate for 200 hz loop //dcm.ki_roll_pitch(0.00000319); // 1/4 of the normal rate for 200 hz loop
#endif #endif
// tune up compass // tune up compass

2
ArduCopter/system.pde
View File

@ -41,7 +41,7 @@ const struct Menu::command main_menu_commands[] PROGMEM = {
}; };
// Create the top-level menu object. // Create the top-level menu object.
MENU(main_menu, "ArduCopter 2.0.41 Beta", main_menu_commands); MENU(main_menu, "ArduCopter 2.0.42 Beta", main_menu_commands);
#endif // CLI_ENABLED #endif // CLI_ENABLED

2
ArduCopter/test.pde
View File

@ -447,7 +447,7 @@ test_imu(uint8_t argc, const Menu::arg *argv)
accels.x, accels.y, accels.z, accels.x, accels.y, accels.z,
gyros.x, gyros.y, gyros.z); gyros.x, gyros.y, gyros.z);
*/ */
///* /*
Serial.printf_P(PSTR("cp: %1.2f, sp: %1.2f, cr: %1.2f, sr: %1.2f, cy: %1.2f, sy: %1.2f,\n"), Serial.printf_P(PSTR("cp: %1.2f, sp: %1.2f, cr: %1.2f, sr: %1.2f, cy: %1.2f, sy: %1.2f,\n"),
cos_pitch_x, cos_pitch_x,
sin_pitch_y, sin_pitch_y,