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

MKSerial: Complete PositionActual Obj

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@818 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
FredericG 2010-06-19 11:04:49 +00:00 committed by FredericG
parent ceadcf49a0
commit 524201d3b6
1 changed files with 33 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -36,11 +36,11 @@
// Configuration // Configuration
// //
#define PORT PIOS_COM_GPS #define PORT PIOS_COM_GPS
#define DEBUG_PORT PIOS_COM_AUX #define DEBUG_PORT PIOS_COM_TELEM_RF
#define STACK_SIZE 1024 #define STACK_SIZE 1024
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) #define TASK_PRIORITY (tskIDLE_PRIORITY + 3)
#define MAX_NB_PARS 100 #define MAX_NB_PARS 100
#define ENABLE_DEBUG_MSG //#define ENABLE_DEBUG_MSG
// //
// Private constants // Private constants
@ -60,6 +60,7 @@
#define OSD_MSG_CURRPOS_IDX 1 #define OSD_MSG_CURRPOS_IDX 1
#define OSD_MSG_NB_SATS_IDX 50 #define OSD_MSG_NB_SATS_IDX 50
#define OSD_MSG_BATT_IDX 57 #define OSD_MSG_BATT_IDX 57
#define OSD_MSG_COMPHEADING_IDX 62
#define OSD_MSG_NICK_IDX 64 #define OSD_MSG_NICK_IDX 64
#define OSD_MSG_ROLL_IDX 65 #define OSD_MSG_ROLL_IDX 65
@ -261,7 +262,6 @@ static bool WaitForMsg(uint8_t cmd, MkMsg_t* msg, portTickType xTicksToWait)
return FALSE; return FALSE;
} }
WaitForBytes(buf, 1, 100 / portTICK_RATE_MS); WaitForBytes(buf, 1, 100 / portTICK_RATE_MS);
// TODO: check to TO
} while (buf[0] != '#'); } while (buf[0] != '#');
// Wait for cmd and address // Wait for cmd and address
@ -484,6 +484,7 @@ static void DoConnectedToNC(void)
AttitudeActualData attitudeData; AttitudeActualData attitudeData;
PositionActualData positionData; PositionActualData positionData;
FlightBatteryStateData flightBatteryData; FlightBatteryStateData flightBatteryData;
uint8_t battStateCnt=0;
DEBUG_MSG("NC\n\r"); DEBUG_MSG("NC\n\r");
@ -501,10 +502,13 @@ static void DoConnectedToNC(void)
//PrintMsg(&msg); //PrintMsg(&msg);
GetGpsPos(&msg, OSD_MSG_CURRPOS_IDX, &pos); GetGpsPos(&msg, OSD_MSG_CURRPOS_IDX, &pos);
#if 0
DEBUG_MSG("Bat=%d\n\r", msg.pars[OSD_MSG_BATT_IDX]); DEBUG_MSG("Bat=%d\n\r", msg.pars[OSD_MSG_BATT_IDX]);
DEBUG_MSG("Nick=%d Roll=%d\n\r", 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=%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); 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);
#else
DEBUG_MSG(".");
#endif
attitudeData.seq++; attitudeData.seq++;
attitudeData.Pitch = -Par2Int8(&msg, OSD_MSG_NICK_IDX); attitudeData.Pitch = -Par2Int8(&msg, OSD_MSG_NICK_IDX);
attitudeData.Roll = -Par2Int8(&msg, OSD_MSG_ROLL_IDX); attitudeData.Roll = -Par2Int8(&msg, OSD_MSG_ROLL_IDX);
@ -514,11 +518,23 @@ static void DoConnectedToNC(void)
positionData.Latitude = pos.latitude; positionData.Latitude = pos.latitude;
positionData.Altitude = pos.altitude; positionData.Altitude = pos.altitude;
positionData.Satellites = msg.pars[OSD_MSG_NB_SATS_IDX]; positionData.Satellites = msg.pars[OSD_MSG_NB_SATS_IDX];
positionData.Status = POSITIONACTUAL_STATUS_FIX3D; // FIXME positionData.Heading = Par2Int16(&msg, OSD_MSG_COMPHEADING_IDX);
if (positionData.Satellites<4)
{
positionData.Status = POSITIONACTUAL_STATUS_NOFIX;
}
else
{
positionData.Status = POSITIONACTUAL_STATUS_FIX3D;
}
PositionActualSet(&positionData); PositionActualSet(&positionData);
flightBatteryData.Tension = (uint32_t)msg.pars[OSD_MSG_BATT_IDX]*100; if (++battStateCnt > 2)
{
flightBatteryData.Voltage = (uint32_t)msg.pars[OSD_MSG_BATT_IDX]*100;
FlightBatteryStateSet(&flightBatteryData); FlightBatteryStateSet(&flightBatteryData);
battStateCnt = 0;
}
} }
else else
{ {
@ -547,7 +563,7 @@ static void MkSerialTask(void* parameters)
{ {
SendMsgParNone(MK_ADDR_ALL, MSGCMD_GET_VERSION); SendMsgParNone(MK_ADDR_ALL, MSGCMD_GET_VERSION);
DEBUG_MSG("Version... "); DEBUG_MSG("Version... ");
if (WaitForMsg(MSGCMD_VERSION, &msg, 100 / portTICK_RATE_MS)) if (WaitForMsg(MSGCMD_VERSION, &msg, 250 / portTICK_RATE_MS))
{ {
version = VersionMsg_GetVersion(&msg); version = VersionMsg_GetVersion(&msg);
DEBUG_MSG("%d\n\r", version); DEBUG_MSG("%d\n\r", version);
@ -570,6 +586,8 @@ static void MkSerialTask(void* parameters)
} }
connectionOk = FALSE; connectionOk = FALSE;
vTaskDelay(250 / portTICK_RATE_MS);
} }
} }