fixed Current logging output.

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1572 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-01-29 05:55:23 +00:00
parent 70f11ef88d
commit 4cf16f67c4
2 changed files with 14 additions and 6 deletions

View File

@ -245,7 +245,7 @@ void Log_Write_Startup(byte type)
struct Location cmd = get_wp_with_index(0); struct Location cmd = get_wp_with_index(0);
Log_Write_Cmd(0, &cmd); Log_Write_Cmd(0, &cmd);
for (int i=1; i<wp_total; i++){ for (int i = 1; i < wp_total; i++){
cmd = get_wp_with_index(i); cmd = get_wp_with_index(i);
Log_Write_Cmd(i, &cmd); Log_Write_Cmd(i, &cmd);
} }
@ -537,13 +537,15 @@ void Log_Read(int start_page, int end_page)
case 0: case 0:
if(data==HEAD_BYTE1) // Head byte 1 if(data==HEAD_BYTE1) // Head byte 1
log_step++; log_step++;
break; break;
case 1: case 1:
if(data==HEAD_BYTE2) // Head byte 2 if(data==HEAD_BYTE2) // Head byte 2
log_step++; log_step++;
else else
log_step = 0; log_step = 0;
break; break;
case 2: case 2:
if(data==LOG_ATTITUDE_MSG){ if(data==LOG_ATTITUDE_MSG){
Log_Read_Attitude(); Log_Read_Attitude();
@ -573,20 +575,26 @@ void Log_Read(int start_page, int end_page)
Log_Read_Cmd(); Log_Read_Cmd();
log_step++; log_step++;
}else if(data==LOG_CURRENT_MSG){
Log_Read_Current();
log_step++;
}else if(data==LOG_STARTUP_MSG){ }else if(data==LOG_STARTUP_MSG){
Log_Read_Startup(); Log_Read_Startup();
log_step++; log_step++;
}else { }else {
if(data==LOG_GPS_MSG){ if(data == LOG_GPS_MSG){
Log_Read_GPS(); Log_Read_GPS();
log_step++; log_step++;
} else { } else {
Serial.print("Error Reading Packet: "); Serial.print("Error Reading Packet: ");
Serial.print(packet_count); Serial.print(packet_count);
log_step=0; // Restart, we have a problem... log_step = 0; // Restart, we have a problem...
} }
} }
break; break;
case 3: case 3:
if(data==END_BYTE){ if(data==END_BYTE){
packet_count++; packet_count++;

View File

@ -264,7 +264,7 @@ test_stabilize(uint8_t argc, const Menu::arg *argv)
ts_num++; ts_num++;
if (ts_num > 10){ if (ts_num > 10){
ts_num = 0; ts_num = 0;
Serial.printf_P(PSTR("r: %d, p:%d, rc1:%d, Int%4.4f, "), Serial.printf_P(PSTR("r: %d, p:%d, rc1:%d, Int:%4.4f, "),
(int)(dcm.roll_sensor/100), (int)(dcm.roll_sensor/100),
(int)(dcm.pitch_sensor/100), (int)(dcm.pitch_sensor/100),
rc_1.pwm_out, rc_1.pwm_out,