rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-21 11:54:15 +01:00
Code Issues Releases Activity

Small fixes

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@726 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
FredericG 2010-06-07 19:56:42 +00:00 committed by FredericG
parent 426b4344b0
commit 2895925bd2
1 changed files with 17 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
flight/OpenPilot/Modules/MK/MKSerial/MKSerial.c
View File

@ -37,7 +37,7 @@
#define DEBUG_PORT COM_USART1 #define DEBUG_PORT COM_USART1
#define STACK_SIZE 1024 #define STACK_SIZE 1024
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) #define TASK_PRIORITY (tskIDLE_PRIORITY + 3)
#define MAX_NB_PARS 40 #define MAX_NB_PARS 100
#define ENABLE_DEBUG_MSG #define ENABLE_DEBUG_MSG
// //
@ -120,7 +120,7 @@ static void MkSerialTask(void* parameters);
static void OnError(int line) static void OnError(int line)
{ {
DEBUG_MSG("MKProcol error %d\n", line); DEBUG_MSG("MKProcol error %d\n\r", line);
} }
@ -152,7 +152,7 @@ static void PrintMsg(const MkMsg_t* msg)
{ {
DEBUG_MSG("%02x ", msg->pars[i]); DEBUG_MSG("%02x ", msg->pars[i]);
} }
DEBUG_MSG("\n"); DEBUG_MSG("\n\r");
} }
#endif #endif
@ -442,7 +442,7 @@ static void DoConnectedToFC(void)
AttitudeActualData attitudeData; AttitudeActualData attitudeData;
MkMsg_t msg; MkMsg_t msg;
DEBUG_MSG("FC\n"); DEBUG_MSG("FC\n\r");
memset(&attitudeData, 0, sizeof(attitudeData)); memset(&attitudeData, 0, sizeof(attitudeData));
@ -460,7 +460,7 @@ static void DoConnectedToFC(void)
nick = Par2Int16(&msg, DEBUG_MSG_NICK_IDX); nick = Par2Int16(&msg, DEBUG_MSG_NICK_IDX);
roll = Par2Int16(&msg, DEBUG_MSG_ROLL_IDX); roll = Par2Int16(&msg, DEBUG_MSG_ROLL_IDX);
DEBUG_MSG("Att: Nick=%5d Roll=%5d\n", nick, roll); DEBUG_MSG("Att: Nick=%5d Roll=%5d\n\r", nick, roll);
attitudeData.seq++; attitudeData.seq++;
attitudeData.Pitch = -(float)nick/10; attitudeData.Pitch = -(float)nick/10;
@ -469,7 +469,7 @@ static void DoConnectedToFC(void)
} }
else else
{ {
DEBUG_MSG("TO\n"); DEBUG_MSG("TO\n\r");
break; break;
} }
} }
@ -482,7 +482,7 @@ static void DoConnectedToNC(void)
AttitudeActualData attitudeData; AttitudeActualData attitudeData;
GpsObjectData gpsData; GpsObjectData gpsData;
DEBUG_MSG("NC\n"); DEBUG_MSG("NC\n\r");
memset(&attitudeData, 0, sizeof(attitudeData)); memset(&attitudeData, 0, sizeof(attitudeData));
memset(&gpsData, 0, sizeof(gpsData)); memset(&gpsData, 0, sizeof(gpsData));
@ -494,11 +494,12 @@ static void DoConnectedToNC(void)
{ {
if (WaitForMsg(MSGCMD_OSD, &msg, 500 / portTICK_RATE_MS)) if (WaitForMsg(MSGCMD_OSD, &msg, 500 / portTICK_RATE_MS))
{ {
//PrintMsg(&msg);
GetGpsPos(&msg, OSD_MSG_CURRPOS_IDX, &pos); GetGpsPos(&msg, OSD_MSG_CURRPOS_IDX, &pos);
DEBUG_MSG("Bat=%f V\n", (float)msg.pars[OSD_MSG_BATT_IDX]/10); DEBUG_MSG("Bat=%d\n\r", msg.pars[OSD_MSG_BATT_IDX]);
DEBUG_MSG("Nick=%d Roll=%d\n", Par2Int8(&msg,OSD_MSG_NICK_IDX), Par2Int8(&msg,OSD_MSG_ROLL_IDX)); DEBUG_MSG("Nick=%d Roll=%d\n\r", Par2Int8(&msg,OSD_MSG_NICK_IDX), Par2Int8(&msg,OSD_MSG_ROLL_IDX));
DEBUG_MSG("POS #Sats=%d stat=%d lat=%f lon=%f alt=%f\n", msg.pars[OSD_MSG_NB_SATS_IDX], pos.status, pos.latitude, pos.longitute, pos.altitude); DEBUG_MSG("POS #Sats=%d stat=%d lat=%d lon=%d alt=%d\n\r", msg.pars[OSD_MSG_NB_SATS_IDX], pos.status, (int)pos.latitude, (int)pos.longitute, (int)pos.altitude);
attitudeData.seq++; attitudeData.seq++;
attitudeData.Pitch = -Par2Int8(&msg,OSD_MSG_NICK_IDX); attitudeData.Pitch = -Par2Int8(&msg,OSD_MSG_NICK_IDX);
@ -514,7 +515,7 @@ static void DoConnectedToNC(void)
} }
else else
{ {
DEBUG_MSG("TO\n"); DEBUG_MSG("TO\n\r");
break; break;
} }
} }
@ -530,7 +531,7 @@ static void MkSerialTask(void* parameters)
PIOS_COM_ChangeBaud(PORT, 57600); PIOS_COM_ChangeBaud(PORT, 57600);
PIOS_COM_ChangeBaud(DEBUG_PORT, 57600); PIOS_COM_ChangeBaud(DEBUG_PORT, 57600);
DEBUG_MSG("MKSerial Started\n"); DEBUG_MSG("MKSerial Started\n\r");
while (1) while (1)
{ {
@ -542,12 +543,12 @@ static void MkSerialTask(void* parameters)
if (WaitForMsg(MSGCMD_VERSION, &msg, 100 / portTICK_RATE_MS)) if (WaitForMsg(MSGCMD_VERSION, &msg, 100 / portTICK_RATE_MS))
{ {
version = VersionMsg_GetVersion(&msg); version = VersionMsg_GetVersion(&msg);
DEBUG_MSG("%d\n", version); DEBUG_MSG("%d\n\r", version);
connectionOk = TRUE; connectionOk = TRUE;
} }
else else
{ {
DEBUG_MSG("TO\n"); DEBUG_MSG("TO\n\r");
} }
} }
@ -560,6 +561,8 @@ static void MkSerialTask(void* parameters)
{ {
DoConnectedToFC(); // Will only return after an error DoConnectedToFC(); // Will only return after an error
} }
connectionOk = FALSE;
} }
} }