mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-20 10:54:14 +01:00
LP-212 DJI GPSMag support
This commit is contained in:
Cliff Geerdes
parent
48a76dd139
commit
2ae1f9c63d
@ -2,7 +2,8 @@
|
||||
******************************************************************************
|
||||
*
|
||||
* @file auxmagsupport.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2016.
|
||||
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
|
||||
* @brief Functions to handle aux mag data and calibration.
|
||||
* --
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
@ -57,6 +58,10 @@ void auxmagsupport_reload_settings()
|
||||
|
||||
// GPSV9, Ext (unused), and Flexi
|
||||
AuxMagSettingsTypeGet(&option);
|
||||
|
||||
const uint8_t status = AUXMAGSENSOR_STATUS_NONE;
|
||||
// next sample from other external mags will provide the right status if present
|
||||
AuxMagSensorStatusSet((uint8_t *)&status);
|
||||
}
|
||||
|
||||
void auxmagsupport_publish_samples(float mags[3], uint8_t status)
|
||||
|
||||
390
flight/modules/GPS/DJI.c
Normal file
390
flight/modules/GPS/DJI.c
Normal file
@ -0,0 +1,390 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup OpenPilotModules OpenPilot Modules
|
||||
* @{
|
||||
* @addtogroup GSPModule GPS Module
|
||||
* @brief Process GPS information (DJI-Naza binary format)
|
||||
* @{
|
||||
*
|
||||
* @file DJI.c
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
|
||||
* @brief GPS module, handles DJI stream
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "pios.h"
|
||||
#include "pios_math.h"
|
||||
#include <pios_helpers.h>
|
||||
#include <pios_delay.h>
|
||||
// dji parser is required for sensorType
|
||||
#if (defined(PIOS_INCLUDE_GPS_DJI_PARSER) && defined(PIOS_INCLUDE_GPS_DJI_PARSER))
|
||||
#include "inc/DJI.h"
|
||||
#include "inc/GPS.h"
|
||||
#include <string.h>
|
||||
|
||||
#include <auxmagsupport.h>
|
||||
|
||||
bool useMag = false;
|
||||
|
||||
// this is defined in DJI.c
|
||||
extern GPSPositionSensorSensorTypeOptions sensorType;
|
||||
|
||||
// parsing functions, roughly ordered by reception rate (higher rate messages on top)
|
||||
static void parse_dji_mag(struct DJIPacket *dji, GPSPositionSensorData *GpsPosition);
|
||||
static void parse_dji_gps(struct DJIPacket *dji, GPSPositionSensorData *GpsPosition);
|
||||
static void parse_dji_ver(struct DJIPacket *dji, GPSPositionSensorData *GpsPosition);
|
||||
|
||||
// parse table item
|
||||
typedef struct {
|
||||
uint8_t msgID;
|
||||
void (*handler)(struct DJIPacket *, GPSPositionSensorData *GpsPosition);
|
||||
} dji_message_handler;
|
||||
|
||||
const dji_message_handler dji_handler_table[] = {
|
||||
{ .msgID = DJI_ID_GPS, .handler = &parse_dji_gps },
|
||||
{ .msgID = DJI_ID_MAG, .handler = &parse_dji_mag },
|
||||
{ .msgID = DJI_ID_VER, .handler = &parse_dji_ver },
|
||||
};
|
||||
#define DJI_HANDLER_TABLE_SIZE NELEMENTS(dji_handler_table)
|
||||
|
||||
|
||||
// detected hw version
|
||||
uint32_t djiHwVersion = -1;
|
||||
uint32_t djiSwVersion = -1;
|
||||
|
||||
// parse incoming character stream for messages in DJI binary format
|
||||
int parse_dji_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPositionSensorData *GpsData, struct GPS_RX_STATS *gpsRxStats)
|
||||
{
|
||||
int ret = PARSER_INCOMPLETE; // message not (yet) complete
|
||||
enum proto_states {
|
||||
START,
|
||||
DJI_SY2,
|
||||
DJI_ID,
|
||||
DJI_LEN,
|
||||
DJI_PAYLOAD,
|
||||
DJI_CHK1,
|
||||
DJI_CHK2,
|
||||
FINISHED
|
||||
};
|
||||
enum restart_states {
|
||||
RESTART_WITH_ERROR,
|
||||
RESTART_NO_ERROR
|
||||
};
|
||||
uint8_t c;
|
||||
static enum proto_states proto_state = START;
|
||||
static uint16_t rx_count = 0;
|
||||
struct DJIPacket *dji = (struct DJIPacket *)gps_rx_buffer;
|
||||
uint16_t i = 0;
|
||||
uint16_t restart_index = 0;
|
||||
enum restart_states restart_state;
|
||||
static bool previous_packet_good = true;
|
||||
bool current_packet_good;
|
||||
|
||||
// switch continue is the normal condition and comes back to here for another byte
|
||||
// switch break is the error state that branches to the end and restarts the scan at the byte after the first sync byte
|
||||
while (i < len) {
|
||||
c = rx[i++];
|
||||
switch (proto_state) {
|
||||
case START: // detect protocol
|
||||
if (c == DJI_SYNC1) { // first DJI sync char found
|
||||
proto_state = DJI_SY2;
|
||||
// restart here, at byte after SYNC1, if we fail to parse
|
||||
restart_index = i; }
|
||||
continue;
|
||||
case DJI_SY2:
|
||||
if (c == DJI_SYNC2) { // second DJI sync char found
|
||||
proto_state = DJI_ID;
|
||||
} else {
|
||||
restart_state = RESTART_NO_ERROR;
|
||||
break;
|
||||
}
|
||||
continue;
|
||||
case DJI_ID:
|
||||
dji->header.id = c;
|
||||
proto_state = DJI_LEN;
|
||||
continue;
|
||||
case DJI_LEN:
|
||||
if (c > sizeof(DJIPayload)) {
|
||||
gpsRxStats->gpsRxOverflow++;
|
||||
#if defined(PIOS_GPS_MINIMAL)
|
||||
restart_state = RESTART_NO_ERROR;
|
||||
break;
|
||||
#else
|
||||
restart_state = RESTART_WITH_ERROR;
|
||||
break;
|
||||
#endif
|
||||
} else {
|
||||
dji->header.len = c;
|
||||
if (c == 0) {
|
||||
proto_state = DJI_CHK1;
|
||||
} else {
|
||||
rx_count = 0;
|
||||
proto_state = DJI_PAYLOAD;
|
||||
}
|
||||
}
|
||||
continue;
|
||||
case DJI_PAYLOAD:
|
||||
if (rx_count < dji->header.len) {
|
||||
dji->payload.payload[rx_count] = c;
|
||||
if (++rx_count == dji->header.len) {
|
||||
proto_state = DJI_CHK1;
|
||||
}
|
||||
}
|
||||
continue;
|
||||
case DJI_CHK1:
|
||||
dji->header.ck_a = c;
|
||||
proto_state = DJI_CHK2;
|
||||
continue;
|
||||
case DJI_CHK2:
|
||||
dji->header.ck_b = c;
|
||||
// ignore checksum errors on correct mag packets that nonetheless have checksum errors
|
||||
// these checksum errors happen very often on clone DJI GPS (never on real DJI GPS)
|
||||
// and are caused by a clone DJI GPS firmware error
|
||||
// the errors happen when it is time to send a non-mag packet (4 or 5 per second)
|
||||
// instead of a mag packet (30 per second)
|
||||
current_packet_good = checksum_dji_message(dji);
|
||||
// message complete and valid or (it's a mag packet and the previous "any" packet was good)
|
||||
if (current_packet_good || (dji->header.id==DJI_ID_MAG && previous_packet_good)) {
|
||||
parse_dji_message(dji, GpsData);
|
||||
gpsRxStats->gpsRxReceived++;
|
||||
proto_state = START;
|
||||
// overwrite PARSER_INCOMPLETE with PARSER_COMPLETE
|
||||
// but don't overwrite PARSER_ERROR with PARSER_COMPLETE
|
||||
// pass PARSER_ERROR to caller if it happens even once
|
||||
// only pass PARSER_COMPLETE back to caller if we parsed a full set of GPS data
|
||||
// that allows the caller to know if we are parsing GPS data
|
||||
// or just other packets for some reason (DJI clone firmware bug that happens sometimes)
|
||||
if (dji->header.id==DJI_ID_GPS && ret==PARSER_INCOMPLETE) {
|
||||
ret = PARSER_COMPLETE; // message complete & processed
|
||||
}
|
||||
} else {
|
||||
gpsRxStats->gpsRxChkSumError++;
|
||||
restart_state = RESTART_WITH_ERROR;
|
||||
previous_packet_good = false;
|
||||
break;
|
||||
}
|
||||
previous_packet_good = current_packet_good;
|
||||
continue;
|
||||
default:
|
||||
continue;
|
||||
}
|
||||
|
||||
// this simple restart doesn't work across calls
|
||||
// but it does work within a single call
|
||||
// and it does the expected thing across calls
|
||||
// if restarting due to error detected in 2nd call to this function (on split packet)
|
||||
// then we just restart at index 0, which is mid-packet, not the second byte
|
||||
if (restart_state == RESTART_WITH_ERROR) {
|
||||
ret = PARSER_ERROR; // inform caller that we found at least one error (along with 0 or more good packets)
|
||||
}
|
||||
rx += restart_index; // restart parsing just past the most recent SYNC1
|
||||
len -= restart_index;
|
||||
i = 0;
|
||||
proto_state = START;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
||||
bool checksum_dji_message(struct DJIPacket *dji)
|
||||
{
|
||||
int i;
|
||||
uint8_t ck_a, ck_b;
|
||||
|
||||
ck_a = dji->header.id;
|
||||
ck_b = ck_a;
|
||||
|
||||
ck_a += dji->header.len;
|
||||
ck_b += ck_a;
|
||||
|
||||
for (i = 0; i < dji->header.len; i++) {
|
||||
ck_a += dji->payload.payload[i];
|
||||
ck_b += ck_a;
|
||||
}
|
||||
|
||||
if (dji->header.ck_a == ck_a &&
|
||||
dji->header.ck_b == ck_b) {
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
static void parse_dji_gps(struct DJIPacket *dji, GPSPositionSensorData *GpsPosition)
|
||||
{
|
||||
static bool inited=false;
|
||||
if (!inited) {
|
||||
inited = true;
|
||||
// Is there a model calculation we can do to get a reasonable value for geoid separation?
|
||||
}
|
||||
|
||||
GPSVelocitySensorData GpsVelocity;
|
||||
struct DJI_GPS *gps = &dji->payload.gps;
|
||||
|
||||
// decode with xor mask
|
||||
uint8_t mask = gps->unused5;
|
||||
//for (uint8_t i=0; i<dji->header->len; ++i) {
|
||||
for (uint8_t i=0; i<56; ++i) {
|
||||
//if (i!=48 && i!=49 && i<=55) {
|
||||
if (i!=48 && i!=49) {
|
||||
dji->payload.payload[i]^=mask;
|
||||
}
|
||||
}
|
||||
|
||||
GpsVelocity.North = (float)gps->velN * 0.01f;
|
||||
GpsVelocity.East = (float)gps->velE * 0.01f;
|
||||
GpsVelocity.Down = (float)gps->velD * 0.01f;
|
||||
GPSVelocitySensorSet(&GpsVelocity);
|
||||
|
||||
GpsPosition->Groundspeed = sqrtf(GpsVelocity.North*GpsVelocity.North + GpsVelocity.East*GpsVelocity.East);
|
||||
GpsPosition->Heading = RAD2DEG(atan2f(-GpsVelocity.East, -GpsVelocity.North)) + 180.0f;
|
||||
GpsPosition->Altitude = (float)gps->hMSL * 0.001f;
|
||||
// there is no source of geoid separation data in the DJI protocol
|
||||
GpsPosition->GeoidSeparation = 0.0f;
|
||||
GpsPosition->Latitude = gps->lat;
|
||||
GpsPosition->Longitude = gps->lon;
|
||||
GpsPosition->Satellites = gps->numSV;
|
||||
GpsPosition->PDOP = gps->pDOP * 0.01f;
|
||||
// cliffg FIXME
|
||||
// might get away with just using a max function here...
|
||||
// hmmm pdop=sqrt(hdop*hdop+hdop*hdop+vdop*vdop)
|
||||
// pdop*pdop=hdop*hdop+hdop*hdop+vdop*vdop
|
||||
// pdop*pdop-vdop*vdop=hdop*hdop+hdop*hdop
|
||||
// (pdop*pdop-vdop*vdop)/2=hdop*hdop
|
||||
// srtq(pdop*pdop-vdop*vdop)/2=hdop
|
||||
GpsPosition->HDOP = sqrtf((float)gps->nDOP*(float)gps->nDOP + (float)gps->eDOP*(float)gps->eDOP) * 0.01f;
|
||||
GpsPosition->VDOP = gps->vDOP * 0.01f;
|
||||
if (gps->flags & FLAGS_GPSFIX_OK) {
|
||||
GpsPosition->Status = gps->fixType == FIXTYPE_3D ?
|
||||
GPSPOSITIONSENSOR_STATUS_FIX3D : GPSPOSITIONSENSOR_STATUS_FIX2D;
|
||||
} else {
|
||||
GpsPosition->Status = GPSPOSITIONSENSOR_STATUS_NOFIX;
|
||||
}
|
||||
GpsPosition->SensorType = GPSPOSITIONSENSOR_SENSORTYPE_DJI;
|
||||
GpsPosition->AutoConfigStatus = GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DISABLED;
|
||||
//GpsPosition->BaudRate = GPSPOSITIONSENSOR_BAUDRATE_115200;
|
||||
GPSPositionSensorSet(GpsPosition);
|
||||
|
||||
// Time is valid, set GpsTime
|
||||
GPSTimeData GpsTime;
|
||||
// cliffg FIXME
|
||||
// the lowest bit of day and the highest bit of hour overlap (xored? no stranger than that)
|
||||
// this causes strange day/hour changes
|
||||
// we could track it here and even if we guess wrong initially
|
||||
// we can massage the data so that time doesn't jump
|
||||
// and maybe make the assumption that most people will fly at 5pm, not 1am
|
||||
// so if it looks if we have to make a choice between 5pm on the 10th and
|
||||
GpsTime.Year = (int16_t)gps->year + 2000;
|
||||
GpsTime.Month = gps->month;
|
||||
GpsTime.Day = gps->day;
|
||||
GpsTime.Hour = gps->hour;
|
||||
GpsTime.Minute = gps->min;
|
||||
GpsTime.Second = gps->sec;
|
||||
GPSTimeSet(&GpsTime);
|
||||
}
|
||||
|
||||
|
||||
static void parse_dji_mag(struct DJIPacket *dji, __attribute__((unused)) GPSPositionSensorData *GpsPosition)
|
||||
{
|
||||
if (!useMag) {
|
||||
return;
|
||||
}
|
||||
struct DJI_MAG *mag = &dji->payload.mag;
|
||||
union {
|
||||
struct {
|
||||
int8_t mask;
|
||||
int8_t mask2;
|
||||
};
|
||||
int16_t maskmask;
|
||||
} u;
|
||||
u.mask = (int8_t) (dji->payload.payload[4]);
|
||||
u.mask = u.mask2 = (((u.mask ^ (u.mask >> 4)) & 0x0F) | ((u.mask << 3) & 0xF0)) ^ (((u.mask & 0x01) << 3) | ((u.mask & 0x01) << 7));
|
||||
// yes, z is only xored by mask<<8, not maskmask
|
||||
float mags[3] = { mag->x^u.maskmask, mag->y^u.maskmask, mag->z^((int16_t)u.mask<<8) };
|
||||
auxmagsupport_publish_samples(mags, AUXMAGSENSOR_STATUS_OK);
|
||||
}
|
||||
|
||||
|
||||
static void parse_dji_ver(struct DJIPacket *dji, __attribute__((unused)) GPSPositionSensorData *GpsPosition)
|
||||
{
|
||||
struct DJI_VER *ver = &dji->payload.ver;
|
||||
|
||||
// decode with xor mask
|
||||
uint8_t mask = (uint8_t)(ver->unused1);
|
||||
//for (uint8_t i=0; i<dji->header->len; ++i) {
|
||||
for (uint8_t i=4; i<12; ++i) {
|
||||
dji->payload.payload[i]^=mask;
|
||||
}
|
||||
|
||||
djiHwVersion = ver->hwVersion;
|
||||
djiSwVersion = ver->swVersion;
|
||||
sensorType = GPSPOSITIONSENSOR_SENSORTYPE_DJI;
|
||||
GPSPositionSensorSensorTypeSet((uint8_t *)&sensorType);
|
||||
}
|
||||
|
||||
|
||||
// DJI message parser
|
||||
// returns UAVObjectID if a UAVObject structure is ready for further processing
|
||||
|
||||
uint32_t parse_dji_message(struct DJIPacket *dji, GPSPositionSensorData *GpsPosition)
|
||||
{
|
||||
uint32_t id = 0;
|
||||
static bool djiInitialized = false;
|
||||
|
||||
if (!djiInitialized) {
|
||||
// initialize dop values. If no DOP sentence is received it is safer to initialize them to a high value rather than 0.
|
||||
GpsPosition->HDOP = 99.99f;
|
||||
GpsPosition->PDOP = 99.99f;
|
||||
GpsPosition->VDOP = 99.99f;
|
||||
djiInitialized = true;
|
||||
}
|
||||
|
||||
for (uint8_t i = 0; i < DJI_HANDLER_TABLE_SIZE; i++) {
|
||||
const dji_message_handler *handler = &dji_handler_table[i];
|
||||
if (handler->msgID == dji->header.id) {
|
||||
handler->handler(dji, GpsPosition);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
uint8_t status;
|
||||
GPSPositionSensorStatusGet(&status);
|
||||
if (status == GPSPOSITIONSENSOR_STATUS_NOGPS) {
|
||||
// Some dji thing has been received so GPS is there
|
||||
status = GPSPOSITIONSENSOR_STATUS_NOFIX;
|
||||
GPSPositionSensorStatusSet(&status);
|
||||
}
|
||||
}
|
||||
|
||||
return id;
|
||||
}
|
||||
|
||||
void dji_load_mag_settings()
|
||||
{
|
||||
if (auxmagsupport_get_type() == AUXMAGSETTINGS_TYPE_DJI) {
|
||||
useMag = true;
|
||||
} else {
|
||||
useMag = false;
|
||||
}
|
||||
}
|
||||
#endif // PIOS_INCLUDE_GPS_DJI_PARSER
|
||||
@ -7,7 +7,8 @@
|
||||
* @{
|
||||
*
|
||||
* @file GPS.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
|
||||
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief GPS module, handles GPS and NMEA stream
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
@ -48,9 +49,11 @@
|
||||
#include "GPS.h"
|
||||
#include "NMEA.h"
|
||||
#include "UBX.h"
|
||||
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
|
||||
#include "DJI.h"
|
||||
#if (defined(PIOS_INCLUDE_GPS_UBX_PARSER) || defined(PIOS_INCLUDE_GPS_DJI_PARSER)) && !defined(PIOS_GPS_MINIMAL)
|
||||
#include "inc/ubx_autoconfig.h"
|
||||
#endif
|
||||
#include <auxmagsupport.h>
|
||||
|
||||
#include <pios_instrumentation_helper.h>
|
||||
PERF_DEFINE_COUNTER(counterBytesIn);
|
||||
@ -67,7 +70,7 @@ static void setHomeLocation(GPSPositionSensorData *gpsData);
|
||||
static float GravityAccel(float latitude, float longitude, float altitude);
|
||||
#endif
|
||||
|
||||
#ifdef PIOS_INCLUDE_GPS_UBX_PARSER
|
||||
#if (defined(PIOS_INCLUDE_GPS_UBX_PARSER) || defined(PIOS_INCLUDE_GPS_DJI_PARSER))
|
||||
void AuxMagSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
|
||||
#ifndef PIOS_GPS_MINIMAL
|
||||
void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev);
|
||||
@ -124,7 +127,7 @@ static char *gps_rx_buffer;
|
||||
|
||||
static uint32_t timeOfLastUpdateMs;
|
||||
|
||||
#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER)
|
||||
#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER) || defined(PIOS_INCLUDE_GPS_DJI_PARSER)
|
||||
static struct GPS_RX_STATS gpsRxStats;
|
||||
#endif
|
||||
|
||||
@ -178,7 +181,7 @@ int32_t GPSInitialize(void)
|
||||
GPSTimeInitialize();
|
||||
GPSSatellitesInitialize();
|
||||
HomeLocationInitialize();
|
||||
#ifdef PIOS_INCLUDE_GPS_UBX_PARSER
|
||||
#if (defined(PIOS_INCLUDE_GPS_UBX_PARSER) || defined(PIOS_INCLUDE_GPS_DJI_PARSER))
|
||||
AuxMagSensorInitialize();
|
||||
AuxMagSettingsInitialize();
|
||||
GPSExtendedStatusInitialize();
|
||||
@ -212,19 +215,34 @@ int32_t GPSInitialize(void)
|
||||
#endif /* if defined(REVOLUTION) */
|
||||
|
||||
if (gpsEnabled) {
|
||||
#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) && defined(PIOS_INCLUDE_GPS_UBX_PARSER)
|
||||
gps_rx_buffer = pios_malloc((sizeof(struct UBXPacket) > NMEA_MAX_PACKET_LENGTH) ? sizeof(struct UBXPacket) : NMEA_MAX_PACKET_LENGTH);
|
||||
#elif defined(PIOS_INCLUDE_GPS_UBX_PARSER)
|
||||
#if defined(PIOS_GPS_MINIMAL)
|
||||
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
|
||||
gps_rx_buffer = pios_malloc(sizeof(struct UBXPacket));
|
||||
#elif defined(PIOS_INCLUDE_GPS_NMEA_PARSER)
|
||||
gps_rx_buffer = pios_malloc(NMEA_MAX_PACKET_LENGTH);
|
||||
#else
|
||||
gps_rx_buffer = NULL;
|
||||
#endif
|
||||
#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER)
|
||||
#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(PIOS_INCLUDE_GPS_NMEA_PARSER)
|
||||
size_t bufSize = NMEA_MAX_PACKET_LENGTH;
|
||||
#else
|
||||
size_t bufSize = 0;
|
||||
#endif
|
||||
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
|
||||
if (bufSize < sizeof(struct UBXPacket)) bufSize = sizeof(struct UBXPacket);
|
||||
#endif
|
||||
#if defined(PIOS_INCLUDE_GPS_DJI_PARSER)
|
||||
if (bufSize < sizeof(struct DJIPacket)) bufSize = sizeof(struct DJIPacket);
|
||||
#endif
|
||||
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) */
|
||||
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(PIOS_GPS_MINIMAL) */
|
||||
#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER) || defined(PIOS_INCLUDE_GPS_DJI_PARSER)
|
||||
PIOS_Assert(gps_rx_buffer);
|
||||
#endif
|
||||
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
|
||||
#if (defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER) || defined(PIOS_INCLUDE_GPS_DJI_PARSER)) && !defined(PIOS_GPS_MINIMAL)
|
||||
HwSettingsConnectCallback(updateHwSettings); // allow changing baud rate even after startup
|
||||
GPSSettingsConnectCallback(updateGpsSettings);
|
||||
#endif
|
||||
@ -251,17 +269,15 @@ static void gpsTask(__attribute__((unused)) void *parameters)
|
||||
// 100ms is way slow too, considering we do everything possible to make the sensor data as contemporary as possible
|
||||
portTickType xDelay = 5 / portTICK_RATE_MS;
|
||||
uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
|
||||
|
||||
#ifdef PIOS_GPS_SETS_HOMELOCATION
|
||||
portTickType homelocationSetDelay = 0;
|
||||
#endif
|
||||
GPSPositionSensorData gpspositionsensor;
|
||||
|
||||
timeOfLastUpdateMs = timeNowMs;
|
||||
|
||||
GPSPositionSensorGet(&gpspositionsensor);
|
||||
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
|
||||
// this should be done in the task because it calls out to actually start the GPS serial reads
|
||||
#if (defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER) || defined(PIOS_INCLUDE_GPS_DJI_PARSER)) && !defined(PIOS_GPS_MINIMAL)
|
||||
// this should be done in the task because it calls out to actually start the ubx GPS serial reads
|
||||
updateGpsSettings(0);
|
||||
#endif
|
||||
|
||||
@ -321,7 +337,7 @@ static void gpsTask(__attribute__((unused)) void *parameters)
|
||||
PERF_TRACK_VALUE(counterBytesIn, cnt);
|
||||
PERF_MEASURE_PERIOD(counterRate);
|
||||
switch (gpsSettings.DataProtocol) {
|
||||
#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER)
|
||||
#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER) && !defined(PIOS_GPS_MINIMAL)
|
||||
case GPSSETTINGS_DATAPROTOCOL_NMEA:
|
||||
res = parse_nmea_stream(c, cnt, gps_rx_buffer, &gpspositionsensor, &gpsRxStats);
|
||||
break;
|
||||
@ -330,6 +346,11 @@ static void gpsTask(__attribute__((unused)) void *parameters)
|
||||
case GPSSETTINGS_DATAPROTOCOL_UBX:
|
||||
res = parse_ubx_stream(c, cnt, gps_rx_buffer, &gpspositionsensor, &gpsRxStats);
|
||||
break;
|
||||
#endif
|
||||
#if defined(PIOS_INCLUDE_GPS_DJI_PARSER) && !defined(PIOS_GPS_MINIMAL)
|
||||
case GPSSETTINGS_DATAPROTOCOL_DJI:
|
||||
res = parse_dji_stream(c, cnt, gps_rx_buffer, &gpspositionsensor, &gpsRxStats);
|
||||
break;
|
||||
#endif
|
||||
default:
|
||||
res = NO_PARSER; // this should not happen
|
||||
@ -343,8 +364,13 @@ static void gpsTask(__attribute__((unused)) void *parameters)
|
||||
}
|
||||
}
|
||||
|
||||
// if there is any error at all, set status to NOGPS
|
||||
// Check for GPS timeout
|
||||
// if there is a protocol error or communication error, or timeout error,
|
||||
// generally, if there is an error that is due to configuration or bad hardware, set status to NOGPS
|
||||
// poor GPS signal gets a different error/alarm (below)
|
||||
//
|
||||
// should this be expanded to include aux mag status as well? currently the aux mag
|
||||
// attached to a GPS protocol (OPV9 and DJI) still says OK after the GPS/mag goes down
|
||||
// (data cable unplugged or flight battery removed with USB still powering the FC)
|
||||
timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
|
||||
if ((res == PARSER_ERROR) ||
|
||||
(timeNowMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS ||
|
||||
@ -356,10 +382,15 @@ static void gpsTask(__attribute__((unused)) void *parameters)
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR);
|
||||
}
|
||||
// if we parsed at least one packet successfully
|
||||
// we aren't offline, but we still may not have a good enough fix to fly
|
||||
// or we might not even be receiving all necessary GPS packets (NMEA)
|
||||
else if (res == PARSER_COMPLETE) {
|
||||
// we appear to be receiving GPS sentences OK, we've had an update
|
||||
// we appear to be receiving packets OK
|
||||
// criteria for GPS-OK taken from this post...
|
||||
// http://forums.openpilot.org/topic/1523-professors-insgps-in-svn/page__view__findpost__p__5220
|
||||
// NMEA doesn't verify that all necessary packet types for an update have been received
|
||||
//
|
||||
// if (the fix is good) {
|
||||
if ((gpspositionsensor.PDOP < gpsSettings.MaxPDOP) && (gpspositionsensor.Satellites >= gpsSettings.MinSatellites) &&
|
||||
(gpspositionsensor.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) &&
|
||||
(gpspositionsensor.Latitude != 0 || gpspositionsensor.Longitude != 0)) {
|
||||
@ -380,9 +411,11 @@ static void gpsTask(__attribute__((unused)) void *parameters)
|
||||
homelocationSetDelay = 0;
|
||||
}
|
||||
#endif
|
||||
// else if (we are at least getting what might be usable GPS data to finish a flight with) {
|
||||
} else if ((gpspositionsensor.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) &&
|
||||
(gpspositionsensor.Latitude != 0 || gpspositionsensor.Longitude != 0)) {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_WARNING);
|
||||
// else data is probably not good enough to fly
|
||||
} else {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL);
|
||||
}
|
||||
@ -498,60 +531,95 @@ void gps_set_fc_baud_from_arg(uint8_t baud)
|
||||
}
|
||||
|
||||
|
||||
// set the FC port baud rate
|
||||
// from HwSettings or override with 115200 if DJI
|
||||
static void gps_set_fc_baud_from_settings()
|
||||
{
|
||||
uint8_t speed;
|
||||
// Retrieve settings
|
||||
#if defined(PIOS_INCLUDE_GPS_DJI_PARSER) && !defined(PIOS_GPS_MINIMAL)
|
||||
if (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_DJI) {
|
||||
speed = HWSETTINGS_GPSSPEED_115200;
|
||||
} else {
|
||||
#endif
|
||||
HwSettingsGPSSpeedGet(&speed);
|
||||
#if defined(PIOS_INCLUDE_GPS_DJI_PARSER) && !defined(PIOS_GPS_MINIMAL)
|
||||
}
|
||||
#endif
|
||||
// set fc baud
|
||||
gps_set_fc_baud_from_arg(speed);
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Update the GPS settings, called on startup.
|
||||
* FIXME: This should be in the GPSSettings object. But objects have
|
||||
* too much overhead yet. Also the GPS has no any specific settings
|
||||
* like protocol, etc. Thus the HwSettings object which contains the
|
||||
* GPS port speed is used for now.
|
||||
* HwSettings callback
|
||||
* Generally speaking, set the FC baud rate
|
||||
* and for UBX, if it is safe, start the auto config process
|
||||
* this allows the user to change the baud rate and see if it works without rebooting
|
||||
*/
|
||||
|
||||
// must updateHwSettings() before updateGpsSettings() so baud rate is set before GPS serial code starts running
|
||||
static void updateHwSettings(UAVObjEvent __attribute__((unused)) *ev)
|
||||
{
|
||||
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
|
||||
#if (defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER) || defined(PIOS_INCLUDE_GPS_DJI_PARSER)) && !defined(PIOS_GPS_MINIMAL)
|
||||
static uint32_t previousGpsPort = 0xf0f0f0f0; // = doesn't match gpsport
|
||||
#endif
|
||||
// if GPS (UBX or NMEA) is enabled at all
|
||||
if (gpsPort && gpsEnabled) {
|
||||
// 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 in startup, or not configured to do ubx and ubx auto config
|
||||
//
|
||||
// on first use of this port (previousGpsPort != gpsPort) we must set the Revo port baud rate
|
||||
// after startup (previousGpsPort == gpsPort) we must set the Revo port baud rate only if autoconfig is disabled
|
||||
// always we must set the Revo port baud rate if not using UBX
|
||||
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
|
||||
if (ev == NULL
|
||||
|| previousGpsPort != gpsPort
|
||||
|| gpsSettings.UbxAutoConfig == GPSSETTINGS_UBXAUTOCONFIG_DISABLED
|
||||
|| gpsSettings.DataProtocol != GPSSETTINGS_DATAPROTOCOL_UBX)
|
||||
#endif
|
||||
{
|
||||
uint8_t speed;
|
||||
// Retrieve settings
|
||||
HwSettingsGPSSpeedGet(&speed);
|
||||
// set fc baud
|
||||
gps_set_fc_baud_from_arg(speed);
|
||||
// always set the baud rate
|
||||
gps_set_fc_baud_from_settings();
|
||||
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
|
||||
// even changing the baud rate will make it re-verify the sensor type
|
||||
// that way the user can just try some baud rates and when the sensor type is valid, the baud rate is correct
|
||||
// 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
|
||||
gps_ubx_reset_sensor_type();
|
||||
#endif
|
||||
}
|
||||
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
|
||||
else {
|
||||
// it will never do this during startup because ev == NULL
|
||||
// else:
|
||||
// - we are past startup
|
||||
// - we are running uxb protocol
|
||||
// - and some form of ubx auto config is enabled
|
||||
//
|
||||
// it will never do this during startup because ev == NULL during startup
|
||||
//
|
||||
// this is here so that runtime (not startup) user changes to HwSettings will restart auto-config
|
||||
gps_ubx_autoconfig_set(NULL);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
|
||||
#if (defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER) || defined(PIOS_INCLUDE_GPS_DJI_PARSER)) && !defined(PIOS_GPS_MINIMAL)
|
||||
previousGpsPort = gpsPort;
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
|
||||
#if (defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER) || defined(PIOS_INCLUDE_GPS_DJI_PARSER)) && !defined(PIOS_GPS_MINIMAL)
|
||||
void AuxMagSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
|
||||
{
|
||||
auxmagsupport_reload_settings();
|
||||
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
|
||||
op_gpsv9_load_mag_settings();
|
||||
#endif
|
||||
#if defined(PIOS_INCLUDE_GPS_DJI_PARSER)
|
||||
dji_load_mag_settings();
|
||||
#endif
|
||||
#if defined(PIOS_INCLUDE_HMC5X83)
|
||||
aux_hmc5x83_load_mag_settings();
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
@ -561,9 +629,17 @@ void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev)
|
||||
|
||||
GPSSettingsGet(&gpsSettings);
|
||||
|
||||
// it's OK that ubx auto config is inited and ready to go, when GPS is disabled or running NMEA
|
||||
#if defined(PIOS_INCLUDE_GPS_DJI_PARSER)
|
||||
// each time there is a protocol change, set the baud rate
|
||||
// so that DJI can be forced to 115200, but changing to another protocol will change the baud rate to the user specified value
|
||||
// note that changes to HwSettings GPS baud rate are detected in the HwSettings callback,
|
||||
// but changing to/from DJI is effectively a baud rate change because DJI is forced to be 115200
|
||||
gps_set_fc_baud_from_settings(); // knows to force 115200 for DJI
|
||||
#endif
|
||||
|
||||
// it's OK that ubx auto config is inited and ready to go, when GPS is disabled or running another protocol
|
||||
// because ubx auto config never gets called
|
||||
// setting it up completely means that if we switch from initial NMEA to UBX or disabled to enabled, that it will start normally
|
||||
// setting it up completely means that if we switch from the other protocol to UBX or disabled to enabled, that it will start normally
|
||||
newconfig.UbxAutoConfig = gpsSettings.UbxAutoConfig;
|
||||
newconfig.navRate = gpsSettings.UbxRate;
|
||||
newconfig.dynamicModel = gpsSettings.UbxDynamicModel == GPSSETTINGS_UBXDYNAMICMODEL_PORTABLE ? UBX_DYNMODEL_PORTABLE :
|
||||
@ -650,9 +726,22 @@ void updateGpsSettings(__attribute__((unused)) UAVObjEvent *ev)
|
||||
break;
|
||||
}
|
||||
|
||||
gps_ubx_autoconfig_set(&newconfig);
|
||||
// handle protocol changes and devices that have just been powered up
|
||||
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
|
||||
if (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX) {
|
||||
// do whatever the user has configured for power on startup
|
||||
// even autoconfig disabled still gets the ubx version
|
||||
gps_ubx_autoconfig_set(&newconfig);
|
||||
}
|
||||
#endif
|
||||
#if defined(PIOS_INCLUDE_GPS_DJI_PARSER)
|
||||
if (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_DJI) {
|
||||
// clear out satellite data because DJI doesn't provide it
|
||||
GPSSatellitesSetDefaults(GPSSatellitesHandle(), 0);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
#endif /* if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL) */
|
||||
#endif /* (defined(PIOS_INCLUDE_GPS_NMEA_PARSER) || defined(PIOS_INCLUDE_GPS_UBX_PARSER) || defined(PIOS_INCLUDE_GPS_DJI_PARSER)) && !defined(PIOS_GPS_MINIMAL) */
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
|
||||
@ -7,7 +7,8 @@
|
||||
* @{
|
||||
*
|
||||
* @file NMEA.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
|
||||
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief GPS module, handles GPS and NMEA stream
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
|
||||
@ -7,7 +7,8 @@
|
||||
* @{
|
||||
*
|
||||
* @file UBX.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2016.
|
||||
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
|
||||
* @brief GPS module, handles GPS and NMEA stream
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
@ -130,7 +131,7 @@ int parse_ubx_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPosition
|
||||
static enum proto_states proto_state = START;
|
||||
static uint16_t rx_count = 0;
|
||||
struct UBXPacket *ubx = (struct UBXPacket *)gps_rx_buffer;
|
||||
uint16_t i = 0;
|
||||
uint16_t i = 0;
|
||||
uint16_t restart_index = 0;
|
||||
enum restart_states restart_state;
|
||||
|
||||
@ -204,15 +205,20 @@ int parse_ubx_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPosition
|
||||
// this has been proven by running it without autoconfig and testing:
|
||||
// data coming from OPV9 "GPS+MCU" port the checksum errors happen roughly every 5 to 30 seconds
|
||||
// same data coming from OPV9 "GPS Only" port the checksums are always good
|
||||
// this also ocassionally causes parse_ubx_message() to issue alarms because not all the messages were received
|
||||
// this also occasionally causes parse_ubx_message() to issue alarms because not all the messages were received
|
||||
// see OP GPSV9 comment in parse_ubx_message() for further information
|
||||
if (checksum_ubx_message(ubx)) { // message complete and valid
|
||||
parse_ubx_message(ubx, GpsData);
|
||||
if (checksum_ubx_message(ubx)) {
|
||||
gpsRxStats->gpsRxReceived++;
|
||||
proto_state = START;
|
||||
// pass PARSER_ERROR to be to caller if it happens even once
|
||||
if (ret == PARSER_INCOMPLETE) {
|
||||
ret = PARSER_COMPLETE; // message complete & processed
|
||||
// overwrite PARSER_INCOMPLETE with PARSER_COMPLETE
|
||||
// but don't overwrite PARSER_ERROR with PARSER_COMPLETE
|
||||
// pass PARSER_ERROR to caller if it happens even once
|
||||
// only pass PARSER_COMPLETE back to caller if we parsed a full set of GPS data
|
||||
// that allows the caller to know if we are parsing GPS data
|
||||
// or just other packets for some reason (mis-configuration)
|
||||
if (parse_ubx_message(ubx, GpsData)==GPSPOSITIONSENSOR_OBJID
|
||||
&& ret == PARSER_INCOMPLETE) {
|
||||
ret = PARSER_COMPLETE;
|
||||
}
|
||||
} else {
|
||||
gpsRxStats->gpsRxChkSumError++;
|
||||
@ -230,9 +236,8 @@ int parse_ubx_stream(uint8_t *rx, uint16_t len, char *gps_rx_buffer, GPSPosition
|
||||
// if restarting due to error detected in 2nd call to this function (on split packet)
|
||||
// then we just restart at index 0, which is mid-packet, not the second byte
|
||||
if (restart_state == RESTART_WITH_ERROR) {
|
||||
ret = PARSER_ERROR; // inform caller that we found at least one error (along with 0 or more good packets)
|
||||
ret = PARSER_ERROR; // inform caller that we found at least one error (along with 0 or more good packets)
|
||||
}
|
||||
// else restart with no error
|
||||
rx += restart_index; // restart parsing just past the most recent SYNC1
|
||||
len -= restart_index;
|
||||
i = 0;
|
||||
|
||||
332
flight/modules/GPS/inc/DJI.h
Normal file
332
flight/modules/GPS/inc/DJI.h
Normal file
@ -0,0 +1,332 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup OpenPilotModules OpenPilot Modules
|
||||
* @{
|
||||
* @addtogroup GSPModule GPS Module
|
||||
* @brief Process GPS information
|
||||
* @{
|
||||
*
|
||||
* @file DJI.h
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
|
||||
* @brief GPS module, handles DJI stream
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#ifndef DJI_H
|
||||
#define DJI_H
|
||||
#include "openpilot.h"
|
||||
#include "gpspositionsensor.h"
|
||||
#include "gpsextendedstatus.h"
|
||||
#ifndef PIOS_GPS_MINIMAL
|
||||
#include "auxmagsensor.h"
|
||||
#endif
|
||||
|
||||
#include "GPS.h"
|
||||
|
||||
#define DJI_SYNC1 0x55 // DJI protocol synchronization characters
|
||||
#define DJI_SYNC2 0xaa
|
||||
|
||||
// Message IDs
|
||||
typedef enum {
|
||||
DJI_ID_GPS = 0x10,
|
||||
DJI_ID_MAG = 0x20,
|
||||
DJI_ID_VER = 0x30,
|
||||
} dji_msg_id;
|
||||
|
||||
// private structures
|
||||
|
||||
/*
|
||||
GPS protocol info from
|
||||
http://www.rcgroups.com/forums/showpost.php?p=26210591&postcount=15
|
||||
|
||||
The 0x10 message contains GPS data. Here is the structure of the message,
|
||||
fields marked with XX I'm not sure about yet. The others will be described below.
|
||||
|
||||
55 AA 10 3A DT DT DT DT LO LO LO LO LA LA LA LA AL AL AL AL HA HA HA HA VA VA VA VA XX XX XX XX
|
||||
NV NV NV NV EV EV EV EV DV DV DV DV PD PD VD VD ND ND ED ED NS XX FT XX SF XX XX XM SN SN CS CS
|
||||
|
||||
The payload is XORed with a mask that changes over time (see below for more details).
|
||||
|
||||
Values in the message are stored in little endian.
|
||||
|
||||
HEADER
|
||||
-------------
|
||||
BYTE 1-2: message header - always 55 AA
|
||||
BYTE 3: message id (0x10 for GPS message)
|
||||
BYTE 4: lenght of the payload (0x3A or 58 decimal for 0x10 message)
|
||||
|
||||
PAYLOAD
|
||||
--------------
|
||||
BYTE 5-8 (DT) : date and time, see details below
|
||||
BYTE 9-12 (LO) : longitude (x10^7, degree decimal)
|
||||
BYTE 13-16 (LA): latitude (x10^7, degree decimal)
|
||||
BYTE 17-20 (AL): altitude (in milimeters)
|
||||
BYTE 21-24 (HA): horizontal accuracy estimate (see uBlox NAV-POSLLH message for details)
|
||||
BYTE 25-28 (VA): vertical accuracy estimate (see uBlox NAV-POSLLH message for details)
|
||||
BYTE 29-32 : ??? (seems to be always 0)
|
||||
BYTE 33-36 (NV): NED north velocity (see uBlox NAV-VELNED message for details)
|
||||
BYTE 37-40 (EV): NED east velocity (see uBlox NAV-VELNED message for details)
|
||||
BYTE 41-44 (DV): NED down velocity (see uBlox NAV-VELNED message for details)
|
||||
BYTE 45-46 (PD): position DOP (see uBlox NAV-DOP message for details)
|
||||
BYTE 47-48 (VD): vertical DOP (see uBlox NAV-DOP message for details)
|
||||
BYTE 49-50 (ND): northing DOP (see uBlox NAV-DOP message for details)
|
||||
BYTE 51-52 (ED): easting DOP (see uBlox NAV-DOP message for details)
|
||||
BYTE 53 (NS) : number of satellites (not XORed)
|
||||
BYTE 54 : ??? (not XORed, seems to be always 0)
|
||||
BYTE 55 (FT) : fix type (0 - no lock, 2 - 2D lock, 3 - 3D lock, not sure if other values can be expected
|
||||
: see uBlox NAV-SOL message for details)
|
||||
BYTE 56 : ??? (seems to be always 0)
|
||||
BYTE 57 (SF) : fix status flags (see uBlox NAV-SOL message for details)
|
||||
BYTE 58-59 : ??? (seems to be always 0)
|
||||
BYTE 60 (XM) : not sure yet, but I use it as the XOR mask
|
||||
BYTE 61-62 (SN): sequence number (not XORed), once there is a lock - increases with every message.
|
||||
When the lock is lost later LSB and MSB are swapped with every message.
|
||||
|
||||
CHECKSUM
|
||||
-----------------
|
||||
BYTE 63-64 (CS): checksum, calculated the same way as for uBlox binary messages
|
||||
|
||||
|
||||
XOR mask
|
||||
---------------
|
||||
All bytes of the payload except 53rd (NS), 54th, 61st (SN LSB) and 62nd (SN MSB) are XORed with a mask.
|
||||
Mask is calculated based on the value of byte 53rd (NS) and 61st (SN LSB).
|
||||
|
||||
If we index bits from LSB to MSB as 0-7 we have:
|
||||
mask[0] = 53rdByte[0] xor 61stByte[4]
|
||||
mask[1] = 53rdByte[1] xor 61stByte[5]
|
||||
mask[2] = 53rdByte[2] xor 61stByte[6]
|
||||
mask[3] = 53rdByte[3] xor 61stByte[7] xor 53rdByte[0];
|
||||
mask[4] = 53rdByte[1];
|
||||
mask[5] = 53rdByte[2];
|
||||
mask[6] = 53rdByte[3];
|
||||
mask[7] = 53rdByte[0] xor 61stByte[4];
|
||||
|
||||
To simplify calculations any of the unknown bytes that when XORer seem to be always 0 (29-32, 56, 58-60)
|
||||
can be used as XOR mask (based on the fact that 0 XOR mask == mask). In the library I use byte 60.
|
||||
|
||||
Date and time format
|
||||
----------------------------
|
||||
Date (Year, Month, Day) and time (Hour, Minute, Second) are stored as little endian 32bit unsigned integer,
|
||||
the meaning of particular bits is as follows:
|
||||
|
||||
YYYYYYYMMMMDDDDDHHHHMMMMMMSSSSSS
|
||||
|
||||
NOTE 1: to get the day value correct you must add 1 when hour is > 7
|
||||
NOTE 2: for the time between 16:00 and 23:59 the hour will be returned as 0-7
|
||||
and there seems to be no way to differentiate between 00:00 - 07:59 and 16:00 - 23:59.
|
||||
From further discussion in the thread, it sounds like the day is written into the buffer
|
||||
(buffer initially zero bits) and then the hour is xored into the buffer
|
||||
with the bottom bit of day and top bit of hour mapped to the same buffer bit?
|
||||
Is that even correct? Or could we have a correct hour and the day is just wrong?
|
||||
|
||||
http://www.rcgroups.com/forums/showpost.php?p=28158918&postcount=180
|
||||
Midnight between 13th and 14th of March
|
||||
0001110 0011 01110 0000 000000 000000 -> 14.3.14 00:00:00 -> 0
|
||||
identical with
|
||||
4PM, 14th of March
|
||||
0001110 0011 01110 0000 000000 000000 -> 14.3.14 00:00:00 -> 0
|
||||
|
||||
Midnight between 14th and 15th of March
|
||||
0001110 0011 01111 0000 000000 000000 -> 14.3.15 00:00:00 -> 16
|
||||
identical with
|
||||
4PM, 15th of March
|
||||
0001110 0011 01111 0000 000000 000000 -> 14.3.15 00:00:00 -> 16
|
||||
|
||||
So as you can see even if we take 5 bits the hour is not correct either
|
||||
Are they are xored? If we knew the date from a different source we would know the time.
|
||||
Maybe the xor mask itself contains the bit. Does the mask change? and how? across the transitions.
|
||||
|
||||
http://www.rcgroups.com/forums/showpost.php?p=28168741&postcount=182
|
||||
Originally Posted by gwouite View Post
|
||||
Question, are you sure that at 4PM, you're day value doesn't increase of 1 ?
|
||||
It does, but it also does decrease by 1 at 8am so you have:
|
||||
00:00 - 07:59 => day = X, hour = 0 - 7
|
||||
08:00 - 15:59 => day = X - 1, hour = 8 - 15
|
||||
16:00 - 23:59 => day = X, hour = 0 - 7
|
||||
|
||||
http://www.rcgroups.com/forums/showpost.php?p=28782603&postcount=218
|
||||
Here is the SBAS config from the Naza GPS
|
||||
CFG-SBAS - 06 16 08 00 01 03 03 00 51 62 06 00
|
||||
If I read it correctly EGNOS (PRN 124/124/126), MSAS (PRN 129/137) and WAAS (PRN 133/134/135/138) are enabled.
|
||||
*/
|
||||
|
||||
// DJI GPS packet
|
||||
struct DJI_GPS { // byte offset from beginning of packet, subtract 5 for struct offset
|
||||
struct { // YYYYYYYMMMMDDDDDHHHHMMMMMMSSSSSS
|
||||
uint32_t sec:6;
|
||||
uint32_t min:6;
|
||||
uint32_t hour:4;
|
||||
uint32_t day:5;
|
||||
uint32_t month:4;
|
||||
uint32_t year:7;
|
||||
}; // BYTE 5-8 (DT): date and time, see details above
|
||||
int32_t lon; // BYTE 9-12 (LO): longitude (x10^7, degree decimal)
|
||||
int32_t lat; // BYTE 13-16 (LA): latitude (x10^7, degree decimal)
|
||||
int32_t hMSL; // BYTE 17-20 (AL): altitude (in millimeters) (is this MSL or geoid?)
|
||||
uint32_t hAcc; // BYTE 21-24 (HA): horizontal accuracy estimate (see uBlox NAV-POSLLH message for details)
|
||||
uint32_t vAcc; // BYTE 25-28 (VA): vertical accuracy estimate (see uBlox NAV-POSLLH message for details)
|
||||
uint32_t unused1; // BYTE 29-32: ??? (seems to be always 0)
|
||||
int32_t velN; // BYTE 33-36 (NV): NED north velocity (see uBlox NAV-VELNED message for details)
|
||||
int32_t velE; // BYTE 37-40 (EV): NED east velocity (see uBlox NAV-VELNED message for details)
|
||||
int32_t velD; // BYTE 41-44 (DV): NED down velocity (see uBlox NAV-VELNED message for details)
|
||||
uint16_t pDOP; // BYTE 45-46 (PD): position DOP (see uBlox NAV-DOP message for details)
|
||||
uint16_t vDOP; // BYTE 47-48 (VD): vertical DOP (see uBlox NAV-DOP message for details)
|
||||
uint16_t nDOP; // BYTE 49-50 (ND): northing DOP (see uBlox NAV-DOP message for details)
|
||||
uint16_t eDOP; // BYTE 51-52 (ED): easting DOP (see uBlox NAV-DOP message for details)
|
||||
uint8_t numSV; // BYTE 53 (NS): number of satellites (not XORed)
|
||||
uint8_t unused2; // BYTE 54: ??? (not XORed, seems to be always 0)
|
||||
uint8_t fixType; // BYTE 55 (FT): fix type (0 - no lock, 2 - 2D lock, 3 - 3D lock, not sure if other values can be expected
|
||||
// see uBlox NAV-SOL message for details)
|
||||
uint8_t unused3; // BYTE 56: ??? (seems to be always 0)
|
||||
uint8_t flags; // BYTE 57 (SF): fix status flags (see uBlox NAV-SOL message for details)
|
||||
uint16_t unused4; // BYTE 58-59: ??? (seems to be always 0)
|
||||
uint8_t unused5; // BYTE 60 (XM): not sure yet, but I use it as the XOR mask
|
||||
uint16_t seqNo; // BYTE 61-62 (SN): sequence number (not XORed), once there is a lock
|
||||
// increases with every message. When the lock is lost later LSB and MSB are swapped with every message.
|
||||
} __attribute__((packed));
|
||||
|
||||
#define FLAGS_GPSFIX_OK (1 << 0)
|
||||
#define FLAGS_DIFFSOLN (1 << 1)
|
||||
#define FLAGS_WKNSET (1 << 2)
|
||||
#define FLAGS_TOWSET (1 << 3)
|
||||
|
||||
#define FIXTYPE_NO_FIX 0
|
||||
#define FIXTYPE_DEAD_RECKON 0x01 // Dead Reckoning only
|
||||
#define FIXTYPE_2D 0x02 // 2D-Fix
|
||||
#define FIXTYPE_3D 0x03 // 3D-Fix
|
||||
#define FIXTYPE_GNSS_DEAD_RECKON 0x04 // GNSS + dead reckoning combined
|
||||
#define FIXTYPE_TIME_ONLY 0x05 // Time only fix
|
||||
|
||||
|
||||
/*
|
||||
mag protocol info from
|
||||
http://www.rcgroups.com/forums/showpost.php?p=26248426&postcount=62
|
||||
|
||||
The 0x20 message contains compass data. Here is the structure of the message,
|
||||
fields marked with XX I'm not sure about yet. The others will be described below.
|
||||
|
||||
55 AA 20 06 CX CX CY CY CZ CZ CS CS
|
||||
|
||||
Values in the message are stored in little endian.
|
||||
|
||||
HEADER
|
||||
-------------
|
||||
BYTE 1-2: message header - always 55 AA
|
||||
BYTE 3: message id (0x20 for compass message)
|
||||
BYTE 4: length of the payload (0x06 or 6 decimal for 0x20 message)
|
||||
|
||||
PAYLOAD
|
||||
--------------
|
||||
BYTE 5-6 (CX): compass X axis data (signed) - see comments below
|
||||
BYTE 7-8 (CY): compass Y axis data (signed) - see comments below
|
||||
BYTE 9-10 (CZ): compass Z axis data (signed) - see comments below
|
||||
|
||||
CHECKSUM
|
||||
-----------------
|
||||
BYTE 11-12 (CS): checksum, calculated the same way as for uBlox binary messages
|
||||
|
||||
All the bytes of the payload except 9th are XORed with a mask.
|
||||
Mask is calculated based on the value of the 9th byte.
|
||||
|
||||
If we index bits from LSB to MSB as 0-7 we have:
|
||||
mask[0] = 9thByte[0] xor 9thByte[4]
|
||||
mask[1] = 9thByte[1] xor 9thByte[5]
|
||||
mask[2] = 9thByte[2] xor 9thByte[6]
|
||||
mask[3] = 9thByte[3] xor 9thByte[7] xor 9thByte[0];
|
||||
mask[4] = 9thByte[1];
|
||||
mask[5] = 9thByte[2];
|
||||
mask[6] = 9thByte[3];
|
||||
mask[7] = 9thByte[4] xor 9thByte[0];
|
||||
|
||||
To calculate the heading (not tilt compensated) you need to do atan2 on the resulting
|
||||
y any a (y and x?) values, convert radians to degrees and add 360 if the result is negative.
|
||||
*/
|
||||
|
||||
struct DJI_MAG { // byte offset from beginning of packet, subtract 5 for struct offset
|
||||
int16_t x; // BYTE 5-6 (CX): compass X axis data (signed) - see comments below
|
||||
int16_t y; // BYTE 7-8 (CY): compass Y axis data (signed) - see comments below
|
||||
int16_t z; // BYTE 9-10 (CZ): compass Z axis data (signed) - see comments below
|
||||
} __attribute__((packed));
|
||||
|
||||
|
||||
/*
|
||||
version info from
|
||||
http://www.rcgroups.com/forums/showpost.php?p=27058649&postcount=120
|
||||
|
||||
This is still to be confirmed but I believe the 0x30 message carries the GPS module hardware id and firmware version.
|
||||
|
||||
55 AA 30 0C XX XX XX XX FW FW FW FW HW HW HW HW CS CS
|
||||
|
||||
Note that you need to read version numbers backwards (02 01 00 06 means v6.0.1.2)
|
||||
|
||||
HEADER
|
||||
-------------
|
||||
BYTE 1-2: message header - always 55 AA
|
||||
BYTE 3: message id (0x30 for GPS module versions message)
|
||||
BYTE 4: length of the payload (0x0C or 12 decimal for 0x30 message)
|
||||
|
||||
PAYLOAD
|
||||
--------------
|
||||
BYTE 5-8" ??? (seems to be always 0)
|
||||
BYTE 9-12 (FW): firmware version
|
||||
BYTE 13-16 (HW): hardware id
|
||||
|
||||
CHECKSUM
|
||||
-----------------
|
||||
BYTE 17-18 (CS): checksum, calculated the same way as for uBlox binary messages
|
||||
*/
|
||||
|
||||
struct DJI_VER { // byte offset from beginning of packet, subtract 5 for struct offset
|
||||
uint32_t unused1; // BYTE 5-8" ??? (seems to be always 0)
|
||||
uint32_t swVersion; // BYTE 9-12 (FW): firmware version
|
||||
uint32_t hwVersion; // BYTE 13-16 (HW): hardware id
|
||||
} __attribute__((packed));
|
||||
|
||||
|
||||
|
||||
typedef union {
|
||||
uint8_t payload[0];
|
||||
// Nav Class
|
||||
struct DJI_GPS gps;
|
||||
struct DJI_MAG mag;
|
||||
struct DJI_VER ver;
|
||||
} DJIPayload;
|
||||
|
||||
struct DJIHeader {
|
||||
uint8_t id;
|
||||
uint8_t len;
|
||||
uint8_t ck_a; // these are not part of the dji header, they are actually in the trailer
|
||||
uint8_t ck_b; // but they are kept here for parsing ease
|
||||
} __attribute__((packed));
|
||||
|
||||
struct DJIPacket {
|
||||
struct DJIHeader header;
|
||||
DJIPayload payload;
|
||||
} __attribute__((packed));
|
||||
|
||||
extern GPSPositionSensorSensorTypeOptions sensorType;
|
||||
|
||||
bool checksum_dji_message(struct DJIPacket *);
|
||||
uint32_t parse_dji_message(struct DJIPacket *, GPSPositionSensorData *);
|
||||
|
||||
int parse_dji_stream(uint8_t *rx, uint16_t len, char *, GPSPositionSensorData *, struct GPS_RX_STATS *);
|
||||
void dji_load_mag_settings();
|
||||
|
||||
#endif /* DJI_H */
|
||||
@ -7,7 +7,8 @@
|
||||
* @{
|
||||
*
|
||||
* @file UBX.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2016.
|
||||
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief GPS module, handles GPS and NMEA stream
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
@ -621,5 +622,6 @@ uint32_t parse_ubx_message(struct UBXPacket *, GPSPositionSensorData *);
|
||||
|
||||
int parse_ubx_stream(uint8_t *rx, uint16_t len, char *, GPSPositionSensorData *, struct GPS_RX_STATS *);
|
||||
void op_gpsv9_load_mag_settings();
|
||||
void aux_hmc5x83_load_mag_settings();
|
||||
|
||||
#endif /* UBX_H */
|
||||
|
||||
@ -7,7 +7,8 @@
|
||||
* @{
|
||||
*
|
||||
* @file ubx_autoconfig.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2016.
|
||||
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
|
||||
* @brief Support code for UBX AutoConfig
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
@ -849,6 +850,8 @@ void gps_ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send)
|
||||
}
|
||||
|
||||
|
||||
// (re)init the autoconfig stuff so that it will run if called
|
||||
//
|
||||
// this can be called from a different thread
|
||||
// so everything it touches must be declared volatile
|
||||
void gps_ubx_autoconfig_set(ubx_autoconfig_settings_t *config)
|
||||
@ -883,17 +886,17 @@ void gps_ubx_autoconfig_set(ubx_autoconfig_settings_t *config)
|
||||
status->currentStep = new_step;
|
||||
status->currentStepSave = new_step;
|
||||
|
||||
// this forces the sensor type detection to occur outside the FSM
|
||||
// and _can_ also engage the autobaud detection that is outside the FSM
|
||||
// don't do it if FSM is enabled as FSM can change the baud itself
|
||||
// (don't do it because the baud rates are already in sync)
|
||||
gps_ubx_reset_sensor_type();
|
||||
|
||||
if (status->currentSettings.UbxAutoConfig >= GPSSETTINGS_UBXAUTOCONFIG_AUTOBAUDANDCONFIGURE) {
|
||||
// enabled refers to autoconfigure
|
||||
// note that sensor type (gps type) detection happens even if completely disabled
|
||||
// also note that AutoBaud is less than Configure
|
||||
// also note that AutoBaud is less than AutoBaudAndConfigure
|
||||
enabled = true;
|
||||
} else {
|
||||
// this forces the sensor type detection to occur outside the FSM
|
||||
// and _can_ also engage the autobaud detection that is outside the FSM
|
||||
// don't do it if FSM is enabled as FSM can change the baud itself
|
||||
// (don't do it because the baud rates are already in sync)
|
||||
gps_ubx_reset_sensor_type();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -7,7 +7,8 @@
|
||||
* @{
|
||||
*
|
||||
* @file sensors.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2016.
|
||||
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
|
||||
* @brief Module to handle fetch and preprocessing of sensor data
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
@ -62,6 +63,7 @@
|
||||
#include <auxmagsupport.h>
|
||||
#include <accelgyrosettings.h>
|
||||
#include <revosettings.h>
|
||||
#include <UBX.h>
|
||||
|
||||
#include <mathmisc.h>
|
||||
#include <taskinfo.h>
|
||||
@ -128,6 +130,10 @@ PERF_DEFINE_COUNTER(counterBaroPeriod);
|
||||
PERF_DEFINE_COUNTER(counterSensorPeriod);
|
||||
PERF_DEFINE_COUNTER(counterSensorResets);
|
||||
|
||||
#if defined(PIOS_INCLUDE_HMC5X83)
|
||||
void aux_hmc5x83_load_settings();
|
||||
#endif
|
||||
|
||||
// Private functions
|
||||
static void SensorsTask(void *parameters);
|
||||
static void settingsUpdatedCb(UAVObjEvent *objEv);
|
||||
@ -141,7 +147,9 @@ static void clearContext(sensor_fetch_context *sensor_context);
|
||||
static void handleAccel(float *samples, float temperature);
|
||||
static void handleGyro(float *samples, float temperature);
|
||||
static void handleMag(float *samples, float temperature);
|
||||
#if defined(PIOS_INCLUDE_HMC5X83)
|
||||
static void handleAuxMag(float *samples);
|
||||
#endif
|
||||
static void handleBaro(float sample, float temperature);
|
||||
|
||||
static void updateAccelTempBias(float temperature);
|
||||
@ -184,6 +192,12 @@ static float baro_temp_bias = 0;
|
||||
static float baro_temperature = NAN;
|
||||
static uint8_t baro_temp_calibration_count = 0;
|
||||
|
||||
#if defined(PIOS_INCLUDE_HMC5X83)
|
||||
// Allow AuxMag to be disabled without reboot
|
||||
// because the other mags are that way
|
||||
static bool useMag = false;
|
||||
#endif
|
||||
|
||||
/**
|
||||
* Initialise the module. Called before the start function
|
||||
* \returns 0 on success or -1 if initialisation failed
|
||||
@ -200,9 +214,11 @@ int32_t SensorsInitialize(void)
|
||||
AttitudeSettingsInitialize();
|
||||
AccelGyroSettingsInitialize();
|
||||
|
||||
#if defined(PIOS_INCLUDE_HMC5X83)
|
||||
// for auxmagsupport.c helpers
|
||||
AuxMagSettingsInitialize();
|
||||
AuxMagSensorInitialize();
|
||||
#endif
|
||||
|
||||
RevoSettingsConnectCallback(&settingsUpdatedCb);
|
||||
RevoCalibrationConnectCallback(&settingsUpdatedCb);
|
||||
@ -366,9 +382,12 @@ static void processSamples3d(sensor_fetch_context *sensor_context, const PIOS_SE
|
||||
|
||||
PIOS_SENSORS_GetScales(sensor, scales, MAX_SENSORS_PER_INSTANCE);
|
||||
float inv_count = 1.0f / (float)sensor_context->count;
|
||||
if ((sensor->type & PIOS_SENSORS_TYPE_3AXIS_ACCEL) ||
|
||||
(sensor->type == PIOS_SENSORS_TYPE_3AXIS_MAG) ||
|
||||
(sensor->type == PIOS_SENSORS_TYPE_3AXIS_AUXMAG)) {
|
||||
if ((sensor->type & PIOS_SENSORS_TYPE_3AXIS_ACCEL)
|
||||
|| (sensor->type == PIOS_SENSORS_TYPE_3AXIS_MAG)
|
||||
#if defined(PIOS_INCLUDE_HMC5X83)
|
||||
|| (sensor->type == PIOS_SENSORS_TYPE_3AXIS_AUXMAG)
|
||||
#endif
|
||||
) {
|
||||
float t = inv_count * scales[0];
|
||||
samples[0] = ((float)sensor_context->accum[0].x * t);
|
||||
samples[1] = ((float)sensor_context->accum[0].y * t);
|
||||
@ -379,12 +398,12 @@ static void processSamples3d(sensor_fetch_context *sensor_context, const PIOS_SE
|
||||
handleMag(samples, temperature);
|
||||
PERF_MEASURE_PERIOD(counterMagPeriod);
|
||||
return;
|
||||
|
||||
#if defined(PIOS_INCLUDE_HMC5X83)
|
||||
case PIOS_SENSORS_TYPE_3AXIS_AUXMAG:
|
||||
handleAuxMag(samples);
|
||||
PERF_MEASURE_PERIOD(counterMagPeriod);
|
||||
return;
|
||||
|
||||
#endif
|
||||
default:
|
||||
PERF_TRACK_VALUE(counterAccelSamples, sensor_context->count);
|
||||
PERF_MEASURE_PERIOD(counterAccelPeriod);
|
||||
@ -473,10 +492,14 @@ static void handleMag(float *samples, float temperature)
|
||||
MagSensorSet(&mag);
|
||||
}
|
||||
|
||||
#if defined(PIOS_INCLUDE_HMC5X83)
|
||||
static void handleAuxMag(float *samples)
|
||||
{
|
||||
auxmagsupport_publish_samples(samples, AUXMAGSENSOR_STATUS_OK);
|
||||
if (useMag) {
|
||||
auxmagsupport_publish_samples(samples, AUXMAGSENSOR_STATUS_OK);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
static void handleBaro(float sample, float temperature)
|
||||
{
|
||||
@ -607,6 +630,18 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
|
||||
fabsf(baroCorrection.c) > 1e-9f ||
|
||||
fabsf(baroCorrection.d) > 1e-9f));
|
||||
}
|
||||
|
||||
#if defined(PIOS_INCLUDE_HMC5X83)
|
||||
void aux_hmc5x83_load_mag_settings() {
|
||||
uint8_t magType = auxmagsupport_get_type();
|
||||
|
||||
if (magType == AUXMAGSETTINGS_TYPE_I2C || magType == AUXMAGSETTINGS_TYPE_FLEXI) {
|
||||
useMag = true;
|
||||
} else {
|
||||
useMag = false;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
|
||||
@ -148,6 +148,7 @@
|
||||
#define PIOS_GPS_MINIMAL
|
||||
/* #define PIOS_INCLUDE_GPS_NMEA_PARSER */
|
||||
#define PIOS_INCLUDE_GPS_UBX_PARSER
|
||||
/* #define PIOS_INCLUDE_GPS_DJI_PARSER */
|
||||
/* #define PIOS_GPS_SETS_HOMELOCATION */
|
||||
|
||||
/* Stabilization options */
|
||||
|
||||
@ -1,5 +1,5 @@
|
||||
#
|
||||
# Copyright (c) 2015, The LibrePilot Project, http://www.librepilot.org
|
||||
# Copyright (c) 2015-2016, The LibrePilot Project, http://www.librepilot.org
|
||||
# Copyright (c) 2009-2013, The OpenPilot Team, http://www.openpilot.org
|
||||
# Copyright (c) 2012, PhoenixPilot, http://github.com/PhoenixPilot
|
||||
#
|
||||
@ -32,7 +32,7 @@ USE_CXX = YES
|
||||
USE_DSP_LIB ?= NO
|
||||
|
||||
# List of mandatory modules to include
|
||||
#MODULES += Sensors
|
||||
MODULES += Sensors
|
||||
#MODULES += Attitude/revolution
|
||||
#MODULES += StateEstimation # use instead of Attitude
|
||||
#MODULES += Altitude/revolution
|
||||
|
||||
@ -5,7 +5,7 @@
|
||||
* @addtogroup OpenPilotCore OpenPilot Core
|
||||
* @{
|
||||
* @file pios_config.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2013.
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2016.
|
||||
* @brief PiOS configuration header, the compile time config file for the PIOS.
|
||||
* Defines which PiOS libraries and features are included in the firmware.
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
@ -149,6 +149,7 @@
|
||||
/* #define PIOS_GPS_MINIMAL */
|
||||
#define PIOS_INCLUDE_GPS_NMEA_PARSER
|
||||
#define PIOS_INCLUDE_GPS_UBX_PARSER
|
||||
#define PIOS_INCLUDE_GPS_DJI_PARSER
|
||||
#define PIOS_GPS_SETS_HOMELOCATION
|
||||
|
||||
/* Stabilization options */
|
||||
|
||||
@ -141,6 +141,7 @@
|
||||
/* #define PIOS_GPS_MINIMAL */
|
||||
#define PIOS_INCLUDE_GPS_NMEA_PARSER
|
||||
#define PIOS_INCLUDE_GPS_UBX_PARSER
|
||||
#define PIOS_INCLUDE_GPS_DJI_PARSER
|
||||
#define PIOS_GPS_SETS_HOMELOCATION
|
||||
|
||||
/* Stabilization options */
|
||||
|
||||
@ -5,7 +5,7 @@
|
||||
* @addtogroup OpenPilotCore OpenPilot Core
|
||||
* @{
|
||||
* @file pios_config.h
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015.
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2016.
|
||||
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2013.
|
||||
* @brief PiOS configuration header, the compile time config file for the PIOS.
|
||||
* Defines which PiOS libraries and features are included in the firmware.
|
||||
@ -153,6 +153,7 @@
|
||||
/* #define PIOS_GPS_MINIMAL */
|
||||
#define PIOS_INCLUDE_GPS_NMEA_PARSER
|
||||
#define PIOS_INCLUDE_GPS_UBX_PARSER
|
||||
#define PIOS_INCLUDE_GPS_DJI_PARSER
|
||||
#define PIOS_GPS_SETS_HOMELOCATION
|
||||
|
||||
/* Stabilization options */
|
||||
|
||||
@ -5,7 +5,7 @@
|
||||
* @addtogroup OpenPilotCore OpenPilot Core
|
||||
* @{
|
||||
* @file pios_config.h
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015.
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2016.
|
||||
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2013.
|
||||
* @brief PiOS configuration header, the compile time config file for the PIOS.
|
||||
* Defines which PiOS libraries and features are included in the firmware.
|
||||
@ -157,6 +157,7 @@
|
||||
/* #define PIOS_GPS_MINIMAL */
|
||||
#define PIOS_INCLUDE_GPS_NMEA_PARSER
|
||||
#define PIOS_INCLUDE_GPS_UBX_PARSER
|
||||
#define PIOS_INCLUDE_GPS_DJI_PARSER
|
||||
#define PIOS_GPS_SETS_HOMELOCATION
|
||||
|
||||
/* Stabilization options */
|
||||
|
||||
@ -5,7 +5,8 @@
|
||||
* @addtogroup OpenPilotCore OpenPilot Core
|
||||
* @{
|
||||
* @file pios_config.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2013.
|
||||
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015-2016.
|
||||
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2013.
|
||||
* @brief PiOS configuration header, the compile time config file for the PIOS.
|
||||
* Defines which PiOS libraries and features are included in the firmware.
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
@ -141,6 +142,7 @@
|
||||
/* #define PIOS_GPS_MINIMAL */
|
||||
#define PIOS_INCLUDE_GPS_NMEA_PARSER
|
||||
#define PIOS_INCLUDE_GPS_UBX_PARSER
|
||||
#define PIOS_INCLUDE_GPS_DJI_PARSER
|
||||
#define PIOS_GPS_SETS_HOMELOCATION
|
||||
|
||||
/* Stabilization options */
|
||||
|
||||
@ -6,7 +6,7 @@
|
||||
defaultvalue="1,0,0,0,1,0,0,0,1"/>
|
||||
<field name="MagBiasNullingRate" units="" type="float" elements="1" defaultvalue="0"/>
|
||||
<field name="BoardRotation" units="deg" type="int16" elementnames="Roll,Pitch,Yaw" defaultvalue="0,0,0"/>
|
||||
<field name="Type" units="" type="enum" elements="1" options="GPSV9,Ext,Flexi" defaultvalue="GPSV9"/>
|
||||
<field name="Type" units="" type="enum" elements="1" options="GPSV9,Flexi,I2C,DJI" defaultvalue="GPSV9"/>
|
||||
<field name="Usage" units="" type="enum" elements="1" options="Both,OnboardOnly,AuxOnly" defaultvalue="Both"/>
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
|
||||
|
||||
@ -12,7 +12,7 @@
|
||||
<field name="PDOP" units="" type="float" elements="1"/>
|
||||
<field name="HDOP" units="" type="float" elements="1"/>
|
||||
<field name="VDOP" units="" type="float" elements="1"/>
|
||||
<field name="SensorType" units="" type="enum" elements="1" options="Unknown,NMEA,UBX,UBX7,UBX8" defaultvalue="Unknown" />
|
||||
<field name="SensorType" units="" type="enum" elements="1" options="Unknown,NMEA,UBX,UBX7,UBX8,DJI" defaultvalue="Unknown" />
|
||||
<field name="AutoConfigStatus" units="" type="enum" elements="1" options="Disabled,Running,Done,Error" defaultvalue="Disabled" />
|
||||
<field name="BaudRate" units="" type="enum" elements="1" options="2400,4800,9600,19200,38400,57600,115200,230400,Unknown" defaultvalue="Unknown" />
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
|
||||
@ -1,7 +1,7 @@
|
||||
<xml>
|
||||
<object name="GPSSettings" singleinstance="true" settings="true" category="Sensors">
|
||||
<description>GPS Module specific settings</description>
|
||||
<field name="DataProtocol" units="" type="enum" elements="1" options="NMEA,UBX" defaultvalue="UBX"/>
|
||||
<field name="DataProtocol" units="" type="enum" elements="1" options="NMEA,UBX,DJI" defaultvalue="UBX"/>
|
||||
<field name="MinSatellites" units="" type="uint8" elements="1" defaultvalue="7"/>
|
||||
<field name="MaxPDOP" units="" type="float" elements="1" defaultvalue="3.5"/>
|
||||
<!-- Ubx self configuration. Enable to allow the flight board to auto configure ubx GPS options,
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user