mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-12 02:48:28 -04:00
fixed Current logging output.
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1572 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
parent
1af6a3b4e8
commit
e1b4ec3132
@ -538,12 +538,14 @@ void Log_Read(int start_page, int end_page)
|
|||||||
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,9 +575,14 @@ 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();
|
||||||
@ -587,6 +594,7 @@ void Log_Read(int start_page, int end_page)
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case 3:
|
case 3:
|
||||||
if(data==END_BYTE){
|
if(data==END_BYTE){
|
||||||
packet_count++;
|
packet_count++;
|
||||||
|
@ -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,
|
||||||
|
Loading…
Reference in New Issue
Block a user