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

OP-1477 - Split and cleanup gpssys module

Browse Source
This commit is contained in:
Alessio Morale 2014-09-09 01:26:13 +02:00
parent cac463e623
commit 2f50dbeee5
8 changed files with 310 additions and 162 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

96
flight/modules/gpsp/gps9gpshandler.c Normal file
View File

@ -0,0 +1,96 @@
/**
******************************************************************************
*
* @file gps9gpshandler.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief handler for GPSV9 onboard ubx gps module.
* --
* @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_struct_helper.h>
#include <pios_helpers.h>
#include <ubx_utils.h>
#include <pios_ubx_ddc.h>
#include "gps9gpshandler.h"
#include "gps9protocol.h"
uint32_t lastUnsentData = 0;
uint8_t buffer[BUFFER_SIZE];
// cfg-prt I2C. In UBX+RTCM, Out UBX, Slave Addr 0x42
const char cfg_settings[] = "\xB5\x62\x06\x00\x14\x00\x00\x00\x00\x00\x84\x00\x00\x00\x00\x00\x00\x00\x07\x00\x01\x00\x00\x00\x00\x00\xA6\xC6";
void handleGPS()
{
bool completeSentenceSent = false;
int8_t maxCount = 3;
do {
int32_t datacounter = PIOS_UBX_DDC_GetAvailableBytes(PIOS_I2C_GPS);
if (datacounter > 0) {
uint8_t toRead = (uint32_t)datacounter > BUFFER_SIZE - lastUnsentData ? BUFFER_SIZE - lastUnsentData : (uint8_t)datacounter;
uint8_t toSend = toRead;
PIOS_UBX_DDC_ReadData(PIOS_I2C_GPS, buffer, toRead);
uint8_t *lastSentence;
static uint16_t lastSentenceLenght;
completeSentenceSent = ubx_getLastSentence(buffer, toRead, &lastSentence, &lastSentenceLenght);
if (completeSentenceSent) {
toSend = (uint8_t)(lastSentence - buffer + lastSentenceLenght);
} else {
lastUnsentData = 0;
}
PIOS_COM_SendBuffer(pios_com_main_id, buffer, toSend);
if (toRead > toSend) {
// move unsent data at the beginning of buffer to be sent next time
lastUnsentData = toRead - toSend;
memcpy(buffer, (buffer + toSend), lastUnsentData);
}
}
datacounter = PIOS_COM_ReceiveBuffer(pios_com_main_id, buffer, BUFFER_SIZE, 0);
if (datacounter > 0) {
PIOS_UBX_DDC_WriteData(PIOS_I2C_GPS, buffer, datacounter);
}
} while (maxCount--);
}
typedef struct {
uint8_t size;
const uint8_t *sentence;
} ubx_init_sentence;
const ubx_init_sentence gps_config[] = {
[0] = {
.sentence = (uint8_t *)cfg_settings,
.size = sizeof(cfg_settings),
},
};
void setupGPS()
{
for (uint8_t i = 0; i < NELEMENTS(gps_config); i++) {
PIOS_UBX_DDC_WriteData(PIOS_I2C_GPS, gps_config[i].sentence, gps_config[i].size);
}
}

53
flight/modules/gpsp/gps9maghandler.c Normal file
View File

@ -0,0 +1,53 @@
/**
******************************************************************************
*
* @file gps9maghandler.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief handles GPSV9 onboard magnetometer and sends its data.
* --
* @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_struct_helper.h>
#include <pios_helpers.h>
#include <ubx_utils.h>
#include <pios_hmc5x83.h>
#include "inc/gps9protocol.h"
extern pios_hmc5x83_dev_t onboard_mag;
void handleMag()
{
if (!PIOS_HMC5x83_NewDataAvailable(onboard_mag)) {
return;
}
static int16_t mag[3];
if (PIOS_HMC5x83_ReadMag(onboard_mag, mag) == 0) {
MagUbxPkt magPkt;
// swap axis so that if side with connector is aligned to revo side with connectors, mags data are aligned
magPkt.fragments.data.X = -mag[1];
magPkt.fragments.data.Y = mag[0];
magPkt.fragments.data.Z = mag[2];
magPkt.fragments.data.status = 1;
ubx_buildPacket(&magPkt.packet, UBX_OP_CUST_CLASS, UBX_OP_MAG, sizeof(MagData));
PIOS_COM_SendBuffer(pios_com_main_id, magPkt.packet.bynarystream, sizeof(MagUbxPkt));
return;
}
}

160
flight/modules/gpsp/gpsdsysmod.c
View File

@ -8,15 +8,15 @@
* (currently only STM32 supported)
*
* @{
* @addtogroup SystemModule System Module
* @addtogroup SystemModule GPSV9 System Module
* @brief Initializes PIOS and other modules runs monitoring
* After initializing all the modules (currently selected by Makefile but in
* future controlled by configuration on SD card) runs basic monitoring and
* alarms.
* @{
*
* @file systemmod.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @file gpsdsystemmod.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief System module
*
* @see The GNU Public License (GPL) Version 3
@ -38,15 +38,12 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include <openpilot.h>
#include <pios_struct_helper.h>
#include <pios_helpers.h>
// private includes
#include "inc/gpsdsysmod.h"
#include <pios_hmc5x83.h>
#include <pios_ubx_ddc.h>
#include <ubx_utils.h>
#include "inc/gps9maghandler.h"
#include "inc/gps9gpshandler.h"
#include "inc/gps9protocol.h"
// UAVOs
#include <systemstats.h>
SystemStatsData systemStats;
@ -57,7 +54,6 @@ extern uint32_t pios_com_main_id;
extern struct pios_flash_driver pios_jedec_flash_driver;
extern pios_hmc5x83_dev_t onboard_mag;
extern uintptr_t flash_id;
// Private constants
@ -65,74 +61,22 @@ extern uintptr_t flash_id;
#define STACK_SIZE_BYTES 450
#define STAT_RATE 1
#define TASK_PRIORITY (tskIDLE_PRIORITY + 1)
#define BUFFER_SIZE 200
const char cfg_settings[] = {};
// cfg-prt I2C. In UBX+RTCM, Out UBX, Slave Addr 0x42
// "\xB5\x62\x06\x00\x14\x00\x00\x00\x00\x00\x84\x00\x00\x00\x00\x00\x00\x00\x07\x00\x01\x00\x00\x00\x00\x00\xA6\xC6"
// cfg-msg: nav-pvt rate 1
// "\xB5\x62\x06\x01\x03\x00\x01\x07\x01\x13\x51"
// cfg-msg: nav-svinfo rate 10
// "\xB5\x62\x06\x01\x03\x00\x01\x30\x0A\x45\xAC"
// CFG-RATE meas period 100ms, nav rate 1
// "\xB5\x62\x06\x08\x06\x00\x64\x00\x01\x00\x01\x00\x7A\x12";
#define TASK_PRIORITY (tskIDLE_PRIORITY + 2)
// Private types
// Private variables
uint8_t buffer[BUFFER_SIZE];
static xTaskHandle systemTaskHandle;
static enum { STACKOVERFLOW_NONE = 0, STACKOVERFLOW_WARNING = 1, STACKOVERFLOW_CRITICAL = 3 } stackOverflow;
// Private functions
static bool mallocFailed;
static void ReadGPS();
static void ReadMag();
static void SetupGPS();
static void updateStats();
static void gpspSystemTask(void *parameters);
typedef struct {
int16_t X;
int16_t Y;
int16_t Z;
uint16_t status;
} __attribute__((packed)) MagData;
typedef union {
struct {
UBXHeader_t header;
MagData data;
UBXFooter_t footer;
} __attribute__((packed)) fragments;
UBXPacket_t packet;
} MagUbxPkt;
typedef struct {
uint32_t flightTime;
uint16_t HeapRemaining;
uint16_t IRQStackRemaining;
uint16_t SystemModStackRemaining;
uint16_t options;
} __attribute__((packed)) SysData;
typedef union {
struct {
UBXHeader_t header;
SysData data;
UBXFooter_t footer;
} fragments;
UBXPacket_t packet;
} SysUbxPkt;
#define SYS_DATA_OPTIONS_FLASH 0x01
#define UBX_OP_CUST_CLASS 0x99
#define UBX_OP_SYS 0x01
#define UBX_OP_MAG 0x02
/**
* Create the module task.
@ -181,8 +125,9 @@ static void gpspSystemTask(__attribute__((unused)) void *parameters)
/* Record a successful boot */
PIOS_IAP_WriteBootCount(0);
#endif
PIOS_COM_ChangeBaud(pios_com_main_id, 115200);
static TickType_t lastUpdate;
SetupGPS();
setupGPS();
uint8_t counter = 0;
while (1) {
// NotificationUpdateStatus();
@ -192,8 +137,8 @@ static void gpspSystemTask(__attribute__((unused)) void *parameters)
}
vTaskDelayUntil(&lastUpdate, SYSTEM_UPDATE_PERIOD_MS * configTICK_RATE_HZ / 1000);
ReadGPS();
ReadMag();
handleGPS();
handleMag();
updateStats();
}
}
@ -299,85 +244,6 @@ void vApplicationMallocFailedHook(void)
#endif
}
void ReadMag()
{
if (!PIOS_HMC5x83_NewDataAvailable(onboard_mag)) {
return;
}
static int16_t mag[3];
if (PIOS_HMC5x83_ReadMag(onboard_mag, mag) == 0) {
MagUbxPkt magPkt;
// swap axis so that if side with connector is aligned to revo side with connectors, mags data are aligned
magPkt.fragments.data.X = -mag[1];
magPkt.fragments.data.Y = mag[0];
magPkt.fragments.data.Z = mag[2];
magPkt.fragments.data.status = 1;
ubx_buildPacket(&magPkt.packet, UBX_OP_CUST_CLASS, UBX_OP_MAG, sizeof(MagData));
PIOS_COM_SendBuffer(pios_com_main_id, magPkt.packet.bynarystream, sizeof(MagUbxPkt));
return;
}
}
uint32_t lastUnsentData = 0;
void ReadGPS()
{
bool completeSentenceSent = false;
int8_t maxCount = 3;
do {
int32_t datacounter = PIOS_UBX_DDC_GetAvailableBytes(PIOS_I2C_GPS);
if (datacounter > 0) {
uint8_t toRead = (uint32_t)datacounter > BUFFER_SIZE - lastUnsentData ? BUFFER_SIZE - lastUnsentData : (uint8_t)datacounter;
uint8_t toSend = toRead;
PIOS_UBX_DDC_ReadData(PIOS_I2C_GPS, buffer, toRead);
uint8_t *lastSentence;
static uint16_t lastSentenceLenght;
completeSentenceSent = ubx_getLastSentence(buffer, toRead, &lastSentence, &lastSentenceLenght);
if (completeSentenceSent) {
toSend = (uint8_t)(lastSentence - buffer + lastSentenceLenght);
} else {
lastUnsentData = 0;
}
PIOS_COM_SendBuffer(pios_com_main_id, buffer, toSend);
if (toRead > toSend) {
// move unsent data at the beginning of buffer to be sent next time
lastUnsentData = toRead - toSend;
memcpy(buffer, (buffer + toSend), lastUnsentData);
}
}
datacounter = PIOS_COM_ReceiveBuffer(pios_com_main_id, buffer, BUFFER_SIZE, 0);
if (datacounter > 0) {
PIOS_UBX_DDC_WriteData(PIOS_I2C_GPS, buffer, datacounter);
}
} while (maxCount--);
}
typedef struct {
uint8_t size;
const uint8_t *sentence;
} ubx_init_sentence;
const ubx_init_sentence gps_config[] = {
[0] = {
.sentence = (uint8_t *)cfg_settings,
.size = sizeof(cfg_settings),
},
};
void SetupGPS()
{
PIOS_COM_ChangeBaud(pios_com_main_id, 115200);
for (uint8_t i = 0; i < NELEMENTS(gps_config); i++) {
PIOS_UBX_DDC_WriteData(PIOS_I2C_GPS, gps_config[i].sentence, gps_config[i].size);
}
}
/**
* @}
* @}

34
flight/modules/gpsp/inc/gps9gpshandler.h Normal file
View File

@ -0,0 +1,34 @@
/**
******************************************************************************
*
* @file gps9gpshandler.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief handler for GPSV9 onboard ubx gps module.
* --
* @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 GPS9GPSHANDLER_H_
#define GPS9GPSHANDLER_H_
#define BUFFER_SIZE 200
void handleGPS();
void setupGPS();
#endif /* GPS9GPSHANDLER_H_ */

32
flight/modules/gpsp/inc/gps9maghandler.h Normal file
View File

@ -0,0 +1,32 @@
/**
******************************************************************************
*
* @file gps9maghandler.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief brief goes here.
* --
* @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 GPS9MAGHANDLER_H
#define GPS9MAGHANDLER_H
void handleMag();
#endif

72
flight/modules/gpsp/inc/gps9protocol.h Normal file
View File

@ -0,0 +1,72 @@
/**
******************************************************************************
*
* @file gpsv9protocol.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief brief goes here.
* --
* @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 GPSV9PROTOCOL_H_
#define GPSV9PROTOCOL_H_
#include <openpilot.h>
#include <pios_struct_helper.h>
#include <pios_helpers.h>
#include <ubx_utils.h>
#define UBX_OP_CUST_CLASS 0x99
#define UBX_OP_SYS 0x01
#define UBX_OP_MAG 0x02
typedef struct {
int16_t X;
int16_t Y;
int16_t Z;
uint16_t status;
} __attribute__((packed)) MagData;
typedef union {
struct {
UBXHeader_t header;
MagData data;
UBXFooter_t footer;
} __attribute__((packed)) fragments;
UBXPacket_t packet;
} MagUbxPkt;
typedef struct {
uint32_t flightTime;
uint16_t HeapRemaining;
uint16_t IRQStackRemaining;
uint16_t SystemModStackRemaining;
uint16_t options;
} __attribute__((packed)) SysData;
typedef union {
struct {
UBXHeader_t header;
SysData data;
UBXFooter_t footer;
} fragments;
UBXPacket_t packet;
} SysUbxPkt;
#endif /* GPSV9PROTOCOL_H_ */

14
flight/modules/gpsp/inc/gpsdsysmod.h
View File

@ -2,10 +2,10 @@
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup SystemModule System Module
* @addtogroup SystemModule GPSV9 System Module
* @{
*
* @file systemmod.h
* @file gpsdsysmod.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief System module
*
@ -27,9 +27,13 @@
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef SYSTEMMOD_H
#define SYSTEMMOD_H
#ifndef GPSSYSTEMMOD_H
#define GPSSYSTEMMOD_H
#include <openpilot.h>
#include <pios_struct_helper.h>
#include <pios_helpers.h>
#include <ubx_utils.h>
int32_t GPSPSystemModInitialize(void);
#endif // SYSTEMMOD_H
#endif // GPSSYSTEMMOD_H

11
flight/targets/boards/gpsplatinum/firmware/Makefile
View File

@ -32,29 +32,20 @@ ERASE_FLASH ?= NO
# List of mandatory modules to include
#MODULES += FirmwareIAP
MODULES += gpsp
# Include all camera options
CDEFS += -DPIOS_APPS_MINIMAL
# Erase flash firmware should be buildable from command line
ifeq ($(ERASE_FLASH), YES)
CDEFS += -DERASE_FLASH
endif
# List C source files here (C dependencies are automatically generated).
# Use file-extension c for "c-only"-files
## The standard CMSIS startup
SRC += $(CMSIS_DEVICEDIR)/system_stm32f0xx.c
## Application Core
SRC += $(OPMODULEDIR)/gpsp/gpsdsysmod.c
SRC += $(OPSYSTEM)/gpsp.c
SRC += $(OPSYSTEM)/pios_board.c
SRC += $(FLIGHTLIB)/ubx_utils.c
#SRC += $(FLIGHTLIB)/alarms.c
#SRC += $(OPUAVOBJ)/uavobjectmanager.c
#SRC += $(OPUAVOBJ)/eventdispatcher.c
SRC += $(PIOSCOMMON)/pios_hmc5x83.c
SRC += $(PIOSCOMMON)/pios_ubx_dcc.c