Minor Yaw update, Log adjustment,

Main loop slightly faster at 125 hz


git-svn-id: https://arducopter.googlecode.com/svn/trunk@1912 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-04-20 06:44:49 +00:00
parent 1b947f5d9e
commit 4cbbeb4860
4 changed files with 10 additions and 6 deletions

View File

@ -454,7 +454,7 @@ void loop()
{
// We want this to execute at 100Hz
// --------------------------------
if (millis() - fast_loopTimer > 9) {
if (millis() - fast_loopTimer > 7) {
delta_ms_fast_loop = millis() - fast_loopTimer;
fast_loopTimer = millis();
load = float(fast_loopTimeStamp - fast_loopTimer) / delta_ms_fast_loop;

View File

@ -116,7 +116,7 @@ output_yaw_with_hold(boolean hold)
// look to see if we have exited rate control properly - ie stopped turning
if(rate_yaw_flag){
// we are still in motion from rate control
if(fabs(omega.z) < .5){
if(fabs(omega.z) < .2){
clear_yaw_control();
hold = true; // just to be explicit
//Serial.print("C");
@ -152,12 +152,12 @@ output_yaw_with_hold(boolean hold)
yaw_debug = YAW_HOLD; //0
}else{
// -error = CCW, +error = CW
if(g.rc_4.control_in == 0){
// we are breaking;
//g.rc_4.servo_out = (omega.z > 0) ? -600 : 600;
// adaptive braking
g.rc_4.servo_out = (int)((-1800.0 * omega.z) / 1.5);
g.rc_4.servo_out = (int)((-1800.0 * omega.z) / 1);
// -1800 * 0.925 / 6 = -277
yaw_debug = YAW_BRAKE; // 1

View File

@ -381,6 +381,8 @@ void Log_Write_Control_Tuning()
DataFlash.WriteInt((int)(g.rc_3.servo_out));
DataFlash.WriteInt((int)(g.rc_4.control_in));
DataFlash.WriteInt((int)(g.rc_4.servo_out));
DataFlash.WriteInt((int)yaw_error);
// Yaw mode
DataFlash.WriteByte(yaw_debug);
@ -492,13 +494,15 @@ void Log_Read_Current()
// Read an control tuning packet
void Log_Read_Control_Tuning()
{
Serial.printf_P(PSTR("CTUN: %d, %d, %d, %d, %d, %1.4f, %1.4f, %1.4f, %d, %d, %ld\n"),
Serial.printf_P(PSTR("CTUN, %d, %d, %d, %d, %d, %d, %1.4f, %1.4f, %1.4f, %d, %d, %ld\n"),
// Control
DataFlash.ReadInt(),
DataFlash.ReadInt(),
DataFlash.ReadInt(),
DataFlash.ReadInt(),
DataFlash.ReadInt(),
// Yaw Mode
(int)DataFlash.ReadByte(),

View File

@ -22,7 +22,7 @@ Command Structure in bytes
Commands below MAV_CMD_NAV_LAST are commands that have a end criteria, eg "reached waypoint" or "reached altitude"
***********************************
Command ID Name Parameter 1 Altitude Latitude Longitude
0x10 / 16 MAV_CMD_NAV_WAYPOINT delay (seconds) altitude lat lon
0x10 / 16 MAV_CMD_NAV_WAYPOINT delay (seconds) altitude lat lon
0x11 / 17 MAV_CMD_NAV_LOITER_UNLIM (forever) altitude lat lon
0x14 / 20 MAV_CMD_NAV_RETURN_TO_LAUNCH - - - -