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

LP-212 readable #ifs and dji compile for cc3d

Browse Source
This commit is contained in:
Cliff Geerdes 2016-02-11 02:20:25 -05:00
parent 2826cb5cd7
commit fac0213987
2 changed files with 53 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
flight/modules/GPS/DJI.c
View File

@ -41,10 +41,12 @@
#include <string.h> #include <string.h>
#include <auxmagsupport.h> #include <auxmagsupport.h>
// parsing functions, roughly ordered by reception rate (higher rate messages on top) // parsing functions
static void parse_dji_mag(struct DJIPacket *dji, GPSPositionSensorData *gpsPosition);
static void parse_dji_gps(struct DJIPacket *dji, GPSPositionSensorData *gpsPosition); static void parse_dji_gps(struct DJIPacket *dji, GPSPositionSensorData *gpsPosition);
#if !defined(PIOS_GPS_MINIMAL)
static void parse_dji_mag(struct DJIPacket *dji, GPSPositionSensorData *gpsPosition);
static void parse_dji_ver(struct DJIPacket *dji, GPSPositionSensorData *gpsPosition); static void parse_dji_ver(struct DJIPacket *dji, GPSPositionSensorData *gpsPosition);
#endif /* !defined(PIOS_GPS_MINIMAL) */
static bool checksum_dji_message(struct DJIPacket *dji); static bool checksum_dji_message(struct DJIPacket *dji);
static uint32_t parse_dji_message(struct DJIPacket *dji, GPSPositionSensorData *gpsPosition); static uint32_t parse_dji_message(struct DJIPacket *dji, GPSPositionSensorData *gpsPosition);
@ -57,8 +59,10 @@ typedef struct {
const djiMessageHandler djiHandlerTable[] = { const djiMessageHandler djiHandlerTable[] = {
{ .msgId = DJI_ID_GPS, .handler = &parse_dji_gps }, { .msgId = DJI_ID_GPS, .handler = &parse_dji_gps },
#if !defined(PIOS_GPS_MINIMAL)
{ .msgId = DJI_ID_MAG, .handler = &parse_dji_mag }, { .msgId = DJI_ID_MAG, .handler = &parse_dji_mag },
{ .msgId = DJI_ID_VER, .handler = &parse_dji_ver }, { .msgId = DJI_ID_VER, .handler = &parse_dji_ver },
#endif /* !defined(PIOS_GPS_MINIMAL) */
}; };
#define DJI_HANDLER_TABLE_SIZE NELEMENTS(djiHandlerTable) #define DJI_HANDLER_TABLE_SIZE NELEMENTS(djiHandlerTable)
@ -283,6 +287,7 @@ static void parse_dji_gps(struct DJIPacket *dji, GPSPositionSensorData *gpsPosit
// gpsPosition->BaudRate = GPSPOSITIONSENSOR_BAUDRATE_115200; // gpsPosition->BaudRate = GPSPOSITIONSENSOR_BAUDRATE_115200;
GPSPositionSensorSet(gpsPosition); GPSPositionSensorSet(gpsPosition);
#if !defined(PIOS_GPS_MINIMAL)
// Time is valid, set GpsTime // Time is valid, set GpsTime
GPSTimeData gpsTime; GPSTimeData gpsTime;
// the lowest bit of day and the highest bit of hour overlap (xored? no stranger than that) // the lowest bit of day and the highest bit of hour overlap (xored? no stranger than that)
@ -299,9 +304,11 @@ static void parse_dji_gps(struct DJIPacket *dji, GPSPositionSensorData *gpsPosit
gpsTime.Minute = djiGps->min; gpsTime.Minute = djiGps->min;
gpsTime.Second = djiGps->sec; gpsTime.Second = djiGps->sec;
GPSTimeSet(&gpsTime); GPSTimeSet(&gpsTime);
#endif /* !defined(PIOS_GPS_MINIMAL) */
} }
#if !defined(PIOS_GPS_MINIMAL)
static void parse_dji_mag(struct DJIPacket *dji, __attribute__((unused)) GPSPositionSensorData *gpsPosition) static void parse_dji_mag(struct DJIPacket *dji, __attribute__((unused)) GPSPositionSensorData *gpsPosition)
{ {
if (!useMag) { if (!useMag) {
@ -345,6 +352,7 @@ static void parse_dji_ver(struct DJIPacket *dji, __attribute__((unused)) GPSPosi
GPSPositionSensorSensorTypeSet((uint8_t *)&sensorType); GPSPositionSensorSensorTypeSet((uint8_t *)&sensorType);
} }
} }
#endif /* !defined(PIOS_GPS_MINIMAL) */
// DJI message parser // DJI message parser

70
flight/modules/GPS/GPS.c
View File

@ -50,8 +50,9 @@
#include "NMEA.h" #include "NMEA.h"
#include "UBX.h" #include "UBX.h"
#include "DJI.h" #include "DJI.h"
#if (defined(PIOS_INCLUDE_GPS_UBX_PARSER) || defined(PIOS_INCLUDE_GPS_DJI_PARSER)) && !defined(PIOS_GPS_MINIMAL) #if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
#include "inc/ubx_autoconfig.h" #include "inc/ubx_autoconfig.h"
#define FULL_UBX_PARSER
#endif #endif
#include <auxmagsupport.h> #include <auxmagsupport.h>
@ -59,22 +60,33 @@
PERF_DEFINE_COUNTER(counterBytesIn); PERF_DEFINE_COUNTER(counterBytesIn);
PERF_DEFINE_COUNTER(counterRate); PERF_DEFINE_COUNTER(counterRate);
PERF_DEFINE_COUNTER(counterParse); PERF_DEFINE_COUNTER(counterParse);
#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER) || defined(PIOS_INCLUDE_GPS_DJI_PARSER)
#define ANY_GPS_PARSER
#endif
#if (defined(ANY_GPS_PARSER)) && !defined(PIOS_GPS_MINIMAL)
#define ANY_FULL_GPS_PARSER
#endif
#if (defined(PIOS_INCLUDE_HMC5X83) || defined(PIOS_INCLUDE_GPS_UBX_PARSER) || defined(PIOS_INCLUDE_GPS_DJI_PARSER)) && !defined(PIOS_GPS_MINIMAL)
#define ANY_FULL_MAG_PARSER
#endif
// **************** // ****************
// Private functions // Private functions
static void gpsTask(__attribute__((unused)) void *parameters); static void gpsTask(__attribute__((unused)) void *parameters);
static void updateHwSettings(__attribute__((unused)) UAVObjEvent *ev); static void updateHwSettings(__attribute__((unused)) UAVObjEvent *ev);
#ifdef PIOS_GPS_SETS_HOMELOCATION #if defined(PIOS_GPS_SETS_HOMELOCATION)
static void setHomeLocation(GPSPositionSensorData *gpsData); static void setHomeLocation(GPSPositionSensorData *gpsData);
static float GravityAccel(float latitude, float longitude, float altitude); static float GravityAccel(float latitude, float longitude, float altitude);
#endif #endif
#if (defined(PIOS_INCLUDE_GPS_UBX_PARSER) || defined(PIOS_INCLUDE_GPS_DJI_PARSER)) #if defined(ANY_FULL_MAG_PARSER)
void AuxMagSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev); void AuxMagSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
#ifndef PIOS_GPS_MINIMAL
void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev);
#endif #endif
#if defined(ANY_FULL_GPS_PARSER)
void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev);
#endif #endif
// **************** // ****************
@ -127,7 +139,7 @@ static char *gps_rx_buffer;
static uint32_t timeOfLastUpdateMs; static uint32_t timeOfLastUpdateMs;
#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER) || defined(PIOS_INCLUDE_GPS_DJI_PARSER) #if defined(ANY_GPS_PARSER)
static struct GPS_RX_STATS gpsRxStats; static struct GPS_RX_STATS gpsRxStats;
#endif #endif
@ -181,7 +193,7 @@ int32_t GPSInitialize(void)
GPSTimeInitialize(); GPSTimeInitialize();
GPSSatellitesInitialize(); GPSSatellitesInitialize();
HomeLocationInitialize(); HomeLocationInitialize();
#if (defined(PIOS_INCLUDE_GPS_UBX_PARSER) || defined(PIOS_INCLUDE_GPS_DJI_PARSER)) #if defined(ANY_FULL_MAG_PARSER)
AuxMagSensorInitialize(); AuxMagSensorInitialize();
AuxMagSettingsInitialize(); AuxMagSettingsInitialize();
GPSExtendedStatusInitialize(); GPSExtendedStatusInitialize();
@ -218,11 +230,13 @@ int32_t GPSInitialize(void)
#if defined(PIOS_GPS_MINIMAL) #if defined(PIOS_GPS_MINIMAL)
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) #if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
gps_rx_buffer = pios_malloc(sizeof(struct UBXPacket)); gps_rx_buffer = pios_malloc(sizeof(struct UBXPacket));
#elif defined(PIOS_INCLUDE_GPS_DJI_PARSER)
gps_rx_buffer = pios_malloc(sizeof(struct DJIPacket));
#else #else
gps_rx_buffer = NULL; gps_rx_buffer = NULL;
#endif #endif
#else /* defined(PIOS_GPS_MINIMAL) */ #else /* defined(PIOS_GPS_MINIMAL) */
#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER) || defined(PIOS_INCLUDE_GPS_DJI_PARSER) #if defined(ANY_GPS_PARSER)
#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) #if defined(PIOS_INCLUDE_GPS_NMEA_PARSER)
size_t bufSize = NMEA_MAX_PACKET_LENGTH; size_t bufSize = NMEA_MAX_PACKET_LENGTH;
#else #else
@ -239,14 +253,14 @@ int32_t GPSInitialize(void)
} }
#endif #endif
gps_rx_buffer = pios_malloc(bufSize); gps_rx_buffer = pios_malloc(bufSize);
#else /* defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER) || defined(PIOS_INCLUDE_GPS_DJI_PARSER) */ #else /* defined(ANY_GPS_PARSER) */
gps_rx_buffer = NULL; gps_rx_buffer = NULL;
#endif /* defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER) || defined(PIOS_INCLUDE_GPS_DJI_PARSER) */ #endif /* defined(ANY_GPS_PARSER) */
#endif /* defined(PIOS_GPS_MINIMAL) */ #endif /* defined(PIOS_GPS_MINIMAL) */
#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER) || defined(PIOS_INCLUDE_GPS_DJI_PARSER) #if defined(ANY_GPS_PARSER)
PIOS_Assert(gps_rx_buffer); PIOS_Assert(gps_rx_buffer);
#endif #endif
#if (defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER) || defined(PIOS_INCLUDE_GPS_DJI_PARSER)) && !defined(PIOS_GPS_MINIMAL) #if defined(ANY_FULL_GPS_PARSER)
HwSettingsConnectCallback(updateHwSettings); // allow changing baud rate even after startup HwSettingsConnectCallback(updateHwSettings); // allow changing baud rate even after startup
GPSSettingsConnectCallback(updateGpsSettings); GPSSettingsConnectCallback(updateGpsSettings);
#endif #endif
@ -281,7 +295,7 @@ static void gpsTask(__attribute__((unused)) void *parameters)
timeOfLastUpdateMs = timeNowMs; timeOfLastUpdateMs = timeNowMs;
GPSPositionSensorGet(&gpspositionsensor); GPSPositionSensorGet(&gpspositionsensor);
#if (defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER) || defined(PIOS_INCLUDE_GPS_DJI_PARSER)) && !defined(PIOS_GPS_MINIMAL) #if defined(ANY_FULL_GPS_PARSER)
// this should be done in the task because it calls out to actually start the ubx GPS serial reads // this should be done in the task because it calls out to actually start the ubx GPS serial reads
updateGpsSettings(0); updateGpsSettings(0);
#endif #endif
@ -296,7 +310,7 @@ static void gpsTask(__attribute__((unused)) void *parameters)
// Loop forever // Loop forever
while (1) { while (1) {
if (gpsPort) { if (gpsPort) {
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) #if defined(FULL_UBX_PARSER)
// do autoconfig stuff for UBX GPS's // do autoconfig stuff for UBX GPS's
if (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX) { if (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX) {
char *buffer = 0; char *buffer = 0;
@ -330,7 +344,7 @@ static void gpsTask(__attribute__((unused)) void *parameters)
GPSPositionSensorAutoConfigStatusSet(&gpspositionsensor.AutoConfigStatus); GPSPositionSensorAutoConfigStatusSet(&gpspositionsensor.AutoConfigStatus);
lastStatus = gpspositionsensor.AutoConfigStatus; lastStatus = gpspositionsensor.AutoConfigStatus;
} }
#endif /* if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) */ #endif /* if defined(FULL_UBX_PARSER) */
uint16_t cnt; uint16_t cnt;
int res; int res;
@ -342,7 +356,7 @@ static void gpsTask(__attribute__((unused)) void *parameters)
PERF_TRACK_VALUE(counterBytesIn, cnt); PERF_TRACK_VALUE(counterBytesIn, cnt);
PERF_MEASURE_PERIOD(counterRate); PERF_MEASURE_PERIOD(counterRate);
switch (gpsSettings.DataProtocol) { switch (gpsSettings.DataProtocol) {
#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) && !defined(PIOS_GPS_MINIMAL) #if defined(PIOS_INCLUDE_GPS_NMEA_PARSER)
case GPSSETTINGS_DATAPROTOCOL_NMEA: case GPSSETTINGS_DATAPROTOCOL_NMEA:
res = parse_nmea_stream(c, cnt, gps_rx_buffer, &gpspositionsensor, &gpsRxStats); res = parse_nmea_stream(c, cnt, gps_rx_buffer, &gpspositionsensor, &gpsRxStats);
break; break;
@ -352,7 +366,7 @@ static void gpsTask(__attribute__((unused)) void *parameters)
res = parse_ubx_stream(c, cnt, gps_rx_buffer, &gpspositionsensor, &gpsRxStats); res = parse_ubx_stream(c, cnt, gps_rx_buffer, &gpspositionsensor, &gpsRxStats);
break; break;
#endif #endif
#if defined(PIOS_INCLUDE_GPS_DJI_PARSER) && !defined(PIOS_GPS_MINIMAL) #if defined(PIOS_INCLUDE_GPS_DJI_PARSER)
case GPSSETTINGS_DATAPROTOCOL_DJI: case GPSSETTINGS_DATAPROTOCOL_DJI:
res = parse_dji_stream(c, cnt, gps_rx_buffer, &gpspositionsensor, &gpsRxStats); res = parse_dji_stream(c, cnt, gps_rx_buffer, &gpspositionsensor, &gpsRxStats);
break; break;
@ -567,13 +581,13 @@ static void gps_set_fc_baud_from_settings()
// must updateHwSettings() before updateGpsSettings() so baud rate is set before GPS serial code starts running // must updateHwSettings() before updateGpsSettings() so baud rate is set before GPS serial code starts running
static void updateHwSettings(UAVObjEvent __attribute__((unused)) *ev) static void updateHwSettings(UAVObjEvent __attribute__((unused)) *ev)
{ {
#if (defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER) || defined(PIOS_INCLUDE_GPS_DJI_PARSER)) && !defined(PIOS_GPS_MINIMAL) #if defined(ANY_FULL_GPS_PARSER)
static uint32_t previousGpsPort = 0xf0f0f0f0; // = doesn't match gpsport static uint32_t previousGpsPort = 0xf0f0f0f0; // = doesn't match gpsport
#endif #endif
// if GPS (UBX or NMEA) is enabled at all // if GPS (UBX or NMEA) is enabled at all
if (gpsPort && gpsEnabled) { if (gpsPort && gpsEnabled) {
// if we have ubx auto config then sometimes we don't set the baud rate // if we have ubx auto config then sometimes we don't set the baud rate
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) #if defined(FULL_UBX_PARSER)
// just for UBX, because it has autoconfig // just for UBX, because it has autoconfig
// if in startup, or not configured to do ubx and ubx auto config // if in startup, or not configured to do ubx and ubx auto config
// //
@ -584,18 +598,18 @@ static void updateHwSettings(UAVObjEvent __attribute__((unused)) *ev)
|| previousGpsPort != gpsPort || previousGpsPort != gpsPort
|| gpsSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_DISABLED || gpsSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_DISABLED
|| gpsSettings.DataProtocol != GPSSETTINGS_DATAPROTOCOL_UBX) || gpsSettings.DataProtocol != GPSSETTINGS_DATAPROTOCOL_UBX)
#endif #endif /* defined(FULL_UBX_PARSER) */
{ {
// always set the baud rate // always set the baud rate
gps_set_fc_baud_from_settings(); gps_set_fc_baud_from_settings();
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) #if defined(FULL_UBX_PARSER)
// just for UBX, because it has subtypes UBX(6), UBX7 and UBX8 // just for UBX, because it has subtypes UBX(6), UBX7 and UBX8
// changing anything in HwSettings will make it re-verify the sensor type (including auto-baud if not completely disabled) // changing anything in HwSettings will make it re-verify the sensor type (including auto-baud if not completely disabled)
// for auto baud disabled, the user can just try some baud rates and when the baud rate is correct, the sensor type becomes valid // for auto baud disabled, the user can just try some baud rates and when the baud rate is correct, the sensor type becomes valid
gps_ubx_reset_sensor_type(); gps_ubx_reset_sensor_type();
#endif #endif /* defined(FULL_UBX_PARSER) */
} }
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) #if defined(FULL_UBX_PARSER)
else { else {
// else: // else:
// - we are past startup // - we are past startup
@ -607,15 +621,15 @@ static void updateHwSettings(UAVObjEvent __attribute__((unused)) *ev)
// this is here so that runtime (not startup) user changes to HwSettings will restart auto-config // this is here so that runtime (not startup) user changes to HwSettings will restart auto-config
gps_ubx_autoconfig_set(NULL); gps_ubx_autoconfig_set(NULL);
} }
#endif #endif /* defined(FULL_UBX_PARSER) */
} }
#if (defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER) || defined(PIOS_INCLUDE_GPS_DJI_PARSER)) && !defined(PIOS_GPS_MINIMAL) #if defined(ANY_FULL_GPS_PARSER)
previousGpsPort = gpsPort; previousGpsPort = gpsPort;
#endif #endif
} }
#if (defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER) || defined(PIOS_INCLUDE_GPS_DJI_PARSER)) && !defined(PIOS_GPS_MINIMAL) #if defined(ANY_FULL_MAG_PARSER)
void AuxMagSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) void AuxMagSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{ {
auxmagsupport_reload_settings(); auxmagsupport_reload_settings();
@ -629,8 +643,10 @@ void AuxMagSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
aux_hmc5x83_load_mag_settings(); aux_hmc5x83_load_mag_settings();
#endif #endif
} }
#endif /* defined(ANY_FULL_MAG_PARSER) */
#if defined(ANY_FULL_GPS_PARSER)
void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev) void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev)
{ {
ubx_autoconfig_settings_t newconfig; ubx_autoconfig_settings_t newconfig;
@ -749,7 +765,7 @@ void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev)
} }
#endif #endif
} }
#endif /* (defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER) || defined(PIOS_INCLUDE_GPS_DJI_PARSER)) && !defined(PIOS_GPS_MINIMAL) */ #endif /* defined(ANY_FULL_GPS_PARSER) */
/** /**
* @} * @}
* @} * @}