1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-18 03:52:11 +01:00

Added PPM packet to packet handler, created replacement system module for PipXtreme (PipXtreme module), removed all UAVObjects from PipXtreme, and added parsing of incoming (over USB/COM) UAVTalk stream.

This commit is contained in:
Brian Webb 2012-04-07 10:07:46 -07:00
parent 56a2006fd8
commit 0f9f897870
9 changed files with 365 additions and 63 deletions

View File

@ -46,7 +46,7 @@ typedef enum {
PACKET_TYPE_ADJUST_TX_PWR, // used to ask the other modem to adjust it's tx power
PACKET_TYPE_DATA, // data packet (packet contains user data)
PACKET_TYPE_ACKED_DATA, // data packet that requies an ACK
PACKET_TYPE_RECEIVER, // Receiver relay values
PACKET_TYPE_PPM, // PPM relay values
PACKET_TYPE_ACK,
PACKET_TYPE_NACK
} PHPacketType;
@ -67,6 +67,13 @@ typedef struct {
uint8_t data[PH_MAX_DATA + RS_ECC_NPARITY];
} PHPacket, *PHPacketHandle;
#define PH_PPM_DATA_SIZE(p) ((uint8_t*)(p->ecc) - (uint8_t*)(((PHPacketHandle)p)->data))
typedef struct {
PHPacketHeader header;
uint16_t channels[PIOS_RCVR_MAX_CHANNELS];
uint8_t ecc[RS_ECC_NPARITY];
} PHPpmPacket, *PHPpmPacketHandle;
typedef struct {
uint8_t txWinSize;
uint16_t maxConnections;
@ -75,13 +82,15 @@ typedef struct {
typedef int32_t (*PHOutputStream)(PHPacketHandle packet);
typedef void (*PHDataHandler)(uint8_t *data, uint8_t len);
typedef void (*PHPPMHandler)(uint16_t *channels);
typedef void* PHInstHandle;
typedef uint32_t PHInstHandle;
// Public functions
PHInstHandle PHInitialize(PacketHandlerConfig *cfg);
void PHRegisterOutputStream(PHInstHandle h, PHOutputStream f);
void PHRegisterDataHandler(PHInstHandle h, PHDataHandler f);
void PHRegisterPPMHandler(PHInstHandle h, PHPPMHandler f);
uint32_t PHConnect(PHInstHandle h, uint32_t dest_id);
PHPacketHandle PHGetRXPacket(PHInstHandle h);
PHPacketHandle PHGetTXPacket(PHInstHandle h);

View File

@ -46,6 +46,7 @@ typedef struct {
xSemaphoreHandle lock;
PHOutputStream output_stream;
PHDataHandler data_handler;
PHPPMHandler ppm_handler;
} PHPacketData, *PHPacketDataHandle;
// Private functions
@ -112,6 +113,18 @@ void PHRegisterDataHandler(PHInstHandle h, PHDataHandler f)
data->data_handler = f;
}
/**
* Register a PPM packet handler
* \param[in] h The packet handler instance data pointer.
* \param[in] f The PPM handler function
*/
void PHRegisterPPMHandler(PHInstHandle h, PHPPMHandler f)
{
PHPacketDataHandle data = (PHPacketDataHandle)h;
data->ppm_handler = f;
}
/**
* Get a packet out of the transmit buffer.
* \param[in] h The packet handler instance data pointer.
@ -207,7 +220,7 @@ uint8_t PHTransmitPacket(PHInstHandle h, PHPacketHandle p)
case PACKET_TYPE_READY:
case PACKET_TYPE_NOTREADY:
case PACKET_TYPE_DATA:
case PACKET_TYPE_RECEIVER:
case PACKET_TYPE_PPM:
PHReleaseTXPacket(h, p);
break;
}
@ -285,7 +298,13 @@ uint8_t PHReceivePacket(PHInstHandle h, PHPacketHandle p)
}
break;
case PACKET_TYPE_RECEIVER:
case PACKET_TYPE_PPM:
if (!rx_error)
// Pass on the channels to the PPM handler.
if(data->ppm_handler)
data->ppm_handler(((PHPpmPacketHandle)p)->channels);
break;
@ -293,7 +312,7 @@ uint8_t PHReceivePacket(PHInstHandle h, PHPacketHandle p)
if (!rx_error)
// Pass on the data to the receiver handler.
// Pass on the data to the data handler.
if(data->data_handler)
data->data_handler(p->data, p->header.data_size);

View File

@ -0,0 +1,35 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup PipXtremeModule PipXtreme Module
* @{
*
* @file pipxtrememod.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief The PipXtreme system 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 PIPXTREMEMOD_H
#define PIPXTREMEMOD_H
int32_t PipXtremeModInitialize(void);
#endif // PIPXTREMEMOD_H

View File

@ -0,0 +1,238 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @brief The OpenPilot Modules do the majority of the control in OpenPilot. The
* @ref PipXtremeModule The PipXtreme Module is the equivalanet of the System
* Module for the PipXtreme modem. it starts all the other modules.
# This is done through the @ref PIOS "PIOS Hardware abstraction layer",
# which then contains hardware specific implementations
* (currently only STM32 supported)
*
* @{
* @addtogroup PipXtremeModule PipXtreme Module
* @brief Initializes PIOS and other modules runs monitoring
* After initializing all the modules runs basic monitoring and
* alarms.
* @{
*
* @file pipxtrememod.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief System 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 "systemmod.h"
// Private constants
#define SYSTEM_UPDATE_PERIOD_MS 1000
#define LED_BLINK_RATE_HZ 5
#ifndef IDLE_COUNTS_PER_SEC_AT_NO_LOAD
#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 995998 // calibrated by running tests/test_cpuload.c
// must be updated if the FreeRTOS or compiler
// optimisation options are changed.
#endif
#if defined(PIOS_SYSTEM_STACK_SIZE)
#define STACK_SIZE_BYTES PIOS_SYSTEM_STACK_SIZE
#else
#define STACK_SIZE_BYTES 924
#endif
#define TASK_PRIORITY (tskIDLE_PRIORITY+2)
// Private types
// Private variables
static uint32_t idleCounter;
static uint32_t idleCounterClear;
static xTaskHandle systemTaskHandle;
static bool stackOverflow;
static bool mallocFailed;
// Private functions
static void systemTask(void *parameters);
/**
* Create the module task.
* \returns 0 on success or -1 if initialization failed
*/
int32_t PipXtremeModStart(void)
{
// Initialize vars
stackOverflow = false;
mallocFailed = false;
// Create pipxtreme system task
xTaskCreate(systemTask, (signed char *)"PipXtreme", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &systemTaskHandle);
// Register task
TaskMonitorAdd(TASKINFO_RUNNING_SYSTEM, systemTaskHandle);
return 0;
}
/**
* Initialize the module, called on startup.
* \returns 0 on success or -1 if initialization failed
*/
int32_t PipXtremeModInitialize(void)
{
// Must registers objects here for system thread because ObjectManager started in OpenPilotInit
// Call the module start function.
PipXtremeModStart();
return 0;
}
MODULE_INITCALL(PipXtremeModInitialize, 0)
/**
* System task, periodically executes every SYSTEM_UPDATE_PERIOD_MS
*/
static void systemTask(void *parameters)
{
portTickType lastSysTime;
/* create all modules thread */
MODULE_TASKCREATE_ALL;
if (mallocFailed) {
/* We failed to malloc during task creation,
* system behaviour is undefined. Reset and let
* the BootFault code recover for us.
*/
PIOS_SYS_Reset();
}
#if defined(PIOS_INCLUDE_IAP)
/* Record a successful boot */
PIOS_IAP_WriteBootCount(0);
#endif
// Initialize vars
idleCounter = 0;
idleCounterClear = 0;
lastSysTime = xTaskGetTickCount();
// Main system loop
while (1) {
// Flash the heartbeat LED
#if defined(PIOS_LED_HEARTBEAT)
PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);
#endif /* PIOS_LED_HEARTBEAT */
// Wait until next period
vTaskDelayUntil(&lastSysTime, SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS);
}
}
#ifdef NEVER
/**
* Called periodically to update the system stats
*/
static uint16_t GetFreeIrqStackSize(void)
{
uint32_t i = 0x200;
#if !defined(ARCH_POSIX) && !defined(ARCH_WIN32) && defined(CHECK_IRQ_STACK)
extern uint32_t _irq_stack_top;
extern uint32_t _irq_stack_end;
uint32_t pattern = 0x0000A5A5;
uint32_t *ptr = &_irq_stack_end;
#if 1 /* the ugly way accurate but takes more time, useful for debugging */
uint32_t stack_size = (((uint32_t)&_irq_stack_top - (uint32_t)&_irq_stack_end) & ~3 ) / 4;
for (i=0; i< stack_size; i++)
{
if (ptr[i] != pattern)
{
i=i*4;
break;
}
}
#else /* faster way but not accurate */
if (*(volatile uint32_t *)((uint32_t)ptr + IRQSTACK_LIMIT_CRITICAL) != pattern)
{
i = IRQSTACK_LIMIT_CRITICAL - 1;
}
else if (*(volatile uint32_t *)((uint32_t)ptr + IRQSTACK_LIMIT_WARNING) != pattern)
{
i = IRQSTACK_LIMIT_WARNING - 1;
}
else
{
i = IRQSTACK_LIMIT_WARNING;
}
#endif
#endif
return i;
}
#endif
/**
* Called by the RTOS when the CPU is idle, used to measure the CPU idle time.
*/
void vApplicationIdleHook(void)
{
// Called when the scheduler has no tasks to run
if (idleCounterClear == 0) {
++idleCounter;
} else {
idleCounter = 0;
idleCounterClear = 0;
}
}
/**
* Called by the RTOS when a stack overflow is detected.
*/
#define DEBUG_STACK_OVERFLOW 0
void vApplicationStackOverflowHook(xTaskHandle * pxTask, signed portCHAR * pcTaskName)
{
stackOverflow = true;
#if DEBUG_STACK_OVERFLOW
static volatile bool wait_here = true;
while(wait_here);
wait_here = true;
#endif
}
/**
* Called by the RTOS when a malloc call fails.
*/
#define DEBUG_MALLOC_FAILURES 0
void vApplicationMallocFailedHook(void)
{
mallocFailed = true;
#if DEBUG_MALLOC_FAILURES
static volatile bool wait_here = true;
while(wait_here);
wait_here = true;
#endif
}
/**
* @}
* @}
*/

View File

@ -31,7 +31,6 @@
// ****************
#include <openpilot.h>
#include <hwsettings.h>
#include <radiocombridge.h>
#include <packet_handler.h>
@ -39,6 +38,8 @@
#include "ecc.h"
#undef PIOS_INCLUDE_USB
extern char *debug_msg;
// ****************
@ -82,8 +83,7 @@ typedef struct {
uint32_t com_tx_errors;
uint32_t radio_tx_errors;
// The packet handler.
PHInstHandle packet_handler;
// The packet timeout.
portTickType send_timeout;
uint16_t min_packet_size;
@ -143,17 +143,9 @@ static int32_t RadioComBridgeInitialize(void)
data->com_tx_errors = 0;
data->radio_tx_errors = 0;
// Initialize the packet handler
PacketHandlerConfig phcfg = {
.txWinSize = PIOS_PH_TX_WIN_SIZE,
.maxConnections = PIOS_PH_MAX_CONNECTIONS,
.id = 0x36249acb,
};
data->packet_handler = PHInitialize(&phcfg);
// Register the callbacks with the packet handler
PHRegisterOutputStream(data->packet_handler, transmitPacket);
PHRegisterDataHandler(data->packet_handler, receiveData);
PHRegisterOutputStream(pios_packet_handler, transmitPacket);
PHRegisterDataHandler(pios_packet_handler, receiveData);
// Initialize the packet send timeout
data->send_timeout = 25; // ms
@ -176,9 +168,8 @@ static void radio2ComBridgeTask(void *parameters)
// Receive data from the radio port
rx_bytes = PIOS_COM_ReceiveBuffer(data->radio_port, data->radio2com_buf, BRIDGE_BUF_LEN, 500);
if (rx_bytes > 0) {
PHReceivePacket(data->packet_handler, (PHPacketHandle)data->radio2com_buf);
}
if (rx_bytes > 0)
PHReceivePacket(pios_packet_handler, (PHPacketHandle)data->radio2com_buf);
}
}
@ -196,22 +187,27 @@ static void com2RadioBridgeTask(void * parameters)
while (1) {
#if defined(PIOS_INCLUDE_USB)
// Determine input port (USB takes priority over telemetry port)
if (PIOS_USB_CheckAvailable(0) && PIOS_COM_TELEM_USB) {
if (PIOS_USB_CheckAvailable(0) && PIOS_COM_TELEM_USB)
inputPort = PIOS_COM_TELEM_USB;
} else
else
#endif /* PIOS_INCLUDE_USB */
{
inputPort = data->com_port;
}
// Receive data from the com port
//debug_msg = "COM receive";
uint32_t cur_rx_bytes = PIOS_COM_ReceiveBuffer(inputPort, data->com2radio_buf +
rx_bytes, BRIDGE_BUF_LEN - rx_bytes, timeout);
//debug_msg = "COM receive done";
rx_bytes += cur_rx_bytes;
// Pass the new data through UAVTalk
for (uint8_t i = 0; i < cur_rx_bytes; i++) {
UAVTalkProcessInputStream(data->uavTalkCon, *(data->com2radio_buf + i + rx_bytes));
/*
if(UAVTalkIdle(data->uavTalkCon))
DEBUG_PRINTF(1, "Idle\n\r");
*/
}
// Do we have an data to send?
rx_bytes += cur_rx_bytes;
if (rx_bytes > 0) {
// Check how long since last update
@ -239,7 +235,7 @@ static void com2RadioBridgeTask(void * parameters)
if (send_packet)
{
// Get a TX packet from the packet handler
PHPacketHandle p = PHGetTXPacket(data->packet_handler);
PHPacketHandle p = PHGetTXPacket(pios_packet_handler);
// Initialize the packet.
//p->header.type = PACKET_TYPE_ACKED_DATA;
@ -250,17 +246,13 @@ static void com2RadioBridgeTask(void * parameters)
memcpy(p->data, data->com2radio_buf, rx_bytes);
// Transmit the packet
PHTransmitPacket(data->packet_handler, p);
PHTransmitPacket(pios_packet_handler, p);
// Reset the timeout
timeout = 500;
rx_bytes = 0;
packet_start_time = 0;
}
// Pass the data through UAVTalk
//for (uint8_t i = 0; i < cur_rx_bytes; i++)
//UAVTalkProcessInputStream(data->uavTalkCon, data->com2radio_buf[i]);
}
}
}
@ -275,13 +267,14 @@ static void com2RadioBridgeTask(void * parameters)
*/
static int32_t transmitData(uint8_t *buf, int32_t length)
{
uint32_t inputPort = data->com_port;
uint32_t outputPort = data->com_port;
#if defined(PIOS_INCLUDE_USB)
// Determine input port (USB takes priority over telemetry port)
// Determine output port (USB takes priority over telemetry port)
if (PIOS_USB_CheckAvailable(0) && PIOS_COM_TELEM_USB)
inputPort = PIOS_COM_TELEM_USB;
outputPort = PIOS_COM_TELEM_USB;
#endif /* PIOS_INCLUDE_USB */
return PIOS_COM_SendBuffer(inputPort, buf, length);
DEBUG_PRINTF(1, "Transmitting UAVTalk data\n\r");
return PIOS_COM_SendBuffer(outputPort, buf, length);
}
/**
@ -303,8 +296,14 @@ static int32_t transmitPacket(PHPacketHandle p)
*/
static void receiveData(uint8_t *buf, uint8_t len)
{
uint32_t outputPort = data->com_port;
#if defined(PIOS_INCLUDE_USB)
// Determine output port (USB takes priority over telemetry port)
if (PIOS_USB_CheckAvailable(0) && PIOS_COM_TELEM_USB)
outputPort = PIOS_COM_TELEM_USB;
#endif /* PIOS_INCLUDE_USB */
/* Send the received data to the com port */
if (PIOS_COM_SendBuffer(data->com_port, buf, len) != len)
if (PIOS_COM_SendBuffer(outputPort, buf, len) != len)
/* Error on transmit */
data->com_tx_errors++;
}

View File

@ -214,20 +214,20 @@ extern uint32_t pios_com_rfm22b_id;
// PIOS_RCVR
// See also pios_board.c
//------------------------
#define PIOS_RCVR_MAX_DEVS 3
#define PIOS_RCVR_MAX_CHANNELS 12
#define PIOS_RCVR_MAX_DEVS 3
#define PIOS_RCVR_MAX_CHANNELS 12
//-------------------------
// Receiver PPM input
//-------------------------
#define PIOS_PPM_MAX_DEVS 1
#define PIOS_PPM_NUM_INPUTS 12
#define PIOS_PPM_MAX_DEVS 1
#define PIOS_PPM_NUM_INPUTS 12
//-------------------------
// Receiver PWM input
//-------------------------
#define PIOS_PWM_MAX_DEVS 1
#define PIOS_PWM_NUM_INPUTS 1
#define PIOS_PWM_MAX_DEVS 1
#define PIOS_PWM_NUM_INPUTS 1
//-------------------------
// Servo outputs
@ -282,6 +282,8 @@ extern uint32_t pios_com_rfm22b_id;
// Packet Handler
//-------------------------
uint32_t pios_packet_handler;
#define PIOS_INCLUDE_PACKET_HANDLER
#define PIOS_PH_MAX_PACKET 255
#define PIOS_PH_TX_WIN_SIZE 3
#define PIOS_PH_MAX_CONNECTIONS 1

View File

@ -66,7 +66,7 @@ FLASH_TOOL = OPENOCD
# List of modules to include
OPTMODULES =
MODULES = FirmwareIAP RadioComBridge
MODULES = RadioComBridge
# Paths
OPSYSTEM = ./System
@ -123,10 +123,9 @@ ifndef TESTAPP
## MODULES
SRC += ${foreach MOD, ${MODULES}, ${wildcard ${OPMODULEDIR}/${MOD}/*.c}}
## OPENPILOT CORE:
SRC += ${OPMODULEDIR}/System/systemmod.c
SRC += ${OPMODULEDIR}/PipXtreme/pipxtrememod.c
SRC += $(OPSYSTEM)/pipxtreme.c
SRC += $(OPSYSTEM)/pios_board.c
SRC += $(OPSYSTEM)/alarms.c
SRC += $(OPUAVTALK)/uavtalk.c
SRC += $(OPUAVOBJ)/uavobjectmanager.c
SRC += $(OPUAVOBJ)/eventdispatcher.c
@ -138,18 +137,6 @@ endif
## UAVOBJECTS
ifndef TESTAPP
SRC += $(OPUAVSYNTHDIR)/accessorydesired.c
SRC += $(OPUAVSYNTHDIR)/objectpersistence.c
SRC += $(OPUAVSYNTHDIR)/gcstelemetrystats.c
SRC += $(OPUAVSYNTHDIR)/flighttelemetrystats.c
SRC += $(OPUAVSYNTHDIR)/flightstatus.c
SRC += $(OPUAVSYNTHDIR)/systemstats.c
SRC += $(OPUAVSYNTHDIR)/systemalarms.c
SRC += $(OPUAVSYNTHDIR)/systemsettings.c
SRC += $(OPUAVSYNTHDIR)/manualcontrolcommand.c
SRC += $(OPUAVSYNTHDIR)/i2cstats.c
SRC += $(OPUAVSYNTHDIR)/watchdogstatus.c
SRC += $(OPUAVSYNTHDIR)/firmwareiapobj.c
SRC += $(OPUAVSYNTHDIR)/gcsreceiver.c
endif

View File

@ -75,9 +75,6 @@ void PIOS_Board_Init(void) {
/* Initialize watchdog as early as possible to catch faults during init */
PIOS_WDG_Init();
/* Initialize the alarms library */
AlarmsInitialize();
/* Initialize IAP */
PIOS_IAP_Init();
@ -101,6 +98,10 @@ void PIOS_Board_Init(void) {
PIOS_TIM_InitClock(&tim_4_cfg);
#endif /* PIOS_INCLUDE_TIM */
#if defined(PIOS_INCLUDE_PACKET_HANDLER)
pios_packet_handler = PHInitialize(&pios_ph_cfg);
#endif /* PIOS_INCLUDE_PACKET_HANDLER */
#if defined(PIOS_INCLUDE_USB)
/* Initialize board specific USB data */
PIOS_USB_BOARD_DATA_Init();

View File

@ -492,3 +492,15 @@ const struct pios_rfm22b_cfg pios_rfm22b_cfg = {
};
#endif /* PIOS_INCLUDE_RFM22B */
#if defined(PIOS_INCLUDE_PACKET_HANDLER)
#include <packet_handler.h>
// Initialize the packet handler
PacketHandlerConfig pios_ph_cfg = {
.txWinSize = PIOS_PH_TX_WIN_SIZE,
.maxConnections = PIOS_PH_MAX_CONNECTIONS,
.id = 0x36249acb,
};
#endif /* PIOS_INCLUDE_PACKET_HANDLER */