rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

Merge remote-tracking branch 'op-public/next' into revo-next

Browse Source 
Conflicts:
	flight/Libraries/inc/NMEA.h
	flight/Modules/GPS/GPS.c
	flight/Modules/GPS/NMEA.c
	flight/Modules/GPS/UBX.c
	flight/Modules/GPS/inc/NMEA.h
	flight/Modules/GPS/inc/UBX.h
	flight/Modules/PathPlanner/inc/pathplanner.h
	flight/PiOS/STM32F10x/link_STM32103CB_PIPXTREME_sections.ld
	flight/Revolution/System/inc/pios_config.h
	flight/Revolution/UAVObjects.inc
	flight/SimPosix/UAVObjects.inc
	ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/qdebughandler.h
	ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
	shared/uavobjectdefinition/gpsvelocity.xml
This commit is contained in:
Stacey Sheldon 2012-07-22 23:39:37 -04:00
commit f5db83bd20
132 changed files with 7261 additions and 2153 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
.gitignore vendored
View File

@ -20,6 +20,9 @@
/flight/OpenPilot/Build
/flight/OpenPilot/Build.win32
#flight/Project/OpenPilotOSX
flight/Project/OpenPilotOSX/build
# /flight/PipBee/
/flight/PipBee/Build

16
HISTORY.txt
View File

@ -1,5 +1,21 @@
Short summary of changes. For a complete list see the git log.
2012-07-20
AeroSimRC simulator plugin is now included into the Windows distribution
(will be installed into .../OpenPilot/misc/AeroSIM-RC directory). Still
being an experimental development tool, it could be used to play with
HITL version 2. Other platforms include udp_test utility which can be
used to check the connectivity with AeroSimRC plugin running on Windows
machine.
2012-07-10
On Windows the installation mode was changed from per-user to per-machine
(for all users) installation. It is recommended to completely uninstall
previous version before installing new one to remove per-user installed
files. Per-machine installation requires elevated (administrator) previleges
during install. But since the same rights are now required to install
optional CDC driver (virtual communication port), it was deemed acceptable.
2012-06-04
AeroSimRC support merged into next

2
flight/CopterControl/Makefile
View File

@ -199,6 +199,8 @@ SRC += $(OPUAVSYNTHDIR)/attitudesettings.c
SRC += $(OPUAVSYNTHDIR)/camerastabsettings.c
SRC += $(OPUAVSYNTHDIR)/cameradesired.c
SRC += $(OPUAVSYNTHDIR)/gpsposition.c
SRC += $(OPUAVSYNTHDIR)/gpsvelocity.c
SRC += $(OPUAVSYNTHDIR)/gpssettings.c
SRC += $(OPUAVSYNTHDIR)/hwsettings.c
SRC += $(OPUAVSYNTHDIR)/gcsreceiver.c
SRC += $(OPUAVSYNTHDIR)/receiveractivity.c

3
flight/CopterControl/System/inc/pios_config.h
View File

@ -58,7 +58,8 @@
#define PIOS_INCLUDE_TELEMETRY_RF
#define PIOS_INCLUDE_GPS
#define PIOS_GPS_MINIMAL
#define PIOS_INCLUDE_GPS_NMEA_PARSER /* Include the NMEA protocol parser */
#define PIOS_INCLUDE_GPS_UBX_PARSER /* Include the UBX protocol parser */
#define PIOS_INCLUDE_SERVO
#define PIOS_INCLUDE_SPI
#define PIOS_INCLUDE_SYS

2
flight/Libraries/inc/packet_handler.h
View File

@ -106,7 +106,7 @@ void PHRegisterStatusHandler(PHInstHandle h, PHStatusHandler f);
void PHRegisterPPMHandler(PHInstHandle h, PHPPMHandler f);
uint32_t PHConnect(PHInstHandle h, uint32_t dest_id);
PHPacketHandle PHGetRXPacket(PHInstHandle h);
void PHReleaseTXPacket(PHInstHandle h, PHPacketHandle p);
void PHReleaseRXPacket(PHInstHandle h, PHPacketHandle p);
PHPacketHandle PHGetTXPacket(PHInstHandle h);
void PHReleaseTXPacket(PHInstHandle h, PHPacketHandle p);
uint8_t PHTransmitPacket(PHInstHandle h, PHPacketHandle p);

35
flight/Libraries/packet_handler.c
View File

@ -169,16 +169,15 @@ PHPacketHandle PHGetTXPacket(PHInstHandle h)
// Lock
xSemaphoreTakeRecursive(data->lock, portMAX_DELAY);
PHPacketHandle p = data->tx_packets + data->tx_win_end;
// Is the window full?
uint8_t next_end = (data->tx_win_end + 1) % data->cfg.winSize;
if(next_end == data->tx_win_start)
{
xSemaphoreGiveRecursive(data->lock);
return NULL;
}
data->tx_win_end = next_end;
// Find a free packet.
PHPacketHandle p = NULL;
for (uint8_t i = 0; i < data->cfg.winSize; ++i)
if (data->tx_packets[i].header.type == PACKET_TYPE_NONE)
{
p = data->tx_packets + i;
break;
}
// Release lock
xSemaphoreGiveRecursive(data->lock);
@ -224,17 +223,15 @@ PHPacketHandle PHGetRXPacket(PHInstHandle h)
// Lock
xSemaphoreTakeRecursive(data->lock, portMAX_DELAY);
PHPacketHandle p = data->rx_packets + data->rx_win_end;
// Is the window full?
uint8_t next_end = (data->rx_win_end + 1) % data->cfg.winSize;
if(next_end == data->rx_win_start)
{
// Release lock
xSemaphoreGiveRecursive(data->lock);
return NULL;
}
data->rx_win_end = next_end;
// Find a free packet.
PHPacketHandle p = NULL;
for (uint8_t i = 0; i < data->cfg.winSize; ++i)
if (data->rx_packets[i].header.type == PACKET_TYPE_NONE)
{
p = data->rx_packets + i;
break;
}
// Release lock
xSemaphoreGiveRecursive(data->lock);

4
flight/Modules/Attitude/attitude.c
View File

@ -375,6 +375,10 @@ static int32_t updateSensorsCC3D(AccelsData * accelsData, GyrosData * gyrosData)
if(xQueueReceive(queue, (void *) &mpu6000_data, SENSOR_PERIOD) == errQUEUE_EMPTY)
return -1; // Error, no data
// Do not read raw sensor data in simulation mode
if (GyrosReadOnly() || AccelsReadOnly())
return 0;
gyros[0] = -mpu6000_data.gyro_y * PIOS_MPU6000_GetScale();
gyros[1] = -mpu6000_data.gyro_x * PIOS_MPU6000_GetScale();
gyros[2] = -mpu6000_data.gyro_z * PIOS_MPU6000_GetScale();

6
flight/Modules/FlightPlan/lib/uavobject.py
View File

@ -109,7 +109,10 @@ class UAVObject:
def addField(self, field):
append(self.fields, field)
'''
#
# Support for getName was removed from embedded UAVO database to save RAM + Flash
#
def getName(self):
"""__NATIVE__
UAVObjHandle objHandle;
@ -143,6 +146,7 @@ class UAVObject:
return PM_RET_OK;
"""
pass
'''
def read(self):
"""__NATIVE__

272
flight/Modules/GPS/GPS.c
View File

@ -33,13 +33,12 @@
#include "openpilot.h"
#include "GPS.h"
#include <stdbool.h>
#include "gpsposition.h"
#include "homelocation.h"
#include "gpstime.h"
#include "gpssatellites.h"
#include "gpsvelocity.h"
#include "gpssettings.h"
#include "WorldMagModel.h"
#include "CoordinateConversions.h"
#include "hwsettings.h"
@ -63,16 +62,18 @@ static float GravityAccel(float latitude, float longitude, float altitude);
// Private constants
#define GPS_TIMEOUT_MS 500
#define NMEA_MAX_PACKET_LENGTH 96 // 82 max NMEA msg size plus 12 margin (because some vendors add custom crap) plus CR plus Linefeed
// same as in COM buffer
#ifdef PIOS_GPS_SETS_HOMELOCATION
// Unfortunately need a good size stack for the WMM calculation
#define STACK_SIZE_BYTES 800
#define STACK_SIZE_BYTES 750
#else
#if defined(PIOS_GPS_MINIMAL)
#define STACK_SIZE_BYTES 500
#else
#define STACK_SIZE_BYTES 650
#endif
#endif // PIOS_GPS_MINIMAL
#endif // PIOS_GPS_SETS_HOMELOCATION
#define TASK_PRIORITY (tskIDLE_PRIORITY + 1)
@ -88,9 +89,8 @@ static char* gps_rx_buffer;
static uint32_t timeOfLastCommandMs;
static uint32_t timeOfLastUpdateMs;
static uint32_t numUpdates;
static uint32_t numChecksumErrors;
static uint32_t numParsingErrors;
static struct GPS_RX_STATS gpsRxStats;
// ****************
/**
@ -122,6 +122,7 @@ int32_t GPSStart(void)
int32_t GPSInitialize(void)
{
gpsPort = PIOS_COM_GPS;
uint8_t gpsProtocol;
#ifdef MODULE_GPS_BUILTIN
gpsEnabled = true;
@ -137,19 +138,9 @@ int32_t GPSInitialize(void)
gpsEnabled = false;
#endif
#if defined(REVOLUTION)
// Revolution expects these objects to always be defined. Not doing so will fail some
// queue connections in navigation
GPSPositionInitialize();
GPSVelocityInitialize();
GPSTimeInitialize();
GPSSatellitesInitialize();
HomeLocationInitialize();
updateSettings();
#else
if (gpsPort && gpsEnabled) {
GPSPositionInitialize();
GPSVelocityInitialize();
#if !defined(PIOS_GPS_MINIMAL)
GPSTimeInitialize();
GPSSatellitesInitialize();
@ -159,10 +150,20 @@ int32_t GPSInitialize(void)
#endif
updateSettings();
}
#endif
if (gpsPort && gpsEnabled) {
gps_rx_buffer = pvPortMalloc(NMEA_MAX_PACKET_LENGTH);
GPSSettingsInitialize();
GPSSettingsDataProtocolGet(&gpsProtocol);
switch (gpsProtocol) {
case GPSSETTINGS_DATAPROTOCOL_NMEA:
gps_rx_buffer = pvPortMalloc(NMEA_MAX_PACKET_LENGTH);
break;
case GPSSETTINGS_DATAPROTOCOL_UBX:
gps_rx_buffer = pvPortMalloc(sizeof(struct UBXPacket));
break;
default:
gps_rx_buffer = NULL;
}
PIOS_Assert(gps_rx_buffer);
return 0;
@ -181,216 +182,75 @@ MODULE_INITCALL(GPSInitialize, GPSStart)
static void gpsTask(void *parameters)
{
portTickType xDelay = 100 / portTICK_RATE_MS;
uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;;
GPSPositionData GpsData;
UBXPacket *ubx = (UBXPacket *)gps_rx_buffer;
uint8_t rx_count = 0;
// bool start_flag = false;
bool found_cr = false;
enum proto_states {START,NMEA,UBX_SY2,UBX_CLASS,UBX_ID,UBX_LEN1,
UBX_LEN2,UBX_PAYLOAD,UBX_CHK1,UBX_CHK2};
enum proto_states proto_state = START;
int32_t gpsRxOverflow = 0;
numUpdates = 0;
numChecksumErrors = 0;
numParsingErrors = 0;
uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
GPSPositionData gpsposition;
uint8_t gpsProtocol;
GPSSettingsDataProtocolGet(&gpsProtocol);
timeOfLastUpdateMs = timeNowMs;
timeOfLastCommandMs = timeNowMs;
GPSPositionGet(&GpsData);
GPSPositionGet(&gpsposition);
// Loop forever
while (1)
{
uint8_t c;
// NMEA or SINGLE-SENTENCE GPS mode
// This blocks the task until there is something on the buffer
while (PIOS_COM_ReceiveBuffer(gpsPort, &c, 1, xDelay) > 0)
{
// detect start while acquiring stream
switch (proto_state)
{
case START: // detect protocol
switch (c)
{
case UBX_SYNC1: // first UBX sync char found
proto_state = UBX_SY2;
continue;
case '$': // NMEA identifier found
proto_state = NMEA;
found_cr = false;
rx_count = 0;
break;
default:
continue;
}
int res;
switch (gpsProtocol) {
#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER)
case GPSSETTINGS_DATAPROTOCOL_NMEA:
res = parse_nmea_stream (c,gps_rx_buffer, &gpsposition, &gpsRxStats);
break;
case UBX_SY2:
if (c == UBX_SYNC2) // second UBX sync char found
{
proto_state = UBX_CLASS;
found_cr = false;
rx_count = 0;
}
else
{
proto_state = START; // reset state
}
continue;
case UBX_CLASS:
ubx->header.class = c;
proto_state = UBX_ID;
continue;
case UBX_ID:
ubx->header.id = c;
proto_state = UBX_LEN1;
continue;
case UBX_LEN1:
ubx->header.len = c;
proto_state = UBX_LEN2;
continue;
case UBX_LEN2:
ubx->header.len += (c << 8);
if ((sizeof (UBXHeader)) + ubx->header.len > NMEA_MAX_PACKET_LENGTH)
{
gpsRxOverflow++;
proto_state = START;
found_cr = false;
rx_count = 0;
}
else
{
proto_state = UBX_PAYLOAD;
}
continue;
case UBX_PAYLOAD:
if (rx_count < ubx->header.len)
{
ubx->payload.payload[rx_count] = c;
if (++rx_count == ubx->header.len)
proto_state = UBX_CHK1;
}
else
proto_state = START;
continue;
case UBX_CHK1:
ubx->header.ck_a = c;
proto_state = UBX_CHK2;
continue;
case UBX_CHK2:
ubx->header.ck_b = c;
if (checksum_ubx_message(ubx))
{
parse_ubx_message(ubx);
}
proto_state = START;
continue;
case NMEA:
#endif
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
case GPSSETTINGS_DATAPROTOCOL_UBX:
res = parse_ubx_stream (c,gps_rx_buffer, &gpsposition, &gpsRxStats);
break;
#endif
default:
res = NO_PARSER; // this should not happen
break;
}
if (rx_count >= NMEA_MAX_PACKET_LENGTH)
{
// The buffer is already full and we haven't found a valid NMEA sentence.
// Flush the buffer and note the overflow event.
gpsRxOverflow++;
proto_state = START;
found_cr = false;
rx_count = 0;
}
else
{
gps_rx_buffer[rx_count] = c;
rx_count++;
}
// look for ending '\r\n' sequence
if (!found_cr && (c == '\r') )
found_cr = true;
else
if (found_cr && (c != '\n') )
found_cr = false; // false end flag
else
if (found_cr && (c == '\n') )
{
// The NMEA functions require a zero-terminated string
// As we detected \r\n, the string as for sure 2 bytes long, we will also strip the \r\n
gps_rx_buffer[rx_count-2] = 0;
// prepare to parse next sentence
proto_state = START;
found_cr = false;
rx_count = 0;
// Our rxBuffer must look like this now:
// [0] = '$'
// ... = zero or more bytes of sentence payload
// [end_pos - 1] = '\r'
// [end_pos] = '\n'
//
// Prepare to consume the sentence from the buffer
// Validate the checksum over the sentence
if (!NMEA_checksum(&gps_rx_buffer[1]))
{ // Invalid checksum. May indicate dropped characters on Rx.
//PIOS_DEBUG_PinHigh(2);
++numChecksumErrors;
//PIOS_DEBUG_PinLow(2);
}
else
{ // Valid checksum, use this packet to update the GPS position
if (!NMEA_update_position(&gps_rx_buffer[1],&GpsData)) {
//PIOS_DEBUG_PinHigh(2);
++numParsingErrors;
//PIOS_DEBUG_PinLow(2);
}
else
++numUpdates;
timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
timeOfLastUpdateMs = timeNowMs;
timeOfLastCommandMs = timeNowMs;
}
if (res == PARSER_COMPLETE) {
timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
timeOfLastUpdateMs = timeNowMs;
timeOfLastCommandMs = timeNowMs;
}
}
// Check for GPS timeout
timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
if ((timeNowMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS)
{ // we have not received any valid GPS sentences for a while.
if ((timeNowMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS) {
// we have not received any valid GPS sentences for a while.
// either the GPS is not plugged in or a hardware problem or the GPS has locked up.
GpsData.Status = GPSPOSITION_STATUS_NOGPS;
GPSPositionSet(&GpsData);
uint8_t status = GPSPOSITION_STATUS_NOGPS;
GPSPositionStatusSet(&status);
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR);
}
else
{ // we appear to be receiving GPS sentences OK, we've had an update
#ifdef PIOS_GPS_SETS_HOMELOCATION
HomeLocationData home;
HomeLocationGet(&home);
if ((GpsData.Status == GPSPOSITION_STATUS_FIX3D) && (home.Set == HOMELOCATION_SET_FALSE))
setHomeLocation(&GpsData);
#endif
} else {
// we appear to be receiving GPS sentences OK, we've had an update
//criteria for GPS-OK taken from this post...
//http://forums.openpilot.org/topic/1523-professors-insgps-in-svn/page__view__findpost__p__5220
if ((GpsData.PDOP < 3.5) && (GpsData.Satellites >= 7))
if ((gpsposition.PDOP < 3.5) && (gpsposition.Satellites >= 7) &&
(gpsposition.Status == GPSPOSITION_STATUS_FIX3D)) {
AlarmsClear(SYSTEMALARMS_ALARM_GPS);
else
if (GpsData.Status == GPSPOSITION_STATUS_FIX3D)
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_WARNING);
else
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL);
#ifdef PIOS_GPS_SETS_HOMELOCATION
HomeLocationData home;
HomeLocationGet(&home);
if (home.Set == HOMELOCATION_SET_FALSE)
setHomeLocation(&gpsposition);
#endif
} else if (gpsposition.Status == GPSPOSITION_STATUS_FIX3D)
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_WARNING);
else
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL);
}
}

122
flight/Modules/GPS/NMEA.c
View File

@ -30,10 +30,14 @@
#include "openpilot.h"
#include "pios.h"
#if defined(PIOS_INCLUDE_GPS_NMEA_PARSER)
#include "gpsposition.h"
#include "NMEA.h"
#include "gpstime.h"
#include "gpssatellites.h"
#include "GPS.h"
//#define ENABLE_DEBUG_MSG ///< define to enable debug-messages
#define DEBUG_PORT PIOS_COM_TELEM_RF ///< defines which serial port is ued for debug-messages
@ -43,7 +47,7 @@
// Debugging
#ifdef ENABLE_DEBUG_MSG
//#define DEBUG_MSG_IN ///< define to display the incoming NMEA messages
//#define DEBUG_PARS ///< define to display the incoming NMEA messages split into its parameters
//#define DEBUG_PARAMS ///< define to display the incoming NMEA messages split into its parameters
//#define DEBUG_MGSID_IN ///< define to display the the names of the incoming NMEA messages
//#define NMEA_DEBUG_PKT ///< define to enable debug of all NMEA messages
//#define NMEA_DEBUG_GGA ///< define to enable debug of GGA messages
@ -63,7 +67,6 @@
struct nmea_parser {
const char *prefix;
bool(*handler) (GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam);
uint32_t cnt;
};
static bool nmeaProcessGPGGA(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam);
@ -75,50 +78,125 @@ static bool nmeaProcessGPGSA(GPSPositionData * GpsData, bool* gpsDataUpdated, ch
static bool nmeaProcessGPGSV(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam);
#endif //PIOS_GPS_MINIMAL
static struct nmea_parser nmea_parsers[] = {
const static struct nmea_parser nmea_parsers[] = {
{
.prefix = "GPGGA",
.handler = nmeaProcessGPGGA,
.cnt = 0,
},
{
.prefix = "GPVTG",
.handler = nmeaProcessGPVTG,
.cnt = 0,
},
{
.prefix = "GPGSA",
.handler = nmeaProcessGPGSA,
.cnt = 0,
},
{
.prefix = "GPRMC",
.handler = nmeaProcessGPRMC,
.cnt = 0,
},
#if !defined(PIOS_GPS_MINIMAL)
{
.prefix = "GPZDA",
.handler = nmeaProcessGPZDA,
.cnt = 0,
},
{
.prefix = "GPGSV",
.handler = nmeaProcessGPGSV,
.cnt = 0,
},
#endif //PIOS_GPS_MINIMAL
};
static struct nmea_parser *NMEA_find_parser_by_prefix(const char *prefix)
int parse_nmea_stream (uint8_t c, char *gps_rx_buffer, GPSPositionData *GpsData, struct GPS_RX_STATS *gpsRxStats)
{
static uint8_t rx_count = 0;
static bool start_flag = false;
static bool found_cr = false;
// detect start while acquiring stream
if (!start_flag && (c == '$')) // NMEA identifier found
{
start_flag = true;
found_cr = false;
rx_count = 0;
}
else
if (!start_flag)
return PARSER_ERROR;
if (rx_count >= NMEA_MAX_PACKET_LENGTH)
{
// The buffer is already full and we haven't found a valid NMEA sentence.
// Flush the buffer and note the overflow event.
gpsRxStats->gpsRxOverflow++;
start_flag = false;
found_cr = false;
rx_count = 0;
return PARSER_OVERRUN;
}
else
{
gps_rx_buffer[rx_count] = c;
rx_count++;
}
// look for ending '\r\n' sequence
if (!found_cr && (c == '\r') )
found_cr = true;
else
if (found_cr && (c != '\n') )
found_cr = false; // false end flag
else
if (found_cr && (c == '\n') )
{
// The NMEA functions require a zero-terminated string
// As we detected \r\n, the string as for sure 2 bytes long, we will also strip the \r\n
gps_rx_buffer[rx_count-2] = 0;
// prepare to parse next sentence
start_flag = false;
found_cr = false;
rx_count = 0;
// Our rxBuffer must look like this now:
// [0] = '$'
// ... = zero or more bytes of sentence payload
// [end_pos - 1] = '\r'
// [end_pos] = '\n'
//
// Prepare to consume the sentence from the buffer
// Validate the checksum over the sentence
if (!NMEA_checksum(&gps_rx_buffer[1]))
{ // Invalid checksum. May indicate dropped characters on Rx.
//PIOS_DEBUG_PinHigh(2);
gpsRxStats->gpsRxChkSumError++;
//PIOS_DEBUG_PinLow(2);
return PARSER_ERROR;
}
else
{ // Valid checksum, use this packet to update the GPS position
if (!NMEA_update_position(&gps_rx_buffer[1], GpsData)) {
//PIOS_DEBUG_PinHigh(2);
gpsRxStats->gpsRxParserError++;
//PIOS_DEBUG_PinLow(2);
}
else
gpsRxStats->gpsRxReceived++;;
return PARSER_COMPLETE;
}
}
return PARSER_INCOMPLETE;
}
const static struct nmea_parser *NMEA_find_parser_by_prefix(const char *prefix)
{
if (!prefix) {
return (NULL);
}
for (uint8_t i = 0; i < NELEMENTS(nmea_parsers); i++) {
struct nmea_parser *parser = &nmea_parsers[i];
const struct nmea_parser *parser = &nmea_parsers[i];
/* Use strcmp to check for exact equality over the entire prefix */
if (!strcmp(prefix, parser->prefix)) {
@ -331,7 +409,7 @@ bool NMEA_update_position(char *nmea_sentence, GPSPositionData *GpsData)
#endif
// The first parameter is the message name, lets see if we find a parser for it
struct nmea_parser *parser;
const struct nmea_parser *parser;
parser = NMEA_find_parser_by_prefix(params[0]);
if (!parser) {
// No parser found
@ -339,16 +417,22 @@ bool NMEA_update_position(char *nmea_sentence, GPSPositionData *GpsData)
return false;
}
parser->cnt++;
#ifdef DEBUG_MGSID_IN
DEBUG_MSG("%s %d ", params[0], parser->cnt);
DEBUG_MSG("%s %d ", params[0]);
#endif
// Send the message to the parser and get it update the GpsData
// Information from various different NMEA messages are temporarily
// cumulated in the GpsData structure. An actual GPSPosition update
// is triggered by GGA messages only. This message type sets the
// gpsDataUpdated flag to request this.
bool gpsDataUpdated = false;
if (!parser->handler(GpsData, &gpsDataUpdated, params, nbParams)) {
// Parse failed
DEBUG_MSG("PARSE FAILED (\"%s\")\n", params[0]);
if (gpsDataUpdated && (GpsData->Status == GPSPOSITION_STATUS_NOFIX)) {
GPSPositionSet(GpsData);
}
return false;
}
@ -392,6 +476,13 @@ static bool nmeaProcessGPGGA(GPSPositionData * GpsData, bool* gpsDataUpdated, ch
*gpsDataUpdated = true;
// check for invalid GPS fix
// do this first to make sure we get this information, even if later checks exit
// this function early
if (param[6][0] == '0') {
GpsData->Status = GPSPOSITION_STATUS_NOFIX; // treat invalid fix as NOFIX
}
// get latitude [DDMM.mmmmm] [N|S]
if (!NMEA_latlon_to_fixed_point(&GpsData->Latitude, param[2], param[3][0] == 'S')) {
return false;
@ -681,3 +772,4 @@ static bool nmeaProcessGPGSA(GPSPositionData * GpsData, bool* gpsDataUpdated, ch
return true;
}
#endif // PIOS_INCLUDE_GPS_NMEA_PARSER

308
flight/Modules/GPS/UBX.c
View File

@ -30,10 +30,137 @@
#include "openpilot.h"
#include "pios.h"
#include "UBX.h"
#include "gpsvelocity.h"
bool checksum_ubx_message (UBXPacket *ubx)
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER)
#include "UBX.h"
#include "GPS.h"
// parse incoming character stream for messages in UBX binary format
int parse_ubx_stream (uint8_t c, char *gps_rx_buffer, GPSPositionData *GpsData, struct GPS_RX_STATS *gpsRxStats)
{
enum proto_states {
START,
UBX_SY2,
UBX_CLASS,
UBX_ID,
UBX_LEN1,
UBX_LEN2,
UBX_PAYLOAD,
UBX_CHK1,
UBX_CHK2,
FINISHED
};
static enum proto_states proto_state = START;
static uint8_t rx_count = 0;
struct UBXPacket *ubx = (struct UBXPacket *)gps_rx_buffer;
switch (proto_state) {
case START: // detect protocol
if (c == UBX_SYNC1) // first UBX sync char found
proto_state = UBX_SY2;
break;
case UBX_SY2:
if (c == UBX_SYNC2) // second UBX sync char found
proto_state = UBX_CLASS;
else
proto_state = START; // reset state
break;
case UBX_CLASS:
ubx->header.class = c;
proto_state = UBX_ID;
break;
case UBX_ID:
ubx->header.id = c;
proto_state = UBX_LEN1;
break;
case UBX_LEN1:
ubx->header.len = c;
proto_state = UBX_LEN2;
break;
case UBX_LEN2:
ubx->header.len += (c << 8);
if (ubx->header.len > sizeof(UBXPayload)) {
gpsRxStats->gpsRxOverflow++;
proto_state = START;
} else {
rx_count = 0;
proto_state = UBX_PAYLOAD;
}
break;
case UBX_PAYLOAD:
if (rx_count < ubx->header.len) {
ubx->payload.payload[rx_count] = c;
if (++rx_count == ubx->header.len)
proto_state = UBX_CHK1;
} else {
gpsRxStats->gpsRxOverflow++;
proto_state = START;
}
break;
case UBX_CHK1:
ubx->header.ck_a = c;
proto_state = UBX_CHK2;
break;
case UBX_CHK2:
ubx->header.ck_b = c;
if (checksum_ubx_message(ubx)) { // message complete and valid
parse_ubx_message(ubx, GpsData);
proto_state = FINISHED;
} else {
gpsRxStats->gpsRxChkSumError++;
proto_state = START;
}
break;
default: break;
}
if (proto_state == START)
return PARSER_ERROR; // parser couldn't use this byte
else if (proto_state == FINISHED) {
gpsRxStats->gpsRxReceived++;
proto_state = START;
return PARSER_COMPLETE; // message complete & processed
}
return PARSER_INCOMPLETE; // message not (yet) complete
}
// Keep track of various GPS messages needed to make up a single UAVO update
// time-of-week timestamp is used to correlate matching messages
#define POSLLH_RECEIVED (1 << 0)
#define STATUS_RECEIVED (1 << 1)
#define DOP_RECEIVED (1 << 2)
#define VELNED_RECEIVED (1 << 3)
#define SOL_RECEIVED (1 << 4)
#define ALL_RECEIVED (SOL_RECEIVED | VELNED_RECEIVED | DOP_RECEIVED | POSLLH_RECEIVED)
#define NONE_RECEIVED 0
static struct msgtracker{
uint32_t currentTOW; // TOW of the message set currently in progress
uint8_t msg_received; // keep track of received message types
} msgtracker;
// Check if a message belongs to the current data set and register it as 'received'
bool check_msgtracker (uint32_t tow, uint8_t msg_flag)
{
if (tow > msgtracker.currentTOW ? true // start of a new message set
: (msgtracker.currentTOW - tow > 6*24*3600*1000)) { // 6 days, TOW wrap around occured
msgtracker.currentTOW = tow;
msgtracker.msg_received = NONE_RECEIVED;
} else if (tow < msgtracker.currentTOW) // message outdated (don't process)
return false;
msgtracker.msg_received |= msg_flag; // register reception of this msg type
return true;
}
bool checksum_ubx_message (struct UBXPacket *ubx)
{
int i;
uint8_t ck_a, ck_b;
@ -50,50 +177,165 @@ bool checksum_ubx_message (UBXPacket *ubx)
ck_a += ubx->header.len >> 8;
ck_b += ck_a;
for (i = 0; i < ubx->header.len; i++)
{
for (i = 0; i < ubx->header.len; i++) {
ck_a += ubx->payload.payload[i];
ck_b += ck_a;
}
if (ubx->header.ck_a == ck_a &&
ubx->header.ck_b == ck_b)
{
return true;
}
else
return false;
}
void parse_ubx_nav_velned (UBXPayload payload)
void parse_ubx_nav_posllh (struct UBX_NAV_POSLLH *posllh, GPSPositionData *GpsPosition)
{
#if defined(REVOLUTION)
GPSVelocityData GpsVelocity;
GPSVelocityGet(&GpsVelocity);
GpsVelocity.North = (float)payload.nav_velned.velN/100.0f;
GpsVelocity.East = (float)payload.nav_velned.velE/100.0f;
GpsVelocity.Down = (float)payload.nav_velned.velD/100.0f;
GPSVelocitySet(&GpsVelocity);
#endif
}
void parse_ubx_message (UBXPacket *ubx)
{
switch (ubx->header.class)
{
case UBX_CLASS_NAV:
switch (ubx->header.id)
{
case UBX_ID_VELNED:
parse_ubx_nav_velned (ubx->payload);
}
break;
if (check_msgtracker(posllh->iTOW, POSLLH_RECEIVED)) {
if (GpsPosition->Status != GPSPOSITION_STATUS_NOFIX) {
GpsPosition->Altitude = (float)posllh->hMSL*0.001f;
GpsPosition->GeoidSeparation = (float)(posllh->height - posllh->hMSL)*0.001f;
GpsPosition->Latitude = posllh->lat;
GpsPosition->Longitude = posllh->lon;
}
}
}
void parse_ubx_nav_sol (struct UBX_NAV_SOL *sol, GPSPositionData *GpsPosition)
{
if (check_msgtracker(sol->iTOW, SOL_RECEIVED)) {
GpsPosition->Satellites = sol->numSV;
if (sol->flags & STATUS_FLAGS_GPSFIX_OK) {
switch (sol->gpsFix) {
case STATUS_GPSFIX_2DFIX:
GpsPosition->Status = GPSPOSITION_STATUS_FIX2D;
break;
case STATUS_GPSFIX_3DFIX:
GpsPosition->Status = GPSPOSITION_STATUS_FIX3D;
break;
default: GpsPosition->Status = GPSPOSITION_STATUS_NOFIX;
}
}
else // fix is not valid so we make sure to treat is as NOFIX
GpsPosition->Status = GPSPOSITION_STATUS_NOFIX;
}
}
void parse_ubx_nav_dop (struct UBX_NAV_DOP *dop, GPSPositionData *GpsPosition)
{
if (check_msgtracker(dop->iTOW, DOP_RECEIVED)) {
GpsPosition->HDOP = (float)dop->hDOP * 0.01f;
GpsPosition->VDOP = (float)dop->vDOP * 0.01f;
GpsPosition->PDOP = (float)dop->pDOP * 0.01f;
}
}
void parse_ubx_nav_velned (struct UBX_NAV_VELNED *velned, GPSPositionData *GpsPosition)
{
GPSVelocityData GpsVelocity;
if (check_msgtracker(velned->iTOW, VELNED_RECEIVED)) {
if (GpsPosition->Status != GPSPOSITION_STATUS_NOFIX) {
GpsVelocity.North = (float)velned->velN/100.0f;
GpsVelocity.East = (float)velned->velE/100.0f;
GpsVelocity.Down = (float)velned->velD/100.0f;
GPSVelocitySet(&GpsVelocity);
GpsPosition->Groundspeed = (float)velned->gSpeed * 0.01f;
GpsPosition->Heading = (float)velned->heading * 1.0e-5f;
}
}
}
#if !defined(PIOS_GPS_MINIMAL)
void parse_ubx_nav_timeutc (struct UBX_NAV_TIMEUTC *timeutc)
{
if (!(timeutc->valid & TIMEUTC_VALIDUTC))
return;
GPSTimeData GpsTime;
GpsTime.Year = timeutc->year;
GpsTime.Month = timeutc->month;
GpsTime.Day = timeutc->day;
GpsTime.Hour = timeutc->hour;
GpsTime.Minute = timeutc->min;
GpsTime.Second = timeutc->sec;
GPSTimeSet(&GpsTime);
}
#endif
#if !defined(PIOS_GPS_MINIMAL)
void parse_ubx_nav_svinfo (struct UBX_NAV_SVINFO *svinfo)
{
uint8_t chan;
GPSSatellitesData svdata;
svdata.SatsInView = 0;
for (chan = 0; chan < svinfo->numCh; chan++) {
if (svdata.SatsInView < GPSSATELLITES_PRN_NUMELEM) {
svdata.Azimuth[svdata.SatsInView] = (float)svinfo->sv[chan].azim;
svdata.Elevation[svdata.SatsInView] = (float)svinfo->sv[chan].elev;
svdata.PRN[svdata.SatsInView] = svinfo->sv[chan].svid;
svdata.SNR[svdata.SatsInView] = svinfo->sv[chan].cno;
svdata.SatsInView++;
}
}
// fill remaining slots (if any)
for (chan = svdata.SatsInView; chan < GPSSATELLITES_PRN_NUMELEM; chan++) {
svdata.Azimuth[chan] = (float)0.0f;
svdata.Elevation[chan] = (float)0.0f;
svdata.PRN[chan] = 0;
svdata.SNR[chan] = 0;
}
GPSSatellitesSet(&svdata);
}
#endif
// UBX message parser
// returns UAVObjectID if a UAVObject structure is ready for further processing
uint32_t parse_ubx_message (struct UBXPacket *ubx, GPSPositionData *GpsPosition)
{
uint32_t id = 0;
switch (ubx->header.class) {
case UBX_CLASS_NAV:
switch (ubx->header.id) {
case UBX_ID_POSLLH:
parse_ubx_nav_posllh (&ubx->payload.nav_posllh, GpsPosition);
break;
case UBX_ID_DOP:
parse_ubx_nav_dop (&ubx->payload.nav_dop, GpsPosition);
break;
case UBX_ID_SOL:
parse_ubx_nav_sol (&ubx->payload.nav_sol, GpsPosition);
break;
case UBX_ID_VELNED:
parse_ubx_nav_velned (&ubx->payload.nav_velned, GpsPosition);
break;
#if !defined(PIOS_GPS_MINIMAL)
case UBX_ID_TIMEUTC:
parse_ubx_nav_timeutc (&ubx->payload.nav_timeutc);
break;
case UBX_ID_SVINFO:
parse_ubx_nav_svinfo (&ubx->payload.nav_svinfo);
break;
#endif
}
break;
}
if (msgtracker.msg_received == ALL_RECEIVED) {
GPSPositionSet(GpsPosition);
msgtracker.msg_received = NONE_RECEIVED;
id = GPSPOSITION_OBJID;
}
return id;
}
#endif // PIOS_INCLUDE_GPS_UBX_PARSER

18
flight/Modules/GPS/inc/GPS.h
View File

@ -34,6 +34,24 @@
#ifndef GPS_H
#define GPS_H
#include "gpsvelocity.h"
#include "gpssatellites.h"
#include "gpsposition.h"
#include "gpstime.h"
#define NO_PARSER -3 // no parser available
#define PARSER_OVERRUN -2 // message buffer overrun before completing the message
#define PARSER_ERROR -1 // message unparsable by this parser
#define PARSER_INCOMPLETE 0 // parser needs more data to complete the message
#define PARSER_COMPLETE 1 // parser has received a complete message and finished processing
struct GPS_RX_STATS {
uint16_t gpsRxReceived;
uint16_t gpsRxChkSumError;
uint16_t gpsRxOverflow;
uint16_t gpsRxParserError;
};
int32_t GPSInitialize(void);
#endif // GPS_H

4
flight/Modules/GPS/inc/NMEA.h
View File

@ -33,8 +33,12 @@
#include <stdbool.h>
#include <stdint.h>
#include "GPS.h"
#define NMEA_MAX_PACKET_LENGTH 96 // 82 max NMEA msg size plus 12 margin (because some vendors add custom crap) plus CR plus Linefeed
extern bool NMEA_update_position(char *nmea_sentence, GPSPositionData *GpsData);
extern bool NMEA_checksum(char *nmea_sentence);
extern int parse_nmea_stream(uint8_t, char *, GPSPositionData *, struct GPS_RX_STATS *);
#endif /* NMEA_H */

304
flight/Modules/GPS/inc/UBX.h
View File

@ -1,80 +1,224 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup GSPModule GPS Module
* @brief Process GPS information
* @{
*
* @file UBX.h
* @author 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
*
*****************************************************************************/
/*
* 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 UBX_H
#define UBX_H
#include "openpilot.h"
#define UBX_SYNC1 0xb5 // UBX protocol synchronization characters
#define UBX_SYNC2 0x62
// From u-blox6 receiver protocol specification
// Messages classes
#define UBX_CLASS_NAV 0x01
// Message IDs
#define UBX_ID_VELNED 0x12
// private structures
typedef struct {
uint32_t iTOW; // ms GPS Millisecond Time of Week
int32_t velN; // cm/s NED north velocity
int32_t velE; // cm/s NED east velocity
int32_t velD; // cm/s NED down velocity
uint32_t speed; // cm/s Speed (3-D)
uint32_t gSpeed; // cm/s Ground Speed (2-D)
int32_t heading; // 1e-5 *deg Heading of motion 2-D
uint32_t sAcc; // cm/s Speed Accuracy Estimate
uint32_t cAcc; // 1e-5 *deg Course / Heading Accuracy Estimate
} UBX_NAV_VELNED;
typedef union { // add more message types later
uint8_t payload[0];
UBX_NAV_VELNED nav_velned;
} UBXPayload;
typedef struct {
uint8_t class;
uint8_t id;
uint16_t len;
uint8_t ck_a;
uint8_t ck_b;
} UBXHeader;
typedef struct {
UBXHeader header;
UBXPayload payload;
} UBXPacket;
bool checksum_ubx_message(UBXPacket *);
void parse_ubx_message(UBXPacket *);
#endif /* UBX_H */
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup GSPModule GPS Module
* @brief Process GPS information
* @{
*
* @file UBX.h
* @author 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
*
*****************************************************************************/
/*
* 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 UBX_H
#define UBX_H
#include "openpilot.h"
#include "gpsposition.h"
#include "GPS.h"
#define UBX_SYNC1 0xb5 // UBX protocol synchronization characters
#define UBX_SYNC2 0x62
// From u-blox6 receiver protocol specification
// Messages classes
#define UBX_CLASS_NAV 0x01
// Message IDs
#define UBX_ID_POSLLH 0x02
#define UBX_ID_STATUS 0x03
#define UBX_ID_DOP 0x04
#define UBX_ID_SOL 0x06
#define UBX_ID_VELNED 0x12
#define UBX_ID_TIMEUTC 0x21
#define UBX_ID_SVINFO 0x30
// private structures
// Geodetic Position Solution
struct UBX_NAV_POSLLH {
uint32_t iTOW; // GPS Millisecond Time of Week (ms)
int32_t lon; // Longitude (deg*1e-7)
int32_t lat; // Latitude (deg*1e-7)
int32_t height; // Height above Ellipsoid (mm)
int32_t hMSL; // Height above mean sea level (mm)
uint32_t hAcc; // Horizontal Accuracy Estimate (mm)
uint32_t vAcc; // Vertical Accuracy Estimate (mm)
};
// Receiver Navigation Status
#define STATUS_GPSFIX_NOFIX 0x00
#define STATUS_GPSFIX_DRONLY 0x01
#define STATUS_GPSFIX_2DFIX 0x02
#define STATUS_GPSFIX_3DFIX 0x03
#define STATUS_GPSFIX_GPSDR 0x04
#define STATUS_GPSFIX_TIMEONLY 0x05
#define STATUS_FLAGS_GPSFIX_OK (1 << 0)
#define STATUS_FLAGS_DIFFSOLN (1 << 1)
#define STATUS_FLAGS_WKNSET (1 << 2)
#define STATUS_FLAGS_TOWSET (1 << 3)
struct UBX_NAV_STATUS {
uint32_t iTOW; // GPS Millisecond Time of Week (ms)
uint8_t gpsFix; // GPS fix type
uint8_t flags; // Navigation Status Flags
uint8_t fixStat; // Fix Status Information
uint8_t flags2; // Additional navigation output information
uint32_t ttff; // Time to first fix (ms)
uint32_t msss; // Milliseconds since startup/reset (ms)
};
// Dilution of precision
struct UBX_NAV_DOP {
uint32_t iTOW; // GPS Millisecond Time of Week (ms)
uint16_t gDOP; // Geometric DOP
uint16_t pDOP; // Position DOP
uint16_t tDOP; // Time DOP
uint16_t vDOP; // Vertical DOP
uint16_t hDOP; // Horizontal DOP
uint16_t nDOP; // Northing DOP
uint16_t eDOP; // Easting DOP
};
// Navigation solution
struct UBX_NAV_SOL {
uint32_t iTOW; // GPS Millisecond Time of Week (ms)
int32_t fTOW; // fractional nanoseconds (ns)
int16_t week; // GPS week
uint8_t gpsFix; // GPS fix type
uint8_t flags; // Fix status flags
int32_t ecefX; // ECEF X coordinate (cm)
int32_t ecefY; // ECEF Y coordinate (cm)
int32_t ecefZ; // ECEF Z coordinate (cm)
uint32_t pAcc; // 3D Position Accuracy Estimate (cm)
int32_t ecefVX; // ECEF X coordinate (cm/s)
int32_t ecefVY; // ECEF Y coordinate (cm/s)
int32_t ecefVZ; // ECEF Z coordinate (cm/s)
uint32_t sAcc; // Speed Accuracy Estimate
uint16_t pDOP; // Position DOP
uint8_t reserved1; // Reserved
uint8_t numSV; // Number of SVs used in Nav Solution
uint32_t reserved2; // Reserved
};
// North/East/Down velocity
struct UBX_NAV_VELNED {
uint32_t iTOW; // ms GPS Millisecond Time of Week
int32_t velN; // cm/s NED north velocity
int32_t velE; // cm/s NED east velocity
int32_t velD; // cm/s NED down velocity
uint32_t speed; // cm/s Speed (3-D)
uint32_t gSpeed; // cm/s Ground Speed (2-D)
int32_t heading; // 1e-5 *deg Heading of motion 2-D
uint32_t sAcc; // cm/s Speed Accuracy Estimate
uint32_t cAcc; // 1e-5 *deg Course / Heading Accuracy Estimate
};
// UTC Time Solution
#define TIMEUTC_VALIDTOW (1 << 0)
#define TIMEUTC_VALIDWKN (1 << 1)
#define TIMEUTC_VALIDUTC (1 << 2)
struct UBX_NAV_TIMEUTC {
uint32_t iTOW; // GPS Millisecond Time of Week (ms)
uint32_t tAcc; // Time Accuracy Estimate (ns)
int32_t nano; // Nanoseconds of second
uint16_t year;
uint8_t month;
uint8_t day;
uint8_t hour;
uint8_t min;
uint8_t sec;
uint8_t valid; // Validity Flags
};
// Space Vehicle (SV) Information
// Single SV information block
#define SVUSED (1 << 0) // This SV is used for navigation
#define DIFFCORR (1 << 1) // Differential correction available
#define ORBITAVAIL (1 << 2) // Orbit information available
#define ORBITEPH (1 << 3) // Orbit information is Ephemeris
#define UNHEALTHY (1 << 4) // SV is unhealthy
#define ORBITALM (1 << 5) // Orbit information is Almanac Plus
#define ORBITAOP (1 << 6) // Orbit information is AssistNow Autonomous
#define SMOOTHED (1 << 7) // Carrier smoothed pseudoranges used
struct UBX_NAV_SVINFO_SV {
uint8_t chn; // Channel number
uint8_t svid; // Satellite ID
uint8_t flags; // Misc SV information
uint8_t quality; // Misc quality indicators
uint8_t cno; // Carrier to Noise Ratio (dbHz)
int8_t elev; // Elevation (integer degrees)
int16_t azim; // Azimuth (integer degrees)
int32_t prRes; // Pseudo range residual (cm)
};
// SV information message
#define MAX_SVS 16
struct UBX_NAV_SVINFO {
uint32_t iTOW; // GPS Millisecond Time of Week (ms)
uint8_t numCh; // Number of channels
uint8_t globalFlags; //
uint16_t reserved2; // Reserved
struct UBX_NAV_SVINFO_SV sv[MAX_SVS]; // Repeated 'numCh' times
};
typedef union {
uint8_t payload[0];
struct UBX_NAV_POSLLH nav_posllh;
struct UBX_NAV_STATUS nav_status;
struct UBX_NAV_DOP nav_dop;
struct UBX_NAV_SOL nav_sol;
struct UBX_NAV_VELNED nav_velned;
#if !defined(PIOS_GPS_MINIMAL)
struct UBX_NAV_TIMEUTC nav_timeutc;
struct UBX_NAV_SVINFO nav_svinfo;
#endif
} UBXPayload;
struct UBXHeader {
uint8_t class;
uint8_t id;
uint16_t len;
uint8_t ck_a;
uint8_t ck_b;
};
struct UBXPacket {
struct UBXHeader header;
UBXPayload payload;
};
bool checksum_ubx_message(struct UBXPacket *);
uint32_t parse_ubx_message(struct UBXPacket *, GPSPositionData *);
int parse_ubx_stream(uint8_t, char *, GPSPositionData *, struct GPS_RX_STATS *);
#endif /* UBX_H */

339
flight/Modules/RadioComBridge/RadioComBridge.c
View File

@ -61,8 +61,9 @@
#define PACKET_QUEUE_SIZE 10
#define MAX_PORT_DELAY 200
#define EV_PACKET_RECEIVED 0x20
#define EV_TRANSMIT_PACKET 0x30
#define EV_SEND_ACK 0x40
#define EV_SEND_NACK 0x80
#define EV_SEND_NACK 0x50
// ****************
// Private types
@ -79,22 +80,36 @@ typedef struct {
} PairStats;
typedef struct {
uint32_t comPort;
UAVTalkConnection UAVTalkCon;
xQueueHandle sendQueue;
xQueueHandle recvQueue;
xQueueHandle gcsQueue;
uint16_t wdg;
bool checkHID;
} UAVTalkComTaskParams;
typedef struct {
// The task handles.
xTaskHandle comUAVTalkTaskHandle;
xTaskHandle GCSUAVTalkRecvTaskHandle;
xTaskHandle UAVTalkRecvTaskHandle;
xTaskHandle radioReceiveTaskHandle;
xTaskHandle sendPacketTaskHandle;
xTaskHandle sendDataTaskHandle;
xTaskHandle UAVTalkSendTaskHandle;
xTaskHandle radioStatusTaskHandle;
xTaskHandle transparentCommTaskHandle;
xTaskHandle ppmInputTaskHandle;
// The UAVTalk connection on the com side.
UAVTalkConnection inUAVTalkCon;
UAVTalkConnection outUAVTalkCon;
UAVTalkConnection UAVTalkCon;
UAVTalkConnection GCSUAVTalkCon;
// Queue handles.
xQueueHandle sendPacketQueue;
xQueueHandle objEventQueue;
xQueueHandle radioPacketQueue;
xQueueHandle gcsEventQueue;
xQueueHandle uavtalkEventQueue;
xQueueHandle ppmOutQueue;
// Error statistics.
uint32_t comTxErrors;
@ -122,6 +137,10 @@ typedef struct {
// The RSSI of the last packet received.
int8_t RSSI;
// Thread parameters.
UAVTalkComTaskParams uavtalk_params;
UAVTalkComTaskParams gcs_uavtalk_params;
} RadioComBridgeData;
typedef struct {
@ -135,21 +154,24 @@ typedef struct {
// ****************
// Private functions
static void comUAVTalkTask(void *parameters);
static void UAVTalkRecvTask(void *parameters);
static void radioReceiveTask(void *parameters);
static void sendPacketTask(void *parameters);
static void sendDataTask(void *parameters);
static void UAVTalkSendTask(void *parameters);
static void transparentCommTask(void * parameters);
static void radioStatusTask(void *parameters);
static void ppmInputTask(void *parameters);
static int32_t transmitData(uint8_t * data, int32_t length);
static int32_t UAVTalkSendHandler(uint8_t * data, int32_t length);
static int32_t GCSUAVTalkSendHandler(uint8_t * data, int32_t length);
static int32_t transmitPacket(PHPacketHandle packet);
static void receiveData(uint8_t *buf, uint8_t len, int8_t rssi, int8_t afc);
static void transmitData(uint32_t outputPort, uint8_t *buf, uint8_t len, bool checkHid);
static void StatusHandler(PHStatusPacketHandle p, int8_t rssi, int8_t afc);
static void PPMHandler(uint16_t *channels);
static BufferedReadHandle BufferedReadInit(uint32_t com_port, uint16_t buffer_length);
static bool BufferedRead(BufferedReadHandle h, uint8_t *value, uint32_t timeout_ms);
static void BufferedReadSetCom(BufferedReadHandle h, uint32_t com_port);
static void queueEvent(xQueueHandle queue, void *obj, uint16_t instId, UAVObjEventType type);
static void updateSettings();
// ****************
@ -165,23 +187,34 @@ static RadioComBridgeData *data;
static int32_t RadioComBridgeStart(void)
{
if(data) {
// Start the primary tasks for receiving/sending UAVTalk packets from the GCS.
xTaskCreate(UAVTalkRecvTask, (signed char *)"GCSUAVTalkRecvTask", STACK_SIZE_BYTES, (void*)&(data->gcs_uavtalk_params), TASK_PRIORITY + 2, &(data->GCSUAVTalkRecvTaskHandle));
xTaskCreate(UAVTalkSendTask, (signed char *)"GCSUAVTalkSendTask", STACK_SIZE_BYTES, (void*)&(data->gcs_uavtalk_params), TASK_PRIORITY+ 2, &(data->UAVTalkSendTaskHandle));
// If a UAVTalk (non-GCS) com port is set it implies that the com port is connected on the flight side.
// In this case we want to start another com thread on the HID port to talk to the GCS when connected.
if (PIOS_COM_UAVTALK)
{
xTaskCreate(UAVTalkRecvTask, (signed char *)"UAVTalkRecvTask", STACK_SIZE_BYTES, (void*)&(data->uavtalk_params), TASK_PRIORITY + 2, &(data->UAVTalkRecvTaskHandle));
xTaskCreate(UAVTalkSendTask, (signed char *)"UAVTalkSendTask", STACK_SIZE_BYTES, (void*)&(data->uavtalk_params), TASK_PRIORITY+ 2, &(data->UAVTalkSendTaskHandle));
}
// Start the tasks
xTaskCreate(comUAVTalkTask, (signed char *)"ComUAVTalk", STACK_SIZE_BYTES, NULL, TASK_PRIORITY + 2, &(data->comUAVTalkTaskHandle));
if(PIOS_COM_TRANS_COM)
xTaskCreate(transparentCommTask, (signed char *)"transparentComm", STACK_SIZE_BYTES, NULL, TASK_PRIORITY + 2, &(data->transparentCommTaskHandle));
xTaskCreate(radioReceiveTask, (signed char *)"RadioReceive", STACK_SIZE_BYTES, NULL, TASK_PRIORITY+ 2, &(data->radioReceiveTaskHandle));
xTaskCreate(sendPacketTask, (signed char *)"SendPacketTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY + 2, &(data->sendPacketTaskHandle));
xTaskCreate(sendDataTask, (signed char *)"SendDataTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY+ 2, &(data->sendDataTaskHandle));
xTaskCreate(radioStatusTask, (signed char *)"RadioStatus", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->radioStatusTaskHandle));
if(PIOS_PPM_RECEIVER)
xTaskCreate(ppmInputTask, (signed char *)"PPMInputTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY + 2, &(data->ppmInputTaskHandle));
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_RegisterFlag(PIOS_WDG_COMUAVTALK);
PIOS_WDG_RegisterFlag(PIOS_WDG_COMGCS);
if(PIOS_COM_UAVTALK)
PIOS_WDG_RegisterFlag(PIOS_WDG_COMUAVTALK);
if(PIOS_COM_TRANS_COM)
PIOS_WDG_RegisterFlag(PIOS_WDG_TRANSCOMM);
PIOS_WDG_RegisterFlag(PIOS_WDG_RADIORECEIVE);
//PIOS_WDG_RegisterFlag(PIOS_WDG_SENDPACKET);
//PIOS_WDG_RegisterFlag(PIOS_WDG_SENDDATA);
if(PIOS_PPM_RECEIVER)
PIOS_WDG_RegisterFlag(PIOS_WDG_PPMINPUT);
#endif
@ -211,12 +244,21 @@ static int32_t RadioComBridgeInitialize(void)
updateSettings();
// Initialise UAVTalk
data->inUAVTalkCon = UAVTalkInitialize(0);
data->outUAVTalkCon = UAVTalkInitialize(&transmitData);
data->GCSUAVTalkCon = UAVTalkInitialize(&GCSUAVTalkSendHandler);
if (PIOS_COM_UAVTALK)
data->UAVTalkCon = UAVTalkInitialize(&UAVTalkSendHandler);
// Initialize the queues.
data->sendPacketQueue = xQueueCreate(PACKET_QUEUE_SIZE, sizeof(PHPacketHandle));
data->objEventQueue = xQueueCreate(PACKET_QUEUE_SIZE, sizeof(UAVObjEvent));
data->ppmOutQueue = 0;
data->radioPacketQueue = xQueueCreate(PACKET_QUEUE_SIZE, sizeof(UAVObjEvent));
data->gcsEventQueue = xQueueCreate(PACKET_QUEUE_SIZE, sizeof(UAVObjEvent));
if (PIOS_COM_UAVTALK)
data->uavtalkEventQueue = xQueueCreate(PACKET_QUEUE_SIZE, sizeof(UAVObjEvent));
else
{
data->uavtalkEventQueue = 0;
data->ppmOutQueue = data->radioPacketQueue;
}
// Initialize the statistics.
data->radioTxErrors = 0;
@ -232,8 +274,8 @@ static int32_t RadioComBridgeInitialize(void)
// Register the callbacks with the packet handler
PHRegisterOutputStream(pios_packet_handler, transmitPacket);
PHRegisterDataHandler(pios_packet_handler, receiveData);
PHRegisterStatusHandler(pios_packet_handler, StatusHandler);
PHRegisterPPMHandler(pios_packet_handler, PPMHandler);
PHRegisterStatusHandler(pios_packet_handler, StatusHandler);
// Initialize the packet send timeout
data->send_timeout = 25; // ms
@ -255,9 +297,28 @@ static int32_t RadioComBridgeInitialize(void)
PipXSettingsPairIDGet(&(data->pairStats[0].pairID));
// Configure our UAVObjects for updates.
UAVObjConnectQueue(UAVObjGetByName("PipXStatus"), data->objEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ);
UAVObjConnectQueue(UAVObjGetByName("GCSReceiver"), data->objEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ);
UAVObjConnectQueue(UAVObjGetByName("ObjectPersistence"), data->objEventQueue, EV_UPDATED | EV_UPDATED_MANUAL);
UAVObjConnectQueue(UAVObjGetByID(PIPXSTATUS_OBJID), data->gcsEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ);
UAVObjConnectQueue(UAVObjGetByID(GCSRECEIVER_OBJID), data->uavtalkEventQueue ? data->uavtalkEventQueue : data->gcsEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ);
UAVObjConnectQueue(UAVObjGetByID(OBJECTPERSISTENCE_OBJID), data->gcsEventQueue, EV_UPDATED | EV_UPDATED_MANUAL);
// Initialize the UAVTalk comm parameters.
data->gcs_uavtalk_params.UAVTalkCon = data->GCSUAVTalkCon;
data->gcs_uavtalk_params.sendQueue = data->radioPacketQueue;
data->gcs_uavtalk_params.recvQueue = data->gcsEventQueue;
data->gcs_uavtalk_params.wdg = PIOS_WDG_COMGCS;
data->gcs_uavtalk_params.checkHID = true;
data->gcs_uavtalk_params.comPort = PIOS_COM_GCS;
if (PIOS_COM_UAVTALK)
{
data->gcs_uavtalk_params.sendQueue = data->uavtalkEventQueue;
data->uavtalk_params.UAVTalkCon = data->UAVTalkCon;
data->uavtalk_params.sendQueue = data->radioPacketQueue;
data->uavtalk_params.recvQueue = data->uavtalkEventQueue;
data->uavtalk_params.gcsQueue = data->gcsEventQueue;
data->uavtalk_params.wdg = PIOS_WDG_COMUAVTALK;
data->uavtalk_params.checkHID = false;
data->uavtalk_params.comPort = PIOS_COM_UAVTALK;
}
return 0;
}
@ -266,30 +327,32 @@ MODULE_INITCALL(RadioComBridgeInitialize, RadioComBridgeStart)
/**
* Reads UAVTalk messages froma com port and creates packets out of them.
*/
static void comUAVTalkTask(void *parameters)
static void UAVTalkRecvTask(void *parameters)
{
UAVTalkComTaskParams *params = (UAVTalkComTaskParams *)parameters;
PHPacketHandle p = NULL;
// Create the buffered reader.
BufferedReadHandle f = BufferedReadInit(PIOS_COM_UAVTALK, TEMP_BUFFER_SIZE);
BufferedReadHandle f = BufferedReadInit(params->comPort, TEMP_BUFFER_SIZE);
while (1) {
#ifdef PIOS_INCLUDE_WDG
// Update the watchdog timer.
PIOS_WDG_UpdateFlag(PIOS_WDG_COMUAVTALK);
if (params->wdg)
PIOS_WDG_UpdateFlag(params->wdg);
#endif /* PIOS_INCLUDE_WDG */
// Receive from USB HID if available, otherwise UAVTalk com if it's available.
#if defined(PIOS_INCLUDE_USB)
// Determine input port (USB takes priority over telemetry port)
if (PIOS_USB_CheckAvailable(0))
if (params->checkHID && PIOS_USB_CheckAvailable(0))
BufferedReadSetCom(f, PIOS_COM_USB_HID);
else
#endif /* PIOS_INCLUDE_USB */
{
if (PIOS_COM_UAVTALK)
BufferedReadSetCom(f, PIOS_COM_UAVTALK);
if (params->comPort)
BufferedReadSetCom(f, params->comPort);
else
{
vTaskDelay(5);
@ -307,7 +370,7 @@ static void comUAVTalkTask(void *parameters)
{
// Wait until we receive a sync.
UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(data->inUAVTalkCon, rx_byte);
UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(params->UAVTalkCon, rx_byte);
if (state != UAVTALK_STATE_TYPE)
continue;
@ -324,7 +387,6 @@ static void comUAVTalkTask(void *parameters)
// Initialize the packet.
p->header.destination_id = data->destination_id;
p->header.source_id = PIOS_RFM22B_DeviceID(pios_rfm22b_id);
//p->header.type = PACKET_TYPE_ACKED_DATA;
p->header.type = PACKET_TYPE_DATA;
p->data[0] = rx_byte;
p->header.data_size = 1;
@ -335,12 +397,19 @@ static void comUAVTalkTask(void *parameters)
p->data[p->header.data_size++] = rx_byte;
// Keep reading until we receive a completed packet.
UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(data->inUAVTalkCon, rx_byte);
UAVTalkConnectionData *connection = (UAVTalkConnectionData*)(data->inUAVTalkCon);
UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(params->UAVTalkCon, rx_byte);
UAVTalkConnectionData *connection = (UAVTalkConnectionData*)(params->UAVTalkCon);
UAVTalkInputProcessor *iproc = &(connection->iproc);
if (state == UAVTALK_STATE_COMPLETE)
{
xQueueHandle sendQueue = params->sendQueue;
#if defined(PIOS_INCLUDE_USB)
if (params->gcsQueue)
if (PIOS_USB_CheckAvailable(0) && PIOS_COM_USB_HID)
sendQueue = params->gcsQueue;
#endif /* PIOS_INCLUDE_USB */
// Is this a local UAVObject?
// We only generate GcsReceiver ojects, we don't consume them.
if ((iproc->obj != NULL) && (iproc->objId != GCSRECEIVER_OBJID))
@ -359,11 +428,7 @@ static void comUAVTalkTask(void *parameters)
if (obj_per.ObjectID == PIPXSETTINGS_OBJID)
{
// Queue up the ACK.
UAVObjEvent ev;
ev.obj = iproc->obj;
ev.instId = iproc->instId;
ev.event = EV_SEND_ACK;
xQueueSend(data->objEventQueue, &ev, MAX_PORT_DELAY);
queueEvent(params->recvQueue, (void*)iproc->obj, iproc->instId, EV_SEND_ACK);
// Is this a save, load, or delete?
bool success = true;
@ -390,6 +455,19 @@ static void comUAVTalkTask(void *parameters)
int32_t ret = PIOS_EEPROM_Save((uint8_t*)&pipxSettings, sizeof(PipXSettingsData));
if (ret != 0)
success = false;
#endif
break;
}
case OBJECTPERSISTENCE_OPERATION_DELETE:
{
#if defined(PIOS_INCLUDE_FLASH_EEPROM)
// Erase the settings.
PipXSettingsData pipxSettings;
uint8_t *ptr = (uint8_t*)&pipxSettings;
memset(ptr, 0, sizeof(PipXSettingsData));
int32_t ret = PIOS_EEPROM_Save(ptr, sizeof(PipXSettingsData));
if (ret != 0)
success = false;
#endif
break;
}
@ -408,14 +486,11 @@ static void comUAVTalkTask(void *parameters)
else
{
// Otherwise, queue the packet for transmission.
xQueueSend(data->sendPacketQueue, &p, MAX_PORT_DELAY);
queueEvent(sendQueue, (void*)p, 0, EV_TRANSMIT_PACKET);
}
}
else
{
UAVObjEvent ev;
ev.obj = iproc->obj;
ev.instId = 0;
switch (iproc->type)
{
case UAVTALK_TYPE_OBJ:
@ -424,16 +499,12 @@ static void comUAVTalkTask(void *parameters)
break;
case UAVTALK_TYPE_OBJ_REQ:
// Queue up an object send request.
ev.event = EV_UPDATE_REQ;
xQueueSend(data->objEventQueue, &ev, MAX_PORT_DELAY);
queueEvent(params->recvQueue, (void*)iproc->obj, iproc->instId, EV_UPDATE_REQ);
break;
case UAVTALK_TYPE_OBJ_ACK:
if (UAVObjUnpack(iproc->obj, iproc->instId, connection->rxBuffer) == 0)
{
// Queue up an ACK
ev.event = EV_SEND_ACK;
xQueueSend(data->objEventQueue, &ev, MAX_PORT_DELAY);
}
queueEvent(params->recvQueue, (void*)iproc->obj, iproc->instId, EV_SEND_ACK);
break;
}
@ -444,22 +515,24 @@ static void comUAVTalkTask(void *parameters)
else
{
// Queue the packet for transmission.
xQueueSend(data->sendPacketQueue, &p, MAX_PORT_DELAY);
queueEvent(sendQueue, (void*)p, 0, EV_TRANSMIT_PACKET);
}
p = NULL;
} else if(state == UAVTALK_STATE_ERROR) {
DEBUG_PRINTF(1, "UAVTalk FAILED!\n\r");
xQueueHandle sendQueue = params->sendQueue;
#if defined(PIOS_INCLUDE_USB)
if (params->gcsQueue)
if (PIOS_USB_CheckAvailable(0) && PIOS_COM_USB_HID)
sendQueue = params->gcsQueue;
#endif /* PIOS_INCLUDE_USB */
data->UAVTalkErrors++;
// Send a NACK if required.
if((iproc->obj) && (iproc->type == UAVTALK_TYPE_OBJ_ACK))
{
// Queue up a NACK
UAVObjEvent ev;
ev.obj = iproc->obj;
ev.event = EV_SEND_NACK;
xQueueSend(data->objEventQueue, &ev, MAX_PORT_DELAY);
queueEvent(params->recvQueue, iproc->obj, iproc->instId, EV_SEND_NACK);
// Release the packet and start over again.
PHReleaseTXPacket(pios_packet_handler, p);
@ -467,7 +540,7 @@ static void comUAVTalkTask(void *parameters)
else
{
// Transmit the packet anyway...
xQueueSend(data->sendPacketQueue, &p, MAX_PORT_DELAY);
queueEvent(sendQueue, (void*)p, 0, EV_TRANSMIT_PACKET);
}
p = NULL;
}
@ -512,7 +585,7 @@ static void radioReceiveTask(void *parameters)
UAVObjEvent ev;
ev.obj = (UAVObjHandle)p;
ev.event = EV_PACKET_RECEIVED;
xQueueSend(data->objEventQueue, &ev, portMAX_DELAY);
xQueueSend(data->gcsEventQueue, &ev, portMAX_DELAY);
} else
{
data->packetErrors++;
@ -527,7 +600,7 @@ static void radioReceiveTask(void *parameters)
*/
static void sendPacketTask(void *parameters)
{
PHPacketHandle p;
UAVObjEvent ev;
// Loop forever
while (1) {
@ -536,19 +609,21 @@ static void sendPacketTask(void *parameters)
//PIOS_WDG_UpdateFlag(PIOS_WDG_SENDPACKET);
#endif /* PIOS_INCLUDE_WDG */
// Wait for a packet on the queue.
if (xQueueReceive(data->sendPacketQueue, &p, MAX_PORT_DELAY) == pdTRUE) {
if (xQueueReceive(data->radioPacketQueue, &ev, MAX_PORT_DELAY) == pdTRUE) {
PHPacketHandle p = (PHPacketHandle)ev.obj;
// Send the packet.
if(!PHTransmitPacket(pios_packet_handler, p))
PHReleaseTXPacket(pios_packet_handler, p);
PHReleaseRXPacket(pios_packet_handler, p);
}
}
}
/**
* Send packets to the radio.
* Send packets to the com port.
*/
static void sendDataTask(void *parameters)
static void UAVTalkSendTask(void *parameters)
{
UAVTalkComTaskParams *params = (UAVTalkComTaskParams *)parameters;
UAVObjEvent ev;
// Loop forever
@ -560,15 +635,16 @@ static void sendDataTask(void *parameters)
//PIOS_WDG_UpdateFlag(PIOS_WDG_SENDDATA);
#endif /* PIOS_INCLUDE_WDG */
// Wait for a packet on the queue.
if (xQueueReceive(data->objEventQueue, &ev, MAX_PORT_DELAY) == pdTRUE) {
if (xQueueReceive(params->recvQueue, &ev, MAX_PORT_DELAY) == pdTRUE) {
if ((ev.event == EV_UPDATED) || (ev.event == EV_UPDATE_REQ))
{
// Send update (with retries)
uint32_t retries = 0;
int32_t success = -1;
while (retries < MAX_RETRIES && success == -1) {
success = UAVTalkSendObject(data->outUAVTalkCon, ev.obj, 0, 0, RETRY_TIMEOUT_MS);
++retries;
success = UAVTalkSendObject(params->UAVTalkCon, ev.obj, 0, 0, RETRY_TIMEOUT_MS) == 0;
if (!success)
++retries;
}
data->comTxRetries += retries;
}
@ -578,8 +654,9 @@ static void sendDataTask(void *parameters)
uint32_t retries = 0;
int32_t success = -1;
while (retries < MAX_RETRIES && success == -1) {
success = UAVTalkSendAck(data->outUAVTalkCon, ev.obj, ev.instId);
++retries;
success = UAVTalkSendAck(params->UAVTalkCon, ev.obj, ev.instId) == 0;
if (!success)
++retries;
}
data->comTxRetries += retries;
}
@ -589,8 +666,9 @@ static void sendDataTask(void *parameters)
uint32_t retries = 0;
int32_t success = -1;
while (retries < MAX_RETRIES && success == -1) {
success = UAVTalkSendNack(data->outUAVTalkCon, UAVObjGetID(ev.obj));
++retries;
success = UAVTalkSendNack(params->UAVTalkCon, UAVObjGetID(ev.obj)) == 0;
if (!success)
++retries;
}
data->comTxRetries += retries;
}
@ -599,6 +677,13 @@ static void sendDataTask(void *parameters)
// Receive the packet.
PHReceivePacket(pios_packet_handler, (PHPacketHandle)ev.obj, false);
}
else if(ev.event == EV_TRANSMIT_PACKET)
{
// Transmit the packet.
PHPacketHandle p = (PHPacketHandle)ev.obj;
transmitData(params->comPort, p->data, p->header.data_size, params->checkHID);
PHReleaseTXPacket(pios_packet_handler, p);
}
}
}
}
@ -643,7 +728,7 @@ static void transparentCommTask(void * parameters)
}
// Receive data from the com port
uint32_t cur_rx_bytes = PIOS_COM_ReceiveBuffer(PIOS_COM_TRANS_COM, p->data + p->header.data_size,
uint32_t cur_rx_bytes = PIOS_COM_ReceiveBuffer(PIOS_COM_TRANS_COM, p->data + p->header.data_size,
PH_MAX_DATA - p->header.data_size, timeout);
// Do we have an data to send?
@ -675,7 +760,7 @@ static void transparentCommTask(void * parameters)
if (send_packet)
{
// Queue the packet for transmission.
xQueueSend(data->sendPacketQueue, &p, MAX_PORT_DELAY);
queueEvent(data->radioPacketQueue, (void*)p, 0, EV_TRANSMIT_PACKET);
// Reset the timeout
timeout = MAX_PORT_DELAY;
@ -767,7 +852,7 @@ static void radioStatusTask(void *parameters)
status_packet.dropped = data->droppedPackets;
status_packet.resets = PIOS_RFM22B_Resets(pios_rfm22b_id);
PHPacketHandle sph = (PHPacketHandle)&status_packet;
xQueueSend(data->sendPacketQueue, &sph, MAX_PORT_DELAY);
queueEvent(data->radioPacketQueue, (void*)sph, 0, EV_TRANSMIT_PACKET);
cntr = 0;
}
}
@ -792,22 +877,52 @@ static void ppmInputTask(void *parameters)
PIOS_WDG_UpdateFlag(PIOS_WDG_PPMINPUT);
#endif /* PIOS_INCLUDE_WDG */
// Send the PPM packet
// Read the receiver.
for (uint8_t i = 1; i <= PIOS_PPM_NUM_INPUTS; ++i)
ppm_packet.channels[i - 1] = PIOS_RCVR_Read(PIOS_PPM_RECEIVER, i);
// Send the packet.
ppm_packet.header.destination_id = data->destination_id;
ppm_packet.header.source_id = PIOS_RFM22B_DeviceID(pios_rfm22b_id);
ppm_packet.header.type = PACKET_TYPE_PPM;
ppm_packet.header.data_size = PH_PPM_DATA_SIZE(&ppm_packet);
xQueueSend(data->sendPacketQueue, &pph, MAX_PORT_DELAY);
// Send the PPM packet
if (data->ppmOutQueue)
{
ppm_packet.header.destination_id = data->destination_id;
ppm_packet.header.source_id = PIOS_RFM22B_DeviceID(pios_rfm22b_id);
ppm_packet.header.type = PACKET_TYPE_PPM;
ppm_packet.header.data_size = PH_PPM_DATA_SIZE(&ppm_packet);
queueEvent(data->ppmOutQueue, (void*)pph, 0, EV_TRANSMIT_PACKET);
}
else
PPMHandler(ppm_packet.channels);
// Delay until the next update period.
vTaskDelay(PIOS_PPM_PACKET_UPDATE_PERIOD_MS / portTICK_RATE_MS);
}
}
/**
* Transmit data buffer to the com port.
* \param[in] params The comm parameters.
* \param[in] buf Data buffer to send
* \param[in] length Length of buffer
* \return -1 on failure
* \return number of bytes transmitted on success
*/
static int32_t UAVTalkSend(UAVTalkComTaskParams *params, uint8_t *buf, int32_t length)
{
uint32_t outputPort = params->comPort;
#if defined(PIOS_INCLUDE_USB)
if (params->checkHID)
{
// Determine output port (USB takes priority over telemetry port)
if (PIOS_USB_CheckAvailable(0) && PIOS_COM_USB_HID)
outputPort = PIOS_COM_USB_HID;
}
#endif /* PIOS_INCLUDE_USB */
if(outputPort)
return PIOS_COM_SendBuffer(outputPort, buf, length);
else
return -1;
}
/**
* Transmit data buffer to the com port.
* \param[in] buf Data buffer to send
@ -815,18 +930,21 @@ static void ppmInputTask(void *parameters)
* \return -1 on failure
* \return number of bytes transmitted on success
*/
static int32_t transmitData(uint8_t *buf, int32_t length)
static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length)
{
uint32_t outputPort = PIOS_COM_UAVTALK;
#if defined(PIOS_INCLUDE_USB)
// Determine output port (USB takes priority over telemetry port)
if (PIOS_USB_CheckAvailable(0) && PIOS_COM_USB_HID)
outputPort = PIOS_COM_USB_HID;
#endif /* PIOS_INCLUDE_USB */
if(outputPort)
return PIOS_COM_SendBuffer(outputPort, buf, length);
else
return -1;
return UAVTalkSend(&(data->uavtalk_params), buf, length);
}
/**
* Transmit data buffer to the com port.
* \param[in] buf Data buffer to send
* \param[in] length Length of buffer
* \return -1 on failure
* \return number of bytes transmitted on success
*/
static int32_t GCSUAVTalkSendHandler(uint8_t *buf, int32_t length)
{
return UAVTalkSend(&(data->gcs_uavtalk_params), buf, length);
}
/**
@ -847,22 +965,15 @@ static int32_t transmitPacket(PHPacketHandle p)
* \param[in] buf The received data buffer
* \param[in] length Length of buffer
*/
static void receiveData(uint8_t *buf, uint8_t len, int8_t rssi, int8_t afc)
static void transmitData(uint32_t outputPort, uint8_t *buf, uint8_t len, bool checkHid)
{
data->RSSI = rssi;
// Packet data should go to transparent com if it's configured,
// USB HID if it's connected, otherwise, UAVTalk com if it's configured.
uint32_t outputPort = PIOS_COM_TRANS_COM;
if (!outputPort)
{
outputPort = PIOS_COM_UAVTALK;
#if defined(PIOS_INCLUDE_USB)
// See if USB is connected if requested.
if(checkHid)
// Determine output port (USB takes priority over telemetry port)
if (PIOS_USB_CheckAvailable(0) && PIOS_COM_USB_HID)
outputPort = PIOS_COM_USB_HID;
#endif /* PIOS_INCLUDE_USB */
}
if (!outputPort)
return;
@ -872,6 +983,22 @@ static void receiveData(uint8_t *buf, uint8_t len, int8_t rssi, int8_t afc)
data->comTxErrors++;
}
/**
* Receive a packet
* \param[in] buf The received data buffer
* \param[in] length Length of buffer
*/
static void receiveData(uint8_t *buf, uint8_t len, int8_t rssi, int8_t afc)
{
data->RSSI = rssi;
// Packet data should go to transparent com if it's configured,
// USB HID if it's connected, otherwise, UAVTalk com if it's configured.
uint32_t outputPort = PIOS_COM_TRANS_COM ? PIOS_COM_TRANS_COM : PIOS_COM_UAVTALK;
bool checkHid = (PIOS_COM_TRANS_COM == 0);
transmitData(outputPort, buf, len, checkHid);
}
/**
* Receive a status packet
* \param[in] status The status structure
@ -987,6 +1114,21 @@ static void BufferedReadSetCom(BufferedReadHandle h, uint32_t com_port)
h->com_port = com_port;
}
/**
* Queue and event into an event queue.
* \param[in] queue The event queue
* \param[in] obj The data pointer
* \param[in] type The event type
*/
static void queueEvent(xQueueHandle queue, void *obj, uint16_t instId, UAVObjEventType type)
{
UAVObjEvent ev;
ev.obj = (UAVObjHandle)obj;
ev.instId = instId;
ev.event = type;
xQueueSend(queue, &ev, portMAX_DELAY);
}
/**
* Update the telemetry settings, called on startup.
* FIXME: This should be in the TelemetrySettings object. But objects
@ -1003,7 +1145,6 @@ static void updateSettings()
// Initialize the destination ID
data->destination_id = pipxSettings.PairID ? pipxSettings.PairID : 0xffffffff;
DEBUG_PRINTF(2, "PairID: %x\n\r", data->destination_id);
if (PIOS_COM_TELEMETRY) {
switch (pipxSettings.TelemetrySpeed) {

2
flight/OSD/Makefile
View File

@ -153,6 +153,8 @@ SRC += $(OPUAVSYNTHDIR)/mixerstatus.c
SRC += $(OPUAVSYNTHDIR)/homelocation.c
SRC += $(OPUAVSYNTHDIR)/gpsposition.c
SRC += $(OPUAVSYNTHDIR)/gpssatellites.c
SRC += $(OPUAVSYNTHDIR)/gpssettings.c
SRC += $(OPUAVSYNTHDIR)/gpsvelocity.c
SRC += $(OPUAVSYNTHDIR)/gpstime.c
SRC += $(OPUAVSYNTHDIR)/osdsettings.c

2
flight/OSD/System/inc/pios_config.h
View File

@ -75,6 +75,8 @@
#define PIOS_TELEM_PRIORITY_QUEUE /* Enable a priority queue in telemetry */
#define PIOS_QUATERNION_STABILIZATION /* Stabilization options */
#define PIOS_GPS_SETS_HOMELOCATION /* GPS options */
#define PIOS_INCLUDE_GPS_NMEA_PARSER /* Include the NMEA protocol parser */
#define PIOS_INCLUDE_GPS_UBX_PARSER /* Include the UBX protocol parser */
/* Alarm Thresholds */
#define HEAP_LIMIT_WARNING 4000

12
flight/PiOS.posix/inc/pios_struct_helper.h Normal file
View File

@ -0,0 +1,12 @@
/* Taken from include/linux/kernel.h from the Linux kernel tree */
/**
* container_of - cast a member of a structure out to the containing structure
* @ptr: the pointer to the member.
* @type: the type of the container struct this is embedded in.
* @member: the name of the member within the struct.
*
*/
#define container_of(ptr, type, member) ({ \
const typeof( ((type *)0)->member ) *__mptr = (ptr); \
(type *)( (char *)__mptr - offsetof(type,member) );})

3
flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h
View File

@ -77,6 +77,7 @@ TIM4 | RC In 1 | Servo 3 | Servo 2 | Servo 1
#define PIOS_WDG_SENDDATA 0x0008
#define PIOS_WDG_TRANSCOMM 0x0010
#define PIOS_WDG_PPMINPUT 0x0020
#define PIOS_WDG_COMGCS 0x0040
//------------------------
// TELEMETRY
@ -157,6 +158,7 @@ extern uint32_t pios_com_telemetry_id;
extern uint32_t pios_com_flexi_id;
extern uint32_t pios_com_vcp_id;
extern uint32_t pios_com_uavtalk_com_id;
extern uint32_t pios_com_gcs_com_id;
extern uint32_t pios_com_trans_com_id;
extern uint32_t pios_com_debug_id;
extern uint32_t pios_com_rfm22b_id;
@ -166,6 +168,7 @@ extern uint32_t pios_ppm_rcvr_id;
#define PIOS_COM_FLEXI (pios_com_flexi_id)
#define PIOS_COM_VCP (pios_com_vcp_id)
#define PIOS_COM_UAVTALK (pios_com_uavtalk_com_id)
#define PIOS_COM_GCS (pios_com_gcs_com_id)
#define PIOS_COM_TRANS_COM (pios_com_trans_com_id)
#define PIOS_COM_DEBUG (pios_com_debug_id)
#define PIOS_COM_RADIO (pios_com_rfm22b_id)

6
flight/PiOS/STM32F10x/Libraries/CMSIS/Core/CM3/core_cm3.c
View File

@ -733,7 +733,7 @@ uint32_t __STREXB(uint8_t value, uint8_t *addr)
{
uint32_t result=0;
__ASM volatile ("strexb %0, %2, [%1]" : "=r" (result) : "r" (addr), "r" (value) );
__ASM volatile ("strexb %0, %2, [%1]" : "=&r" (result) : "r" (addr), "r" (value) );
return(result);
}
@ -750,7 +750,7 @@ uint32_t __STREXH(uint16_t value, uint16_t *addr)
{
uint32_t result=0;
__ASM volatile ("strexh %0, %2, [%1]" : "=r" (result) : "r" (addr), "r" (value) );
__ASM volatile ("strexh %0, %2, [%1]" : "=&r" (result) : "r" (addr), "r" (value) );
return(result);
}
@ -767,7 +767,7 @@ uint32_t __STREXW(uint32_t value, uint32_t *addr)
{
uint32_t result=0;
__ASM volatile ("strex %0, %2, [%1]" : "=r" (result) : "r" (addr), "r" (value) );
__ASM volatile ("strex %0, %2, [%1]" : "=&r" (result) : "r" (addr), "r" (value) );
return(result);
}

4
flight/PiOS/STM32F10x/link_STM32103CB_PIPXTREME_sections.ld
View File

@ -99,8 +99,8 @@ SECTIONS
} > SRAM
_eram = ORIGIN(SRAM) + LENGTH(SRAM) ;
_ebss = _eram;
_ebss = _eram ;
/* keep the heap section at the end of the SRAM
* this will allow to claim the remaining bytes not used
* at run time! (done by the reset vector).

12
flight/PiOS/inc/pios_struct_helper.h Normal file
View File

@ -0,0 +1,12 @@
/* Taken from include/linux/kernel.h from the Linux kernel tree */
/**
* container_of - cast a member of a structure out to the containing structure
* @ptr: the pointer to the member.
* @type: the type of the container struct this is embedded in.
* @member: the name of the member within the struct.
*
*/
#define container_of(ptr, type, member) ({ \
const typeof( ((type *)0)->member ) *__mptr = (ptr); \
(type *)( (char *)__mptr - offsetof(type,member) );})

40
flight/PipXtreme/System/pios_board.c
View File

@ -33,26 +33,27 @@
#include <pipxsettings.h>
#include <board_hw_defs.c>
#define PIOS_COM_SERIAL_RX_BUF_LEN 256
#define PIOS_COM_SERIAL_TX_BUF_LEN 256
#define PIOS_COM_SERIAL_RX_BUF_LEN 128
#define PIOS_COM_SERIAL_TX_BUF_LEN 128
#define PIOS_COM_FLEXI_RX_BUF_LEN 256
#define PIOS_COM_FLEXI_TX_BUF_LEN 256
#define PIOS_COM_FLEXI_RX_BUF_LEN 128
#define PIOS_COM_FLEXI_TX_BUF_LEN 128
#define PIOS_COM_TELEM_USB_RX_BUF_LEN 256
#define PIOS_COM_TELEM_USB_TX_BUF_LEN 256
#define PIOS_COM_TELEM_USB_RX_BUF_LEN 128
#define PIOS_COM_TELEM_USB_TX_BUF_LEN 128
#define PIOS_COM_VCP_USB_RX_BUF_LEN 256
#define PIOS_COM_VCP_USB_TX_BUF_LEN 256
#define PIOS_COM_VCP_USB_RX_BUF_LEN 128
#define PIOS_COM_VCP_USB_TX_BUF_LEN 128
#define PIOS_COM_RFM22B_RF_RX_BUF_LEN 256
#define PIOS_COM_RFM22B_RF_TX_BUF_LEN 256
#define PIOS_COM_RFM22B_RF_RX_BUF_LEN 128
#define PIOS_COM_RFM22B_RF_TX_BUF_LEN 128
uint32_t pios_com_telem_usb_id = 0;
uint32_t pios_com_telemetry_id;
uint32_t pios_com_flexi_id;
uint32_t pios_com_vcp_id;
uint32_t pios_com_uavtalk_com_id = 0;
uint32_t pios_com_gcs_com_id = 0;
uint32_t pios_com_trans_com_id = 0;
uint32_t pios_com_debug_id = 0;
uint32_t pios_com_rfm22b_id = 0;
@ -96,7 +97,7 @@ void PIOS_Board_Init(void) {
PipXSettingsData pipxSettings;
#if defined(PIOS_INCLUDE_FLASH_EEPROM)
PIOS_EEPROM_Init(&pios_eeprom_cfg);
PIOS_EEPROM_Init(&pios_eeprom_cfg);
/* Read the settings from flash. */
/* NOTE: We probably need to save/restore the objID here incase the object changed but the size doesn't */
@ -129,20 +130,17 @@ void PIOS_Board_Init(void) {
/* Flags to determine if various USB interfaces are advertised */
bool usb_hid_present = false;
bool usb_cdc_present = false;
#if defined(PIOS_INCLUDE_USB_CDC)
if (PIOS_USB_DESC_HID_CDC_Init()) {
PIOS_Assert(0);
}
usb_hid_present = true;
usb_cdc_present = true;
#else
if (PIOS_USB_DESC_HID_ONLY_Init()) {
PIOS_Assert(0);
}
usb_hid_present = true;
#endif
uint32_t pios_usb_id;
@ -172,7 +170,7 @@ void PIOS_Board_Init(void) {
tx_buffer, PIOS_COM_VCP_USB_TX_BUF_LEN)) {
PIOS_Assert(0);
}
switch (pipxSettings.TelemetryConfig)
switch (pipxSettings.VCPConfig)
{
case PIPXSETTINGS_VCPCONFIG_SERIAL:
pios_com_trans_com_id = pios_com_vcp_id;
@ -190,10 +188,6 @@ void PIOS_Board_Init(void) {
#if defined(PIOS_INCLUDE_USB_HID)
if (!usb_hid_present) {
PIOS_Assert(0);
}
/* Configure the usb HID port */
#if defined(PIOS_INCLUDE_COM)
{
@ -222,6 +216,7 @@ void PIOS_Board_Init(void) {
{
case PIPXSETTINGS_TELEMETRYCONFIG_SERIAL:
case PIPXSETTINGS_TELEMETRYCONFIG_UAVTALK:
case PIPXSETTINGS_TELEMETRYCONFIG_GCS:
case PIPXSETTINGS_TELEMETRYCONFIG_DEBUG:
{
uint32_t pios_usart1_id;
@ -245,6 +240,9 @@ void PIOS_Board_Init(void) {
case PIPXSETTINGS_TELEMETRYCONFIG_UAVTALK:
pios_com_uavtalk_com_id = pios_com_telemetry_id;
break;
case PIPXSETTINGS_TELEMETRYCONFIG_GCS:
pios_com_gcs_com_id = pios_com_telemetry_id;
break;
case PIPXSETTINGS_TELEMETRYCONFIG_DEBUG:
pios_com_debug_id = pios_com_telemetry_id;
break;
@ -260,6 +258,7 @@ void PIOS_Board_Init(void) {
{
case PIPXSETTINGS_FLEXICONFIG_SERIAL:
case PIPXSETTINGS_FLEXICONFIG_UAVTALK:
case PIPXSETTINGS_FLEXICONFIG_GCS:
case PIPXSETTINGS_FLEXICONFIG_DEBUG:
{
uint32_t pios_usart3_id;
@ -283,6 +282,9 @@ void PIOS_Board_Init(void) {
case PIPXSETTINGS_FLEXICONFIG_UAVTALK:
pios_com_uavtalk_com_id = pios_com_flexi_id;
break;
case PIPXSETTINGS_FLEXICONFIG_GCS:
pios_com_gcs_com_id = pios_com_flexi_id;
break;
case PIPXSETTINGS_FLEXICONFIG_DEBUG:
pios_com_debug_id = pios_com_flexi_id;
break;

1
flight/Project/gdb/simposix Normal file
View File

@ -0,0 +1 @@
handle SIGUSR1 noprint nostop

220
flight/Revolution/System/inc/pios_config.h
View File

@ -1,120 +1,122 @@
/**
******************************************************************************
* @addtogroup OpenPilotSystem OpenPilot System
* @{
* @addtogroup OpenPilotCore OpenPilot Core
* @{
*
* @file pios_config.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief PiOS configuration header.
* Central compile time config for the project.
* In particular, pios_config.h is where you define which PiOS libraries
* and features are included in the firmware.
* @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 PIOS_CONFIG_H
#define PIOS_CONFIG_H
/* Major features */
#define PIOS_INCLUDE_FREERTOS
#define PIOS_INCLUDE_BL_HELPER
/* Enable/Disable PiOS Modules */
/**
******************************************************************************
* @addtogroup OpenPilotSystem OpenPilot System
* @{
* @addtogroup OpenPilotCore OpenPilot Core
* @{
*
* @file pios_config.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief PiOS configuration header.
* Central compile time config for the project.
* In particular, pios_config.h is where you define which PiOS libraries
* and features are included in the firmware.
* @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 PIOS_CONFIG_H
#define PIOS_CONFIG_H
/* Major features */
#define PIOS_INCLUDE_FREERTOS
#define PIOS_INCLUDE_BL_HELPER
/* Enable/Disable PiOS Modules */
#define PIOS_INCLUDE_ADC
#define PIOS_INCLUDE_DELAY
#define PIOS_INCLUDE_I2C
#define PIOS_INCLUDE_IRQ
#define PIOS_INCLUDE_LED
#define PIOS_INCLUDE_IAP
#define PIOS_INCLUDE_SERVO
#define PIOS_INCLUDE_SPI
#define PIOS_INCLUDE_SYS
#define PIOS_INCLUDE_USART
#define PIOS_INCLUDE_USB
#define PIOS_INCLUDE_USB_HID
//#define PIOS_INCLUDE_GPIO
#define PIOS_INCLUDE_EXTI
#define PIOS_INCLUDE_RTC
#define PIOS_INCLUDE_WDG
/* Select the sensors to include */
#define PIOS_INCLUDE_BMA180
#define PIOS_INCLUDE_HMC5883
#define PIOS_INCLUDE_MPU6000
#define PIOS_MPU6000_ACCEL
#define PIOS_INCLUDE_L3GD20
#define PIOS_INCLUDE_MS5611
#define PIOS_INCLUDE_DELAY
#define PIOS_INCLUDE_I2C
#define PIOS_INCLUDE_IRQ
#define PIOS_INCLUDE_LED
#define PIOS_INCLUDE_IAP
#define PIOS_INCLUDE_SERVO
#define PIOS_INCLUDE_SPI
#define PIOS_INCLUDE_SYS
#define PIOS_INCLUDE_USART
#define PIOS_INCLUDE_USB
#define PIOS_INCLUDE_USB_HID
//#define PIOS_INCLUDE_GPIO
#define PIOS_INCLUDE_EXTI
#define PIOS_INCLUDE_RTC
#define PIOS_INCLUDE_WDG
/* Select the sensors to include */
#define PIOS_INCLUDE_BMA180
#define PIOS_INCLUDE_HMC5883
#define PIOS_INCLUDE_MPU6000
#define PIOS_MPU6000_ACCEL
#define PIOS_INCLUDE_L3GD20
#define PIOS_INCLUDE_MS5611
#define PIOS_INCLUDE_ETASV3
#define PIOS_INCLUDE_MPXV5004
#define PIOS_INCLUDE_MPXV7002
//#define PIOS_INCLUDE_HCSR04
#define PIOS_FLASH_ON_ACCEL /* true for second revo */
#define FLASH_FREERTOS
/* Com systems to include */
#define PIOS_INCLUDE_COM
#define PIOS_INCLUDE_COM_TELEM
#define PIOS_INCLUDE_COM_AUX
#define PIOS_INCLUDE_COM_AUXSBUS
#define PIOS_INCLUDE_COM_FLEXI
#define PIOS_INCLUDE_GPS
#define PIOS_OVERO_SPI
/* Supported receiver interfaces */
#define PIOS_INCLUDE_RCVR
#define PIOS_INCLUDE_DSM
//#define PIOS_INCLUDE_SBUS
#define PIOS_INCLUDE_PPM
#define PIOS_INCLUDE_PWM
//#define PIOS_INCLUDE_GCSRCVR
#define PIOS_INCLUDE_SETTINGS
#define PIOS_INCLUDE_FLASH
/* A really shitty setting saving implementation */
#define PIOS_INCLUDE_FLASH_SECTOR_SETTINGS
/* Other Interfaces */
//#define PIOS_INCLUDE_I2C_ESC
/* Flags that alter behaviors - mostly to lower resources for CC */
#define PIOS_INCLUDE_INITCALL /* Include init call structures */
#define PIOS_TELEM_PRIORITY_QUEUE /* Enable a priority queue in telemetry */
//#define PIOS_INCLUDE_HCSR04
#define PIOS_FLASH_ON_ACCEL /* true for second revo */
#define FLASH_FREERTOS
/* Com systems to include */
#define PIOS_INCLUDE_COM
#define PIOS_INCLUDE_COM_TELEM
#define PIOS_INCLUDE_COM_AUX
#define PIOS_INCLUDE_COM_AUXSBUS
#define PIOS_INCLUDE_COM_FLEXI
#define PIOS_INCLUDE_GPS
#define PIOS_OVERO_SPI
/* Supported receiver interfaces */
#define PIOS_INCLUDE_RCVR
#define PIOS_INCLUDE_DSM
//#define PIOS_INCLUDE_SBUS
#define PIOS_INCLUDE_PPM
#define PIOS_INCLUDE_PWM
//#define PIOS_INCLUDE_GCSRCVR
#define PIOS_INCLUDE_SETTINGS
#define PIOS_INCLUDE_FLASH
/* A really shitty setting saving implementation */
#define PIOS_INCLUDE_FLASH_SECTOR_SETTINGS
/* Other Interfaces */
//#define PIOS_INCLUDE_I2C_ESC
/* Flags that alter behaviors - mostly to lower resources for CC */
#define PIOS_INCLUDE_INITCALL /* Include init call structures */
#define PIOS_TELEM_PRIORITY_QUEUE /* Enable a priority queue in telemetry */
//#define PIOS_QUATERNION_STABILIZATION /* Stabilization options */
#define PIOS_GPS_SETS_HOMELOCATION /* GPS options */
/* Alarm Thresholds */
#define PIOS_GPS_SETS_HOMELOCATION /* GPS options */
#define PIOS_INCLUDE_GPS_NMEA_PARSER /* Include the NMEA protocol parser */
#define PIOS_INCLUDE_GPS_UBX_PARSER /* Include the UBX protocol parser */
/* Alarm Thresholds */
#define HEAP_LIMIT_WARNING 1000
#define HEAP_LIMIT_CRITICAL 500
#define IRQSTACK_LIMIT_WARNING 150
#define IRQSTACK_LIMIT_CRITICAL 80
#define CPULOAD_LIMIT_WARNING 80
#define CPULOAD_LIMIT_CRITICAL 95
// This actually needs calibrating
#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD (8379692)
#define REVOLUTION
#endif /* PIOS_CONFIG_H */
/**
* @}
* @}
*/
// This actually needs calibrating
#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD (8379692)
#define REVOLUTION
#endif /* PIOS_CONFIG_H */
/**
* @}
* @}
*/

1
flight/Revolution/UAVObjects.inc
View File

@ -48,6 +48,7 @@ UAVOBJSRCFILENAMES += flighttelemetrystats
UAVOBJSRCFILENAMES += gcstelemetrystats
UAVOBJSRCFILENAMES += gpsposition
UAVOBJSRCFILENAMES += gpssatellites
UAVOBJSRCFILENAMES += gpssettings
UAVOBJSRCFILENAMES += gpstime
UAVOBJSRCFILENAMES += gpsvelocity
UAVOBJSRCFILENAMES += fixedwingpathfollowersettings

2
flight/SimPosix/Makefile
View File

@ -38,7 +38,7 @@ OUTDIR := $(TOP)/build/$(TARGET)
# Set developer code and compile options
# Set to YES for debugging
DEBUG ?= NO
DEBUG ?= YES
# Set to YES when using Code Sourcery toolchain
CODE_SOURCERY ?= NO

1
flight/SimPosix/UAVObjects.inc
View File

@ -48,6 +48,7 @@ UAVOBJSRCFILENAMES += gcstelemetrystats
UAVOBJSRCFILENAMES += gpsposition
UAVOBJSRCFILENAMES += gpssatellites
UAVOBJSRCFILENAMES += gpstime
UAVOBJSRCFILENAMES += gpssettings
UAVOBJSRCFILENAMES += gpsvelocity
UAVOBJSRCFILENAMES += vtolpathfollowersettings
UAVOBJSRCFILENAMES += fixedwingpathfollowersettings

59
flight/UAVObjects/inc/uavobjectmanager.h
View File

@ -133,7 +133,7 @@ typedef void (*UAVObjEventCallback)(UAVObjEvent* ev);
/**
* Callback used to initialize the object fields to their default values.
*/
typedef void (*UAVObjInitializeCallback)(UAVObjHandle obj, uint16_t instId);
typedef void (*UAVObjInitializeCallback)(UAVObjHandle obj_handle, uint16_t instId);
/**
* Event manager statistics
@ -148,25 +148,23 @@ typedef struct {
int32_t UAVObjInitialize();
void UAVObjGetStats(UAVObjStats* statsOut);
void UAVObjClearStats();
UAVObjHandle UAVObjRegister(uint32_t id, const char* name, const char* metaName, int32_t isMetaobject,
UAVObjHandle UAVObjRegister(uint32_t id,
int32_t isSingleInstance, int32_t isSettings, uint32_t numBytes, UAVObjInitializeCallback initCb);
UAVObjHandle UAVObjGetByID(uint32_t id);
UAVObjHandle UAVObjGetByName(char* name);
uint32_t UAVObjGetID(UAVObjHandle obj);
const char* UAVObjGetName(UAVObjHandle obj);
uint32_t UAVObjGetNumBytes(UAVObjHandle obj);
uint16_t UAVObjGetNumInstances(UAVObjHandle obj);
UAVObjHandle UAVObjGetLinkedObj(UAVObjHandle obj);
uint16_t UAVObjCreateInstance(UAVObjHandle obj, UAVObjInitializeCallback initCb);
int32_t UAVObjIsSingleInstance(UAVObjHandle obj);
int32_t UAVObjIsMetaobject(UAVObjHandle obj);
int32_t UAVObjIsSettings(UAVObjHandle obj);
int32_t UAVObjUnpack(UAVObjHandle obj, uint16_t instId, const uint8_t* dataIn);
int32_t UAVObjPack(UAVObjHandle obj, uint16_t instId, uint8_t* dataOut);
int32_t UAVObjSave(UAVObjHandle obj, uint16_t instId);
int32_t UAVObjLoad(UAVObjHandle obj, uint16_t instId);
int32_t UAVObjDelete(UAVObjHandle obj, uint16_t instId);
int32_t UAVObjSaveToFile(UAVObjHandle obj, uint16_t instId, FILEINFO* file);
uint16_t UAVObjCreateInstance(UAVObjHandle obj_handle, UAVObjInitializeCallback initCb);
bool UAVObjIsSingleInstance(UAVObjHandle obj);
bool UAVObjIsMetaobject(UAVObjHandle obj);
bool UAVObjIsSettings(UAVObjHandle obj);
int32_t UAVObjUnpack(UAVObjHandle obj_handle, uint16_t instId, const uint8_t* dataIn);
int32_t UAVObjPack(UAVObjHandle obj_handle, uint16_t instId, uint8_t* dataOut);
int32_t UAVObjSave(UAVObjHandle obj_handle, uint16_t instId);
int32_t UAVObjLoad(UAVObjHandle obj_handle, uint16_t instId);
int32_t UAVObjDelete(UAVObjHandle obj_handle, uint16_t instId);
int32_t UAVObjSaveToFile(UAVObjHandle obj_handle, uint16_t instId, FILEINFO* file);
UAVObjHandle UAVObjLoadFromFile(FILEINFO* file);
int32_t UAVObjSaveSettings();
int32_t UAVObjLoadSettings();
@ -174,18 +172,17 @@ int32_t UAVObjDeleteSettings();
int32_t UAVObjSaveMetaobjects();
int32_t UAVObjLoadMetaobjects();
int32_t UAVObjDeleteMetaobjects();
int32_t UAVObjSetData(UAVObjHandle obj, const void* dataIn);
int32_t UAVObjSetDataField(UAVObjHandle obj, const void* dataIn, uint32_t offset, uint32_t size);
int32_t UAVObjGetData(UAVObjHandle obj, void* dataOut);
int32_t UAVObjGetDataField(UAVObjHandle obj, void* dataOut, uint32_t offset, uint32_t size);
int32_t UAVObjSetInstanceData(UAVObjHandle obj, uint16_t instId, const void* dataIn);
int32_t UAVObjSetInstanceDataField(UAVObjHandle obj, uint16_t instId, const void* dataIn, uint32_t offset, uint32_t size);
int32_t UAVObjGetInstanceData(UAVObjHandle obj, uint16_t instId, void* dataOut);
int32_t UAVObjGetInstanceDataField(UAVObjHandle obj, uint16_t instId, void* dataOut, uint32_t offset, uint32_t size);
int32_t UAVObjSetMetadata(UAVObjHandle obj, const UAVObjMetadata* dataIn);
int32_t UAVObjGetMetadata(UAVObjHandle obj, UAVObjMetadata* dataOut);
int32_t UAVObjSetData(UAVObjHandle obj_handle, const void* dataIn);
int32_t UAVObjSetDataField(UAVObjHandle obj_handle, const void* dataIn, uint32_t offset, uint32_t size);
int32_t UAVObjGetData(UAVObjHandle obj_handle, void* dataOut);
int32_t UAVObjGetDataField(UAVObjHandle obj_handle, void* dataOut, uint32_t offset, uint32_t size);
int32_t UAVObjSetInstanceData(UAVObjHandle obj_handle, uint16_t instId, const void* dataIn);
int32_t UAVObjSetInstanceDataField(UAVObjHandle obj_handle, uint16_t instId, const void* dataIn, uint32_t offset, uint32_t size);
int32_t UAVObjGetInstanceData(UAVObjHandle obj_handle, uint16_t instId, void* dataOut);
int32_t UAVObjGetInstanceDataField(UAVObjHandle obj_handle, uint16_t instId, void* dataOut, uint32_t offset, uint32_t size);
int32_t UAVObjSetMetadata(UAVObjHandle obj_handle, const UAVObjMetadata* dataIn);
int32_t UAVObjGetMetadata(UAVObjHandle obj_handle, UAVObjMetadata* dataOut);
uint8_t UAVObjGetMetadataAccess(const UAVObjMetadata* dataOut);
void UAVObjMetadataInitialize(UAVObjMetadata* dataOut);
UAVObjAccessType UAVObjGetAccess(const UAVObjMetadata* dataOut);
void UAVObjSetAccess(UAVObjMetadata* dataOut, UAVObjAccessType mode);
UAVObjAccessType UAVObjGetGcsAccess(const UAVObjMetadata* dataOut);
@ -199,14 +196,14 @@ void UAVObjSetTelemetryUpdateMode(UAVObjMetadata* dataOut, UAVObjUpdateMode val)
UAVObjUpdateMode UAVObjGetGcsTelemetryUpdateMode(const UAVObjMetadata* dataOut);
void UAVObjSetTelemetryGcsUpdateMode(UAVObjMetadata* dataOut, UAVObjUpdateMode val);
int8_t UAVObjReadOnly(UAVObjHandle obj);
int32_t UAVObjConnectQueue(UAVObjHandle obj, xQueueHandle queue, uint8_t eventMask);
int32_t UAVObjDisconnectQueue(UAVObjHandle obj, xQueueHandle queue);
int32_t UAVObjConnectCallback(UAVObjHandle obj, UAVObjEventCallback cb, uint8_t eventMask);
int32_t UAVObjDisconnectCallback(UAVObjHandle obj, UAVObjEventCallback cb);
int32_t UAVObjConnectQueue(UAVObjHandle obj_handle, xQueueHandle queue, uint8_t eventMask);
int32_t UAVObjDisconnectQueue(UAVObjHandle obj_handle, xQueueHandle queue);
int32_t UAVObjConnectCallback(UAVObjHandle obj_handle, UAVObjEventCallback cb, uint8_t eventMask);
int32_t UAVObjDisconnectCallback(UAVObjHandle obj_handle, UAVObjEventCallback cb);
void UAVObjRequestUpdate(UAVObjHandle obj);
void UAVObjRequestInstanceUpdate(UAVObjHandle obj, uint16_t instId);
void UAVObjRequestInstanceUpdate(UAVObjHandle obj_handle, uint16_t instId);
void UAVObjUpdated(UAVObjHandle obj);
void UAVObjInstanceUpdated(UAVObjHandle obj, uint16_t instId);
void UAVObjInstanceUpdated(UAVObjHandle obj_handle, uint16_t instId);
void UAVObjIterate(void (*iterator)(UAVObjHandle obj));
#endif // UAVOBJECTMANAGER_H

2
flight/UAVObjects/inc/uavobjecttemplate.h
View File

@ -42,8 +42,6 @@
// Object constants
#define $(NAMEUC)_OBJID $(OBJIDHEX)
#define $(NAMEUC)_NAME "$(NAME)"
#define $(NAMEUC)_METANAME "$(NAME)Meta"
#define $(NAMEUC)_ISSINGLEINST $(ISSINGLEINST)
#define $(NAMEUC)_ISSETTINGS $(ISSETTINGS)
#define $(NAMEUC)_NUMBYTES sizeof($(NAME)Data)

2387
flight/UAVObjects/uavobjectmanager.c
View File

@ -1,20 +1,20 @@
/**
******************************************************************************
* @addtogroup UAVObjects OpenPilot UAVObjects
* @{
* @addtogroup UAV Object Manager
* @brief The core UAV Objects functions, most of which are wrappered by
* autogenerated defines
* @{
*
*
* @file uavobjectmanager.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Object manager library. This library holds a collection of all objects.
* It can be used by all modules/libraries to find an object reference.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
******************************************************************************
* @addtogroup UAVObjects OpenPilot UAVObjects
* @{
* @addtogroup UAV Object Manager
* @brief The core UAV Objects functions, most of which are wrappered by
* autogenerated defines
* @{
*
*
* @file uavobjectmanager.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Object manager library. This library holds a collection of all objects.
* It can be used by all modules/libraries to find an object reference.
* @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
@ -32,6 +32,7 @@
*/
#include "openpilot.h"
#include "pios_struct_helper.h"
// Constants
@ -39,84 +40,144 @@
// Macros
#define SET_BITS(var, shift, value, mask) var = (var & ~(mask << shift)) | (value << shift);
#define OLGetIsMetaobject(olp) ((olp)->flags & OL_IS_METAOBJECT)
#define OLSetIsMetaobject(olp, val) ((olp)->flags = (((val) == 0) ? ((olp)->flags & ~OL_IS_METAOBJECT) : ((olp)->flags | OL_IS_METAOBJECT)))
#define OLGetIsSingleInstance(olp) ((olp)->flags & OL_IS_SINGLE_INSTANCE)
#define OLSetIsSingleInstance(olp, val) ((olp)->flags = (((val) == 0) ? ((olp)->flags & ~OL_IS_SINGLE_INSTANCE) : ((olp)->flags | OL_IS_SINGLE_INSTANCE)))
#define OLGetIsSettings(olp) ((olp)->flags & OL_IS_SETTINGS)
#define OLSetIsSettings(olp, val) ((olp)->flags = (((val) == 0) ? ((olp)->flags & ~OL_IS_SETTINGS) : ((olp)->flags | OL_IS_SETTINGS)))
/**
* List of event queues and the eventmask associated with the queue.
*/
struct ObjectEventListStruct {
xQueueHandle queue;
UAVObjEventCallback cb;
uint8_t eventMask;
struct ObjectEventListStruct *next;
};
typedef struct ObjectEventListStruct ObjectEventList;
/**
* List of object instances, holds the actual data structure and instance ID
*/
struct ObjectInstListStruct {
void *data;
uint16_t instId;
struct ObjectInstListStruct *next;
};
typedef struct ObjectInstListStruct ObjectInstList;
/** opaque type for instances **/
typedef void* InstanceHandle;
typedef enum {
OL_IS_METAOBJECT = 0x01, /** Set if this is a metaobject */
OL_IS_SINGLE_INSTANCE = 0x02, /** Set if this object has a single instance */
OL_IS_SETTINGS = 0x04 /** Set if this object is a settings object */
} ObjectListFlags;
/**
* List of objects registered in the object manager
*/
struct ObjectListStruct {
uint32_t id;
/** The object ID */
const char *name;
/** The object name */
ObjectListFlags flags;
/** The object list mode flags */
uint16_t numBytes;
/** Number of data bytes contained in the object (for a single instance) */
uint16_t numInstances;
/** Number of instances */
struct ObjectListStruct *linkedObj;
/** Linked object, for regular objects this is the metaobject and for metaobjects it is the parent object */
ObjectInstList instances;
/** List of object instances, instance 0 always exists */
ObjectEventList *events;
/** Event queues registered on the object */
struct ObjectListStruct *next;
/** Needed by linked list library (utlist.h) */
struct ObjectEventEntry {
xQueueHandle queue;
UAVObjEventCallback cb;
uint8_t eventMask;
struct ObjectEventEntry * next;
};
typedef struct ObjectListStruct ObjectList;
/*
MetaInstance == [UAVOBase [UAVObjMetadata]]
SingleInstance == [UAVOBase [UAVOData [InstanceData]]]
MultiInstance == [UAVOBase [UAVOData [NumInstances [InstanceData0 [next]]]]
____________________/
\-->[InstanceData1 [next]]
_________...________/
\-->[InstanceDataN [next]]
*/
/*
* UAVO Base Type
* - All Types of UAVObjects are of this base type
* - The flags determine what type(s) this object
*/
struct UAVOBase {
/* Let these objects be added to an event queue */
struct ObjectEventEntry * next_event;
/* Describe the type of object that follows this header */
struct UAVOInfo {
bool isMeta : 1;
bool isSingle : 1;
bool isSettings : 1;
} flags;
} __attribute__((packed));
/* Augmented type for Meta UAVO */
struct UAVOMeta {
struct UAVOBase base;
UAVObjMetadata instance0;
} __attribute__((packed));
/* Shared data structure for all data-carrying UAVObjects (UAVOSingle and UAVOMulti) */
struct UAVOData {
struct UAVOBase base;
uint32_t id;
/*
* Embed the Meta object as another complete UAVO
* inside the payload for this UAVO.
*/
struct UAVOMeta metaObj;
struct UAVOData * next;
uint16_t instance_size;
} __attribute__((packed));
/* Augmented type for Single Instance Data UAVO */
struct UAVOSingle {
struct UAVOData uavo;
uint8_t instance0[];
/*
* Additional space will be malloc'd here to hold the
* the data for this instance.
*/
} __attribute__((packed));
/* Part of a linked list of instances chained off of a multi instance UAVO. */
struct UAVOMultiInst {
struct UAVOMultiInst * next;
uint8_t instance[];
/*
* Additional space will be malloc'd here to hold the
* the data for this instance.
*/
} __attribute__((packed));
/* Augmented type for Multi Instance Data UAVO */
struct UAVOMulti {
struct UAVOData uavo;
uint16_t num_instances;
struct UAVOMultiInst instance0;
/*
* Additional space will be malloc'd here to hold the
* the data for instance 0.
*/
} __attribute__((packed));
/** all information about a metaobject are hardcoded constants **/
#define MetaNumBytes sizeof(UAVObjMetadata)
#define MetaBaseObjectPtr(obj) ((struct UAVOData *)((obj)-offsetof(struct UAVOData, metaObj)))
#define MetaObjectPtr(obj) ((struct UAVODataMeta*) &((obj)->metaObj))
#define MetaDataPtr(obj) ((UAVObjMetadata*)&((obj)->instance0))
#define LinkedMetaDataPtr(obj) ((UAVObjMetadata*)&((obj)->metaObj.instance0))
#define MetaObjectId(id) ((id)+1)
/** all information about instances are dependant on object type **/
#define ObjSingleInstanceDataOffset(obj) ((void*)(&(( (struct UAVOSingle*)obj )->instance0)))
#define InstanceDataOffset(inst) ((void*)&(( (struct UAVOMultiInst*)inst )->instance))
#define InstanceData(instance) (void*)instance
// Private functions
static int32_t sendEvent(ObjectList * obj, uint16_t instId,
UAVObjEventType event);
static ObjectInstList *createInstance(ObjectList * obj, uint16_t instId);
static ObjectInstList *getInstance(ObjectList * obj, uint16_t instId);
static int32_t connectObj(UAVObjHandle obj, xQueueHandle queue,
UAVObjEventCallback cb, uint8_t eventMask);
static int32_t disconnectObj(UAVObjHandle obj, xQueueHandle queue,
UAVObjEventCallback cb);
static int32_t sendEvent(struct UAVOBase * obj, uint16_t instId,
UAVObjEventType event);
static InstanceHandle createInstance(struct UAVOData * obj, uint16_t instId);
static InstanceHandle getInstance(struct UAVOData * obj, uint16_t instId);
static int32_t connectObj(UAVObjHandle obj_handle, xQueueHandle queue,
UAVObjEventCallback cb, uint8_t eventMask);
static int32_t disconnectObj(UAVObjHandle obj_handle, xQueueHandle queue,
UAVObjEventCallback cb);
#if defined(PIOS_INCLUDE_SDCARD)
static void objectFilename(ObjectList * obj, uint8_t * filename);
static void objectFilename(UAVObjHandle obj_handle, uint8_t * filename);
static void customSPrintf(uint8_t * buffer, uint8_t * format, ...);
#endif
// Private variables
static ObjectList *objList;
static struct UAVOData * uavo_list;
static xSemaphoreHandle mutex;
static UAVObjMetadata defMetadata;
static const UAVObjMetadata defMetadata = {
.flags = (ACCESS_READWRITE << UAVOBJ_ACCESS_SHIFT |
ACCESS_READWRITE << UAVOBJ_GCS_ACCESS_SHIFT |
1 << UAVOBJ_TELEMETRY_ACKED_SHIFT |
1 << UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT |
UPDATEMODE_ONCHANGE << UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT |
UPDATEMODE_ONCHANGE << UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT),
.telemetryUpdatePeriod = 0,
.gcsTelemetryUpdatePeriod = 0,
.loggingUpdatePeriod = 0,
};
static UAVObjStats stats;
/**
@ -126,31 +187,32 @@ static UAVObjStats stats;
*/
int32_t UAVObjInitialize()
{
// Initialize variables
objList = NULL;
memset(&stats, 0, sizeof(UAVObjStats));
// Initialize variables
uavo_list = NULL;
memset(&stats, 0, sizeof(UAVObjStats));
// Create mutex
mutex = xSemaphoreCreateRecursiveMutex();
if (mutex == NULL)
return -1;
// Create mutex
mutex = xSemaphoreCreateRecursiveMutex();
if (mutex == NULL)
return -1;
// Initialize default metadata structure (metadata of metaobjects)
UAVObjMetadataInitialize(&defMetadata);
// Done
return 0;
// Done
return 0;
}
/*****************
* Statistics
****************/
/**
* Get the statistics counters
* @param[out] statsOut The statistics counters will be copied there
*/
void UAVObjGetStats(UAVObjStats * statsOut)
{
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
memcpy(statsOut, &stats, sizeof(UAVObjStats));
xSemaphoreGiveRecursive(mutex);
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
memcpy(statsOut, &stats, sizeof(UAVObjStats));
xSemaphoreGiveRecursive(mutex);
}
/**
@ -158,17 +220,84 @@ void UAVObjGetStats(UAVObjStats * statsOut)
*/
void UAVObjClearStats()
{
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
memset(&stats, 0, sizeof(UAVObjStats));
xSemaphoreGiveRecursive(mutex);
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
memset(&stats, 0, sizeof(UAVObjStats));
xSemaphoreGiveRecursive(mutex);
}
/************************
* Object Initialization
***********************/
static void UAVObjInitMetaData (struct UAVOMeta * obj_meta)
{
/* Fill in the common part of the UAVO */
struct UAVOBase * uavo_base = &(obj_meta->base);
memset(uavo_base, 0, sizeof(*uavo_base));
uavo_base->flags.isMeta = true;
uavo_base->flags.isSingle = true;
uavo_base->next_event = NULL;
/* Clear the instance data carried in the UAVO */
memset(&(obj_meta->instance0), 0, sizeof(obj_meta->instance0));
}
static struct UAVOData * UAVObjAllocSingle(uint32_t num_bytes)
{
/* Compute the complete size of the object, including the data for a single embedded instance */
uint32_t object_size = sizeof(struct UAVOSingle) + num_bytes;
/* Allocate the object from the heap */
struct UAVOSingle * uavo_single = (struct UAVOSingle *) pvPortMalloc(object_size);
if (!uavo_single)
return (NULL);
/* Fill in the common part of the UAVO */
struct UAVOBase * uavo_base = &(uavo_single->uavo.base);
memset(uavo_base, 0, sizeof(*uavo_base));
uavo_base->flags.isSingle = true;
uavo_base->next_event = NULL;
/* Clear the instance data carried in the UAVO */
memset(&(uavo_single->instance0), 0, num_bytes);
/* Give back the generic UAVO part */
return (&(uavo_single->uavo));
}
static struct UAVOData * UAVObjAllocMulti(uint32_t num_bytes)
{
/* Compute the complete size of the object, including the data for a single embedded instance */
uint32_t object_size = sizeof(struct UAVOMulti) + num_bytes;
/* Allocate the object from the heap */
struct UAVOMulti * uavo_multi = (struct UAVOMulti *) pvPortMalloc(object_size);
if (!uavo_multi)
return (NULL);
/* Fill in the common part of the UAVO */
struct UAVOBase * uavo_base = &(uavo_multi->uavo.base);
memset(uavo_base, 0, sizeof(*uavo_base));
uavo_base->flags.isSingle = false;
uavo_base->next_event = NULL;
/* Set up the type-specific part of the UAVO */
uavo_multi->num_instances = 1;
/* Clear the instance data carried in the UAVO */
memset (&(uavo_multi->instance0), 0, num_bytes);
/* Give back the generic UAVO part */
return (&(uavo_multi->uavo));
}
/**************************
* UAVObject Database APIs
*************************/
/**
* Register and new object in the object manager.
* \param[in] id Unique object ID
* \param[in] name Object name
* \param[in] nameName Metaobject name
* \param[in] isMetaobject Is this a metaobject (1:true, 0:false)
* \param[in] isSingleInstance Is this a single instance or multi-instance object
* \param[in] isSettings Is this a settings object
* \param[in] numBytes Number of bytes of object data (for one instance)
@ -176,85 +305,60 @@ void UAVObjClearStats()
* \return Object handle, or NULL if failure.
* \return
*/
UAVObjHandle UAVObjRegister(uint32_t id, const char *name,
const char *metaName, int32_t isMetaobject,
int32_t isSingleInstance, int32_t isSettings,
uint32_t numBytes,
UAVObjInitializeCallback initCb)
UAVObjHandle UAVObjRegister(uint32_t id,
int32_t isSingleInstance, int32_t isSettings,
uint32_t num_bytes,
UAVObjInitializeCallback initCb)
{
ObjectList *objEntry;
ObjectInstList *instEntry;
ObjectList *metaObj;
struct UAVOData * uavo_data = NULL;
// Get lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Check that the object is not already registered
LL_FOREACH(objList, objEntry) {
if (objEntry->id == id) {
// Already registered, ignore
xSemaphoreGiveRecursive(mutex);
return NULL;
}
}
/* Don't allow duplicate registrations */
if (UAVObjGetByID(id))
goto unlock_exit;
// Create and append entry
objEntry = (ObjectList *) pvPortMalloc(sizeof(ObjectList));
if (objEntry == NULL) {
xSemaphoreGiveRecursive(mutex);
return NULL;
}
objEntry->id = id;
objEntry->name = name;
OLSetIsMetaobject(objEntry, isMetaobject);
OLSetIsSingleInstance(objEntry, isSingleInstance);
OLSetIsSettings(objEntry, isSettings);
objEntry->numBytes = numBytes;
objEntry->events = NULL;
objEntry->numInstances = 0;
objEntry->instances.data = NULL;
objEntry->instances.instId = 0xFFFF;
objEntry->instances.next = NULL;
objEntry->linkedObj = NULL; // will be set later
LL_APPEND(objList, objEntry);
/* Map the various flags to one of the UAVO types we understand */
if (isSingleInstance) {
uavo_data = UAVObjAllocSingle (num_bytes);
} else {
uavo_data = UAVObjAllocMulti (num_bytes);
}
// Create instance zero
instEntry = createInstance(objEntry, 0);
if (instEntry == NULL) {
xSemaphoreGiveRecursive(mutex);
return NULL;
}
// Create metaobject and update linkedObj
if (isMetaobject) {
objEntry->linkedObj = NULL; // will be set later
} else {
// Create metaobject
metaObj =
(ObjectList *) UAVObjRegister(id + 1, metaName,
NULL, 1, 1, 0,
sizeof
(UAVObjMetadata),
NULL);
// Link two objects
objEntry->linkedObj = metaObj;
metaObj->linkedObj = objEntry;
}
if (!uavo_data)
goto unlock_exit;
// Initialize object fields and metadata to default values
if (initCb != NULL) {
initCb((UAVObjHandle) objEntry, 0);
}
// Attempt to load object's metadata from the SD card (not done directly on the metaobject, but through the object)
if (!OLGetIsMetaobject(objEntry)) {
UAVObjLoad((UAVObjHandle) objEntry->linkedObj, 0);
}
// If this is a settings object, attempt to load from SD card
if (OLGetIsSettings(objEntry)) {
UAVObjLoad((UAVObjHandle) objEntry, 0);
}
// Release lock
xSemaphoreGiveRecursive(mutex);
return (UAVObjHandle) objEntry;
/* Fill in the details about this UAVO */
uavo_data->id = id;
uavo_data->instance_size = num_bytes;
if (isSettings) {
uavo_data->base.flags.isSettings = true;
}
/* Initialize the embedded meta UAVO */
UAVObjInitMetaData (&uavo_data->metaObj);
/* Add the newly created object to the global list of objects */
LL_APPEND(uavo_list, uavo_data);
/* Initialize object fields and metadata to default values */
if (initCb)
initCb((UAVObjHandle) uavo_data, 0);
/* Always try to load the meta object from flash */
UAVObjLoad((UAVObjHandle) &(uavo_data->metaObj), 0);
/* Attempt to load settings object from flash */
if (uavo_data->base.flags.isSettings)
UAVObjLoad((UAVObjHandle) uavo_data, 0);
// fire events for outer object and its embedded meta object
UAVObjInstanceUpdated((UAVObjHandle) uavo_data, 0);
UAVObjInstanceUpdated((UAVObjHandle) &(uavo_data->metaObj), 0);
unlock_exit:
xSemaphoreGiveRecursive(mutex);
return (UAVObjHandle) uavo_data;
}
/**
@ -264,52 +368,27 @@ UAVObjHandle UAVObjRegister(uint32_t id, const char *name,
*/
UAVObjHandle UAVObjGetByID(uint32_t id)
{
ObjectList *objEntry;
UAVObjHandle * found_obj = (UAVObjHandle *) NULL;
// Get lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Get lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Look for object
LL_FOREACH(objList, objEntry) {
if (objEntry->id == id) {
// Release lock
xSemaphoreGiveRecursive(mutex);
// Done, object found
return (UAVObjHandle) objEntry;
}
}
// Look for object
struct UAVOData * tmp_obj;
LL_FOREACH(uavo_list, tmp_obj) {
if (tmp_obj->id == id) {
found_obj = (UAVObjHandle *)tmp_obj;
goto unlock_exit;
}
if (MetaObjectId(tmp_obj->id) == id) {
found_obj = (UAVObjHandle *)&(tmp_obj->metaObj);
goto unlock_exit;
}
}
// Object not found, release lock and return error
xSemaphoreGiveRecursive(mutex);
return NULL;
}
/**
* Retrieve an object from the list given its name
* \param[in] name The name of the object
* \return The object or NULL if not found.
*/
UAVObjHandle UAVObjGetByName(char *name)
{
ObjectList *objEntry;
// Get lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Look for object
LL_FOREACH(objList, objEntry) {
if (objEntry->name != NULL
&& strcmp(objEntry->name, name) == 0) {
// Release lock
xSemaphoreGiveRecursive(mutex);
// Done, object found
return (UAVObjHandle) objEntry;
}
}
// Object not found, release lock and return error
xSemaphoreGiveRecursive(mutex);
return NULL;
unlock_exit:
xSemaphoreGiveRecursive(mutex);
return found_obj;
}
/**
@ -317,19 +396,24 @@ UAVObjHandle UAVObjGetByName(char *name)
* \param[in] obj The object handle
* \return The object ID
*/
uint32_t UAVObjGetID(UAVObjHandle obj)
uint32_t UAVObjGetID(UAVObjHandle obj_handle)
{
return ((ObjectList *) obj)->id;
}
PIOS_Assert(obj_handle);
/**
* Get the object's name
* \param[in] obj The object handle
* \return The object's name
*/
const char *UAVObjGetName(UAVObjHandle obj)
{
return ((ObjectList *) obj)->name;
/* Recover the common object header */
struct UAVOBase * uavo_base = (struct UAVOBase *) obj_handle;
if (UAVObjIsMetaobject(obj_handle)) {
/* We have a meta object, find our containing UAVO */
struct UAVOData * uavo_data = container_of ((struct UAVOMeta *)uavo_base, struct UAVOData, metaObj);
return MetaObjectId (uavo_data->id);
} else {
/* We have a data object, augment our pointer */
struct UAVOData * uavo_data = (struct UAVOData *) uavo_base;
return (uavo_data->id);
}
}
/**
@ -339,7 +423,23 @@ const char *UAVObjGetName(UAVObjHandle obj)
*/
uint32_t UAVObjGetNumBytes(UAVObjHandle obj)
{
return ((ObjectList *) obj)->numBytes;
PIOS_Assert(obj);
uint32_t instance_size;
/* Recover the common object header */
struct UAVOBase * uavo_base = (struct UAVOBase *) obj;
if (uavo_base->flags.isMeta) {
instance_size = MetaNumBytes;
} else {
/* We have a data object, augment our pointer */
struct UAVOData * uavo = (struct UAVOData *) uavo_base;
instance_size = uavo->instance_size;
}
return (instance_size);
}
/**
@ -349,9 +449,24 @@ uint32_t UAVObjGetNumBytes(UAVObjHandle obj)
* \param[in] obj The object handle
* \return The object linked object handle
*/
UAVObjHandle UAVObjGetLinkedObj(UAVObjHandle obj)
UAVObjHandle UAVObjGetLinkedObj(UAVObjHandle obj_handle)
{
return (UAVObjHandle) (((ObjectList *) obj)->linkedObj);
PIOS_Assert(obj_handle);
/* Recover the common object header */
struct UAVOBase * uavo_base = (struct UAVOBase *) obj_handle;
if (UAVObjIsMetaobject(obj_handle)) {
/* We have a meta object, find our containing UAVO. */
struct UAVOData * uavo_data = container_of ((struct UAVOMeta *)uavo_base, struct UAVOData, metaObj);
return (UAVObjHandle) uavo_data;
} else {
/* We have a data object, augment our pointer */
struct UAVOData * uavo_data = (struct UAVOData *) uavo_base;
return (UAVObjHandle) &(uavo_data->metaObj);
}
}
/**
@ -359,13 +474,19 @@ UAVObjHandle UAVObjGetLinkedObj(UAVObjHandle obj)
* \param[in] obj The object handle
* \return The number of instances
*/
uint16_t UAVObjGetNumInstances(UAVObjHandle obj)
uint16_t UAVObjGetNumInstances(UAVObjHandle obj_handle)
{
uint32_t numInstances;
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
numInstances = ((ObjectList *) obj)->numInstances;
xSemaphoreGiveRecursive(mutex);
return numInstances;
PIOS_Assert(obj_handle);
if (UAVObjIsSingleInstance(obj_handle)) {
/* Only one instance is allowed */
return 1;
} else {
/* Multi-instance object. Inspect the object */
/* Augment our pointer to reflect the proper type */
struct UAVOMulti * uavo_multi = (struct UAVOMulti *) obj_handle;
return uavo_multi->num_instances;
}
}
/**
@ -373,29 +494,37 @@ uint16_t UAVObjGetNumInstances(UAVObjHandle obj)
* \param[in] obj The object handle
* \return The instance ID or 0 if an error
*/
uint16_t UAVObjCreateInstance(UAVObjHandle obj,
UAVObjInitializeCallback initCb)
uint16_t UAVObjCreateInstance(UAVObjHandle obj_handle,
UAVObjInitializeCallback initCb)
{
ObjectList *objEntry;
ObjectInstList *instEntry;
PIOS_Assert(obj_handle);
// Lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
if (UAVObjIsMetaobject(obj_handle)) {
return 0;
}
// Create new instance
objEntry = (ObjectList *) obj;
instEntry = createInstance(objEntry, objEntry->numInstances);
if (instEntry == NULL) {
xSemaphoreGiveRecursive(mutex);
return -1;
}
// Initialize instance data
if (initCb != NULL) {
initCb(obj, instEntry->instId);
}
// Unlock
xSemaphoreGiveRecursive(mutex);
return instEntry->instId;
// Lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
InstanceHandle instEntry;
uint16_t instId = 0;
// Create new instance
instId = UAVObjGetNumInstances(obj_handle);
instEntry = createInstance( (struct UAVOData *)obj_handle, instId);
if (instEntry == NULL) {
goto unlock_exit;
}
// Initialize instance data
if (initCb) {
initCb(obj_handle, instId);
}
unlock_exit:
xSemaphoreGiveRecursive(mutex);
return instId;
}
/**
@ -403,9 +532,14 @@ uint16_t UAVObjCreateInstance(UAVObjHandle obj,
* \param[in] obj The object handle
* \return True (1) if this is a single instance object
*/
int32_t UAVObjIsSingleInstance(UAVObjHandle obj)
bool UAVObjIsSingleInstance(UAVObjHandle obj_handle)
{
return OLGetIsSingleInstance((ObjectList *) obj);
PIOS_Assert(obj_handle);
/* Recover the common object header */
struct UAVOBase * uavo_base = (struct UAVOBase *) obj_handle;
return uavo_base->flags.isSingle;
}
/**
@ -413,9 +547,14 @@ int32_t UAVObjIsSingleInstance(UAVObjHandle obj)
* \param[in] obj The object handle
* \return True (1) if this is metaobject
*/
int32_t UAVObjIsMetaobject(UAVObjHandle obj)
bool UAVObjIsMetaobject(UAVObjHandle obj_handle)
{
return OLGetIsMetaobject((ObjectList *) obj);
PIOS_Assert(obj_handle);
/* Recover the common object header */
struct UAVOBase * uavo_base = (struct UAVOBase *) obj_handle;
return uavo_base->flags.isMeta;
}
/**
@ -423,9 +562,14 @@ int32_t UAVObjIsMetaobject(UAVObjHandle obj)
* \param[in] obj The object handle
* \return True (1) if this is a settings object
*/
int32_t UAVObjIsSettings(UAVObjHandle obj)
bool UAVObjIsSettings(UAVObjHandle obj_handle)
{
return OLGetIsSettings((ObjectList *) obj);
PIOS_Assert(obj_handle);
/* Recover the common object header */
struct UAVOBase * uavo_base = (struct UAVOBase *) obj_handle;
return uavo_base->flags.isSettings;
}
/**
@ -435,39 +579,49 @@ int32_t UAVObjIsSettings(UAVObjHandle obj)
* \param[in] dataIn The byte array
* \return 0 if success or -1 if failure
*/
int32_t UAVObjUnpack(UAVObjHandle obj, uint16_t instId,
const uint8_t * dataIn)
int32_t UAVObjUnpack(UAVObjHandle obj_handle, uint16_t instId,
const uint8_t * dataIn)
{
ObjectList *objEntry;
ObjectInstList *instEntry;
PIOS_Assert(obj_handle);
// Lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Cast handle to object
objEntry = (ObjectList *) obj;
int32_t rc = -1;
// Get the instance
instEntry = getInstance(objEntry, instId);
if (UAVObjIsMetaobject(obj_handle)) {
if (instId != 0) {
goto unlock_exit;
}
memcpy(MetaDataPtr((struct UAVOMeta *)obj_handle), dataIn, MetaNumBytes);
} else {
struct UAVOData *obj;
InstanceHandle instEntry;
// If the instance does not exist create it and any other instances before it
if (instEntry == NULL) {
instEntry = createInstance(objEntry, instId);
if (instEntry == NULL) {
// Error, unlock and return
xSemaphoreGiveRecursive(mutex);
return -1;
}
}
// Set the data
memcpy(instEntry->data, dataIn, objEntry->numBytes);
// Cast handle to object
obj = (struct UAVOData *) obj_handle;
// Fire event
sendEvent(objEntry, instId, EV_UNPACKED);
// Get the instance
instEntry = getInstance(obj, instId);
// Unlock
xSemaphoreGiveRecursive(mutex);
return 0;
// If the instance does not exist create it and any other instances before it
if (instEntry == NULL) {
instEntry = createInstance(obj, instId);
if (instEntry == NULL) {
goto unlock_exit;
}
}
// Set the data
memcpy(InstanceData(instEntry), dataIn, obj->instance_size);
}
// Fire event
sendEvent((struct UAVOBase*)obj_handle, instId, EV_UNPACKED);
rc = 0;
unlock_exit:
xSemaphoreGiveRecursive(mutex);
return rc;
}
/**
@ -477,30 +631,41 @@ int32_t UAVObjUnpack(UAVObjHandle obj, uint16_t instId,
* \param[out] dataOut The byte array
* \return 0 if success or -1 if failure
*/
int32_t UAVObjPack(UAVObjHandle obj, uint16_t instId, uint8_t * dataOut)
int32_t UAVObjPack(UAVObjHandle obj_handle, uint16_t instId, uint8_t * dataOut)
{
ObjectList *objEntry;
ObjectInstList *instEntry;
PIOS_Assert(obj_handle);
// Lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Cast handle to object
objEntry = (ObjectList *) obj;
int32_t rc = -1;
// Get the instance
instEntry = getInstance(objEntry, instId);
if (instEntry == NULL) {
// Error, unlock and return
xSemaphoreGiveRecursive(mutex);
return -1;
}
// Pack data
memcpy(dataOut, instEntry->data, objEntry->numBytes);
if (UAVObjIsMetaobject(obj_handle)) {
if (instId != 0) {
goto unlock_exit;
}
memcpy(dataOut, MetaDataPtr((struct UAVOMeta *)obj_handle), MetaNumBytes);
} else {
struct UAVOData *obj;
InstanceHandle instEntry;
// Unlock
xSemaphoreGiveRecursive(mutex);
return 0;
// Cast handle to object
obj = (struct UAVOData *) obj_handle;
// Get the instance
instEntry = getInstance(obj, instId);
if (instEntry == NULL) {
goto unlock_exit;
}
// Pack data
memcpy(dataOut, InstanceData(instEntry), obj->instance_size);
}
rc = 0;
unlock_exit:
xSemaphoreGiveRecursive(mutex);
return rc;
}
/**
@ -512,50 +677,72 @@ int32_t UAVObjPack(UAVObjHandle obj, uint16_t instId, uint8_t * dataOut)
* @param[in] file File to append to
* @return 0 if success or -1 if failure
*/
int32_t UAVObjSaveToFile(UAVObjHandle obj, uint16_t instId,
FILEINFO * file)
int32_t UAVObjSaveToFile(UAVObjHandle obj_handle, uint16_t instId,
FILEINFO * file)
{
PIOS_Assert(obj_handle);
#if defined(PIOS_INCLUDE_SDCARD)
uint32_t bytesWritten;
ObjectList *objEntry;
ObjectInstList *instEntry;
uint32_t bytesWritten;
// Check for file system availability
if (PIOS_SDCARD_IsMounted() == 0) {
return -1;
}
// Lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Check for file system availability
if (PIOS_SDCARD_IsMounted() == 0) {
return -1;
}
// Lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
if (UAVObjIsMetaobject(obj_handle)) {
// Get the instance information
if (instId != 0) {
xSemaphoreGiveRecursive(mutex);
return -1;
}
// Write the object ID
uint32_t objId = UAVObjGetID(obj_handle);
PIOS_FWRITE(file, &objId, sizeof(objId),
&bytesWritten);
// Cast to object
objEntry = (ObjectList *) obj;
// Write the data and check that the write was successful
PIOS_FWRITE(file, MetaDataPtr((struct UAVOMeta *)obj_handle), MetaNumBytes,
&bytesWritten);
if (bytesWritten != MetaNumBytes) {
xSemaphoreGiveRecursive(mutex);
return -1;
}
} else {
struct UAVOData * uavo;
InstanceHandle instEntry;
// Get the instance information
instEntry = getInstance(objEntry, instId);
if (instEntry == NULL) {
xSemaphoreGiveRecursive(mutex);
return -1;
}
// Write the object ID
PIOS_FWRITE(file, &objEntry->id, sizeof(objEntry->id),
&bytesWritten);
// Cast to object
uavo = (struct UAVOData *) obj_handle;
// Write the instance ID
if (!OLGetIsSingleInstance(objEntry)) {
PIOS_FWRITE(file, &instEntry->instId,
sizeof(instEntry->instId), &bytesWritten);
}
// Write the data and check that the write was successful
PIOS_FWRITE(file, instEntry->data, objEntry->numBytes,
&bytesWritten);
if (bytesWritten != objEntry->numBytes) {
xSemaphoreGiveRecursive(mutex);
return -1;
}
// Done
xSemaphoreGiveRecursive(mutex);
// Get the instance information
instEntry = getInstance(uavo, instId);
if (instEntry == NULL) {
xSemaphoreGiveRecursive(mutex);
return -1;
}
// Write the object ID
PIOS_FWRITE(file, &uavo->id, sizeof(uavo->id),
&bytesWritten);
// Write the instance ID
if (!UAVObjIsSingleInstance(obj_handle)) {
PIOS_FWRITE(file, &instId,
sizeof(instId), &bytesWritten);
}
// Write the data and check that the write was successful
PIOS_FWRITE(file, InstanceData(instEntry), uavo->instance_size,
&bytesWritten);
if (bytesWritten != uavo->instance_size) {
xSemaphoreGiveRecursive(mutex);
return -1;
}
}
// Done
xSemaphoreGiveRecursive(mutex);
#endif /* PIOS_INCLUDE_SDCARD */
return 0;
return 0;
}
/**
@ -568,59 +755,60 @@ int32_t UAVObjSaveToFile(UAVObjHandle obj, uint16_t instId,
* @param[in] file File to append to
* @return 0 if success or -1 if failure
*/
int32_t UAVObjSave(UAVObjHandle obj, uint16_t instId)
int32_t UAVObjSave(UAVObjHandle obj_handle, uint16_t instId)
{
PIOS_Assert(obj_handle);
#if defined(PIOS_INCLUDE_FLASH_SECTOR_SETTINGS)
ObjectList *objEntry = (ObjectList *) obj;
if (UAVObjIsMetaobject(obj_handle)) {
if (instId != 0)
return -1;
if (objEntry == NULL)
return -1;
if (PIOS_FLASHFS_ObjSave(obj_handle, instId, (uint8_t*) MetaDataPtr((struct UAVOMeta *)obj_handle)) != 0)
return -1;
} else {
InstanceHandle instEntry = getInstance( (struct UAVOData *)obj_handle, instId);
ObjectInstList *instEntry = getInstance(objEntry, instId);
if (instEntry == NULL)
return -1;
if (instEntry == NULL)
return -1;
if (InstanceData(instEntry) == NULL)
return -1;
if (instEntry->data == NULL)
return -1;
if (PIOS_FLASHFS_ObjSave(obj, instId, instEntry->data) != 0)
return -1;
if (PIOS_FLASHFS_ObjSave(obj_handle, instId, InstanceData(instEntry)) != 0)
return -1;
}
#endif
#if defined(PIOS_INCLUDE_SDCARD)
FILEINFO file;
ObjectList *objEntry;
uint8_t filename[14];
FILEINFO file;
uint8_t filename[14];
// Check for file system availability
if (PIOS_SDCARD_IsMounted() == 0) {
return -1;
}
// Lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Check for file system availability
if (PIOS_SDCARD_IsMounted() == 0) {
return -1;
}
// Lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Cast to object
objEntry = (ObjectList *) obj;
// Get filename
objectFilename(obj_handle, filename);
// Get filename
objectFilename(objEntry, filename);
// Open file
if (PIOS_FOPEN_WRITE(filename, file)) {
xSemaphoreGiveRecursive(mutex);
return -1;
}
// Append object
if (UAVObjSaveToFile(obj, instId, &file) == -1) {
PIOS_FCLOSE(file);
xSemaphoreGiveRecursive(mutex);
return -1;
}
// Done, close file and unlock
PIOS_FCLOSE(file);
xSemaphoreGiveRecursive(mutex);
// Open file
if (PIOS_FOPEN_WRITE(filename, file)) {
xSemaphoreGiveRecursive(mutex);
return -1;
}
// Append object
if (UAVObjSaveToFile(obj_handle, instId, &file) == -1) {
PIOS_FCLOSE(file);
xSemaphoreGiveRecursive(mutex);
return -1;
}
// Done, close file and unlock
PIOS_FCLOSE(file);
xSemaphoreGiveRecursive(mutex);
#endif /* PIOS_INCLUDE_SDCARD */
return 0;
return 0;
}
/**
@ -631,68 +819,87 @@ int32_t UAVObjSave(UAVObjHandle obj, uint16_t instId)
UAVObjHandle UAVObjLoadFromFile(FILEINFO * file)
{
#if defined(PIOS_INCLUDE_SDCARD)
uint32_t bytesRead;
ObjectList *objEntry;
ObjectInstList *instEntry;
uint32_t objId;
uint16_t instId;
UAVObjHandle obj;
uint32_t bytesRead;
struct UAVOBase *objEntry;
InstanceHandle instEntry;
uint32_t objId;
uint16_t instId;
UAVObjHandle obj_handle;
// Check for file system availability
if (PIOS_SDCARD_IsMounted() == 0) {
return NULL;
}
// Lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Check for file system availability
if (PIOS_SDCARD_IsMounted() == 0) {
return NULL;
}
// Lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Read the object ID
if (PIOS_FREAD(file, &objId, sizeof(objId), &bytesRead)) {
xSemaphoreGiveRecursive(mutex);
return NULL;
}
// Get the object
obj = UAVObjGetByID(objId);
if (obj == 0) {
xSemaphoreGiveRecursive(mutex);
return NULL;
}
objEntry = (ObjectList *) obj;
// Read the object ID
if (PIOS_FREAD(file, &objId, sizeof(objId), &bytesRead)) {
xSemaphoreGiveRecursive(mutex);
return NULL;
}
// Get the object
obj_handle = UAVObjGetByID(objId);
if (obj_handle == 0) {
xSemaphoreGiveRecursive(mutex);
return NULL;
}
objEntry = (struct UAVOBase *) obj_handle;
// Get the instance ID
instId = 0;
if (!OLGetIsSingleInstance(objEntry)) {
if (PIOS_FREAD
// Get the instance ID
instId = 0;
if (!UAVObjIsSingleInstance(obj_handle)) {
if (PIOS_FREAD
(file, &instId, sizeof(instId), &bytesRead)) {
xSemaphoreGiveRecursive(mutex);
return NULL;
}
}
// Get the instance information
instEntry = getInstance(objEntry, instId);
xSemaphoreGiveRecursive(mutex);
return NULL;
}
}
// If the instance does not exist create it and any other instances before it
if (instEntry == NULL) {
instEntry = createInstance(objEntry, instId);
if (instEntry == NULL) {
// Error, unlock and return
xSemaphoreGiveRecursive(mutex);
return NULL;
}
}
// Read the instance data
if (PIOS_FREAD
(file, instEntry->data, objEntry->numBytes, &bytesRead)) {
xSemaphoreGiveRecursive(mutex);
return NULL;
}
// Fire event
sendEvent(objEntry, instId, EV_UNPACKED);
if (UAVObjIsMetaobject(obj_handle)) {
// If the instance does not exist create it and any other instances before it
if (instId != 0) {
// Error, unlock and return
xSemaphoreGiveRecursive(mutex);
return NULL;
}
// Read the instance data
if (PIOS_FREAD
(file, MetaDataPtr((struct UAVOMeta *)obj_handle), MetaNumBytes, &bytesRead)) {
xSemaphoreGiveRecursive(mutex);
return NULL;
}
} else {
// Unlock
xSemaphoreGiveRecursive(mutex);
return obj;
// Get the instance information
instEntry = getInstance((struct UAVOData *)objEntry, instId);
// If the instance does not exist create it and any other instances before it
if (instEntry == NULL) {
instEntry = createInstance((struct UAVOData *)objEntry, instId);
if (instEntry == NULL) {
// Error, unlock and return
xSemaphoreGiveRecursive(mutex);
return NULL;
}
}
// Read the instance data
if (PIOS_FREAD
(file, InstanceData(instEntry), ((struct UAVOData *)objEntry)->instance_size, &bytesRead)) {
xSemaphoreGiveRecursive(mutex);
return NULL;
}
}
// Fire event
sendEvent(objEntry, instId, EV_UNPACKED);
// Unlock
xSemaphoreGiveRecursive(mutex);
return obj_handle;
#else /* PIOS_INCLUDE_SDCARD */
return NULL;
return NULL;
#endif
}
@ -704,74 +911,74 @@ UAVObjHandle UAVObjLoadFromFile(FILEINFO * file)
* @param[in] instId The object instance
* @return 0 if success or -1 if failure
*/
int32_t UAVObjLoad(UAVObjHandle obj, uint16_t instId)
int32_t UAVObjLoad(UAVObjHandle obj_handle, uint16_t instId)
{
PIOS_Assert(obj_handle);
#if defined(PIOS_INCLUDE_FLASH_SECTOR_SETTINGS)
ObjectList *objEntry = (ObjectList *) obj;
if (UAVObjIsMetaobject(obj_handle)) {
if (instId != 0)
return -1;
if (objEntry == NULL)
return -1;
// Fire event on success
if (PIOS_FLASHFS_ObjLoad(obj_handle, instId, (uint8_t*) MetaDataPtr((struct UAVOMeta *)obj_handle)) == 0)
sendEvent((struct UAVOBase*)obj_handle, instId, EV_UNPACKED);
else
return -1;
} else {
ObjectInstList *instEntry = getInstance(objEntry, instId);
InstanceHandle instEntry = getInstance( (struct UAVOData *)obj_handle, instId);
if (instEntry == NULL)
return -1;
if (instEntry == NULL)
return -1;
if (instEntry->data == NULL)
return -1;
// Fire event on success
if (PIOS_FLASHFS_ObjLoad(obj_handle, instId, InstanceData(instEntry)) == 0)
sendEvent((struct UAVOBase*)obj_handle, instId, EV_UNPACKED);
else
return -1;
}
// Fire event on success
int32_t retval;
if ((retval = PIOS_FLASHFS_ObjLoad(obj, instId, instEntry->data)) == 0)
sendEvent(objEntry, instId, EV_UNPACKED);
else
return retval;
#endif
#if defined(PIOS_INCLUDE_SDCARD)
FILEINFO file;
ObjectList *objEntry;
UAVObjHandle loadedObj;
ObjectList *loadedObjEntry;
uint8_t filename[14];
FILEINFO file;
UAVObjHandle loadedObj;
uint8_t filename[14];
// Check for file system availability
if (PIOS_SDCARD_IsMounted() == 0) {
return -1;
}
// Lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Check for file system availability
if (PIOS_SDCARD_IsMounted() == 0) {
return -1;
}
// Lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Cast to object
objEntry = (ObjectList *) obj;
// Get filename
objectFilename(obj_handle, filename);
// Get filename
objectFilename(objEntry, filename);
// Open file
if (PIOS_FOPEN_READ(filename, file)) {
xSemaphoreGiveRecursive(mutex);
return -1;
}
// Load object
loadedObj = UAVObjLoadFromFile(&file);
if (loadedObj == 0) {
PIOS_FCLOSE(file);
xSemaphoreGiveRecursive(mutex);
return -1;
}
// Check that the IDs match
loadedObjEntry = (ObjectList *) loadedObj;
if (loadedObjEntry->id != objEntry->id) {
PIOS_FCLOSE(file);
xSemaphoreGiveRecursive(mutex);
return -1;
}
// Done, close file and unlock
PIOS_FCLOSE(file);
xSemaphoreGiveRecursive(mutex);
// Open file
if (PIOS_FOPEN_READ(filename, file)) {
xSemaphoreGiveRecursive(mutex);
return -1;
}
// Load object
loadedObj = UAVObjLoadFromFile(&file);
if (loadedObj == 0) {
PIOS_FCLOSE(file);
xSemaphoreGiveRecursive(mutex);
return -1;
}
// Check that the IDs match
if (UAVObjGetID(loadedObj) != UAVObjGetID(obj_handle)) {
PIOS_FCLOSE(file);
xSemaphoreGiveRecursive(mutex);
return -1;
}
// Done, close file and unlock
PIOS_FCLOSE(file);
xSemaphoreGiveRecursive(mutex);
#endif /* PIOS_INCLUDE_SDCARD */
return 0;
return 0;
}
/**
@ -780,35 +987,32 @@ int32_t UAVObjLoad(UAVObjHandle obj, uint16_t instId)
* @param[in] instId The object instance
* @return 0 if success or -1 if failure
*/
int32_t UAVObjDelete(UAVObjHandle obj, uint16_t instId)
int32_t UAVObjDelete(UAVObjHandle obj_handle, uint16_t instId)
{
PIOS_Assert(obj_handle);
#if defined(PIOS_INCLUDE_FLASH_SECTOR_SETTINGS)
PIOS_FLASHFS_ObjDelete(obj, instId);
PIOS_FLASHFS_ObjDelete(obj_handle, instId);
#endif
#if defined(PIOS_INCLUDE_SDCARD)
ObjectList *objEntry;
uint8_t filename[14];
uint8_t filename[14];
// Check for file system availability
if (PIOS_SDCARD_IsMounted() == 0) {
return -1;
}
// Lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Check for file system availability
if (PIOS_SDCARD_IsMounted() == 0) {
return -1;
}
// Lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Cast to object
objEntry = (ObjectList *) obj;
// Get filename
objectFilename(obj_handle, filename);
// Get filename
objectFilename(objEntry, filename);
// Delete file
PIOS_FUNLINK(filename);
// Delete file
PIOS_FUNLINK(filename);
// Done
xSemaphoreGiveRecursive(mutex);
// Done
xSemaphoreGiveRecursive(mutex);
#endif /* PIOS_INCLUDE_SDCARD */
return 0;
return 0;
}
/**
@ -817,27 +1021,30 @@ int32_t UAVObjDelete(UAVObjHandle obj, uint16_t instId)
*/
int32_t UAVObjSaveSettings()
{
ObjectList *objEntry;
struct UAVOData *obj;
// Get lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Get lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Save all settings objects
LL_FOREACH(objList, objEntry) {
// Check if this is a settings object
if (OLGetIsSettings(objEntry)) {
// Save object
if (UAVObjSave((UAVObjHandle) objEntry, 0) ==
-1) {
xSemaphoreGiveRecursive(mutex);
return -1;
}
}
}
int32_t rc = -1;
// Done
xSemaphoreGiveRecursive(mutex);
return 0;
// Save all settings objects
LL_FOREACH(uavo_list, obj) {
// Check if this is a settings object
if (UAVObjIsSettings(obj)) {
// Save object
if (UAVObjSave((UAVObjHandle) obj, 0) ==
-1) {
goto unlock_exit;
}
}
}
rc = 0;
unlock_exit:
xSemaphoreGiveRecursive(mutex);
return rc;
}
/**
@ -846,27 +1053,30 @@ int32_t UAVObjSaveSettings()
*/
int32_t UAVObjLoadSettings()
{
ObjectList *objEntry;
struct UAVOData *obj;
// Get lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Get lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Load all settings objects
LL_FOREACH(objList, objEntry) {
// Check if this is a settings object
if (OLGetIsSettings(objEntry)) {
// Load object
if (UAVObjLoad((UAVObjHandle) objEntry, 0) ==
-1) {
xSemaphoreGiveRecursive(mutex);
return -1;
}
}
}
int32_t rc = -1;
// Done
xSemaphoreGiveRecursive(mutex);
return 0;
// Load all settings objects
LL_FOREACH(uavo_list, obj) {
// Check if this is a settings object
if (UAVObjIsSettings(obj)) {
// Load object
if (UAVObjLoad((UAVObjHandle) obj, 0) ==
-1) {
goto unlock_exit;
}
}
}
rc = 0;
unlock_exit:
xSemaphoreGiveRecursive(mutex);
return rc;
}
/**
@ -875,27 +1085,30 @@ int32_t UAVObjLoadSettings()
*/
int32_t UAVObjDeleteSettings()
{
ObjectList *objEntry;
struct UAVOData *obj;
// Get lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Get lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Save all settings objects
LL_FOREACH(objList, objEntry) {
// Check if this is a settings object
if (OLGetIsSettings(objEntry)) {
// Save object
if (UAVObjDelete((UAVObjHandle) objEntry, 0)
== -1) {
xSemaphoreGiveRecursive(mutex);
return -1;
}
}
}
int32_t rc = -1;
// Done
xSemaphoreGiveRecursive(mutex);
return 0;
// Save all settings objects
LL_FOREACH(uavo_list, obj) {
// Check if this is a settings object
if (UAVObjIsSettings(obj)) {
// Save object
if (UAVObjDelete((UAVObjHandle) obj, 0)
== -1) {
goto unlock_exit;
}
}
}
rc = 0;
unlock_exit:
xSemaphoreGiveRecursive(mutex);
return rc;
}
/**
@ -904,27 +1117,27 @@ int32_t UAVObjDeleteSettings()
*/
int32_t UAVObjSaveMetaobjects()
{
ObjectList *objEntry;
struct UAVOData *obj;
// Get lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Get lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Save all settings objects
LL_FOREACH(objList, objEntry) {
// Check if this is a settings object
if (OLGetIsMetaobject(objEntry)) {
// Save object
if (UAVObjSave((UAVObjHandle) objEntry, 0) ==
-1) {
xSemaphoreGiveRecursive(mutex);
return -1;
}
}
}
int32_t rc = -1;
// Done
xSemaphoreGiveRecursive(mutex);
return 0;
// Save all settings objects
LL_FOREACH(uavo_list, obj) {
// Save object
if (UAVObjSave( (UAVObjHandle) MetaObjectPtr(obj), 0) ==
-1) {
goto unlock_exit;
}
}
rc = 0;
unlock_exit:
xSemaphoreGiveRecursive(mutex);
return rc;
}
/**
@ -933,27 +1146,27 @@ int32_t UAVObjSaveMetaobjects()
*/
int32_t UAVObjLoadMetaobjects()
{
ObjectList *objEntry;
struct UAVOData *obj;
// Get lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Get lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Load all settings objects
LL_FOREACH(objList, objEntry) {
// Check if this is a settings object
if (OLGetIsMetaobject(objEntry)) {
// Load object
if (UAVObjLoad((UAVObjHandle) objEntry, 0) ==
-1) {
xSemaphoreGiveRecursive(mutex);
return -1;
}
}
}
int32_t rc = -1;
// Done
xSemaphoreGiveRecursive(mutex);
return 0;
// Load all settings objects
LL_FOREACH(uavo_list, obj) {
// Load object
if (UAVObjLoad((UAVObjHandle) MetaObjectPtr(obj), 0) ==
-1) {
goto unlock_exit;
}
}
rc = 0;
unlock_exit:
xSemaphoreGiveRecursive(mutex);
return rc;
}
/**
@ -962,253 +1175,293 @@ int32_t UAVObjLoadMetaobjects()
*/
int32_t UAVObjDeleteMetaobjects()
{
ObjectList *objEntry;
struct UAVOData *obj;
// Get lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Load all settings objects
LL_FOREACH(objList, objEntry) {
// Check if this is a settings object
if (OLGetIsMetaobject(objEntry)) {
// Load object
if (UAVObjDelete((UAVObjHandle) objEntry, 0)
== -1) {
xSemaphoreGiveRecursive(mutex);
return -1;
}
}
}
// Done
xSemaphoreGiveRecursive(mutex);
return 0;
}
/**
* Set the object data
* \param[in] obj The object handle
* \param[in] dataIn The object's data structure
* \return 0 if success or -1 if failure
*/
int32_t UAVObjSetData(UAVObjHandle obj, const void *dataIn)
{
return UAVObjSetInstanceData(obj, 0, dataIn);
}
/**
* Set the object data
* \param[in] obj The object handle
* \param[in] dataIn The object's data structure
* \return 0 if success or -1 if failure
*/
int32_t UAVObjSetDataField(UAVObjHandle obj, const void* dataIn, uint32_t offset, uint32_t size)
{
return UAVObjSetInstanceDataField(obj, 0, dataIn, offset, size);
}
/**
* Get the object data
* \param[in] obj The object handle
* \param[out] dataOut The object's data structure
* \return 0 if success or -1 if failure
*/
int32_t UAVObjGetData(UAVObjHandle obj, void *dataOut)
{
return UAVObjGetInstanceData(obj, 0, dataOut);
}
/**
* Get the object data
* \param[in] obj The object handle
* \param[out] dataOut The object's data structure
* \return 0 if success or -1 if failure
*/
int32_t UAVObjGetDataField(UAVObjHandle obj, void* dataOut, uint32_t offset, uint32_t size)
{
return UAVObjGetInstanceDataField(obj, 0, dataOut, offset, size);
}
/**
* Set the data of a specific object instance
* \param[in] obj The object handle
* \param[in] instId The object instance ID
* \param[in] dataIn The object's data structure
* \return 0 if success or -1 if failure
*/
int32_t UAVObjSetInstanceData(UAVObjHandle obj, uint16_t instId,
const void *dataIn)
{
ObjectList *objEntry;
ObjectInstList *instEntry;
UAVObjMetadata *mdata;
// Lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Cast to object info
objEntry = (ObjectList *) obj;
// Check access level
if (!OLGetIsMetaobject(objEntry)) {
mdata =
(UAVObjMetadata *) (objEntry->linkedObj->instances.
data);
if (UAVObjGetAccess(mdata) == ACCESS_READONLY) {
xSemaphoreGiveRecursive(mutex);
return -1;
}
}
// Get instance information
instEntry = getInstance(objEntry, instId);
if (instEntry == NULL) {
// Error, unlock and return
xSemaphoreGiveRecursive(mutex);
return -1;
}
// Set data
memcpy(instEntry->data, dataIn, objEntry->numBytes);
// Fire event
sendEvent(objEntry, instId, EV_UPDATED);
// Unlock
xSemaphoreGiveRecursive(mutex);
return 0;
}
/**
* Set the data of a specific object instance
* \param[in] obj The object handle
* \param[in] instId The object instance ID
* \param[in] dataIn The object's data structure
* \return 0 if success or -1 if failure
*/
int32_t UAVObjSetInstanceDataField(UAVObjHandle obj, uint16_t instId, const void* dataIn, uint32_t offset, uint32_t size)
{
ObjectList* objEntry;
ObjectInstList* instEntry;
UAVObjMetadata* mdata;
// Lock
// Get lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Cast to object info
objEntry = (ObjectList*)obj;
int32_t rc = -1;
// Check access level
if ( !OLGetIsMetaobject(objEntry) )
{
mdata = (UAVObjMetadata*)(objEntry->linkedObj->instances.data);
if ( UAVObjGetAccess(mdata) == ACCESS_READONLY )
{
xSemaphoreGiveRecursive(mutex);
return -1;
// Load all settings objects
LL_FOREACH(uavo_list, obj) {
// Load object
if (UAVObjDelete((UAVObjHandle) MetaObjectPtr(obj), 0)
== -1) {
goto unlock_exit;
}
}
// Get instance information
instEntry = getInstance(objEntry, instId);
if ( instEntry == NULL )
{
// Error, unlock and return
xSemaphoreGiveRecursive(mutex);
return -1;
}
rc = 0;
// return if we set too much of what we have
if ( (size + offset) > objEntry->numBytes) {
// Error, unlock and return
xSemaphoreGiveRecursive(mutex);
return -1;
}
// Set data
memcpy(instEntry->data + offset, dataIn, size);
// Fire event
sendEvent(objEntry, instId, EV_UPDATED);
// Unlock
unlock_exit:
xSemaphoreGiveRecursive(mutex);
return 0;
return rc;
}
/**
* Get the data of a specific object instance
* Set the object data
* \param[in] obj The object handle
* \param[in] instId The object instance ID
* \param[out] dataOut The object's data structure
* \param[in] dataIn The object's data structure
* \return 0 if success or -1 if failure
*/
int32_t UAVObjGetInstanceData(UAVObjHandle obj, uint16_t instId,
void *dataOut)
int32_t UAVObjSetData(UAVObjHandle obj_handle, const void *dataIn)
{
ObjectList *objEntry;
ObjectInstList *instEntry;
// Lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Cast to object info
objEntry = (ObjectList *) obj;
// Get instance information
instEntry = getInstance(objEntry, instId);
if (instEntry == NULL) {
// Error, unlock and return
xSemaphoreGiveRecursive(mutex);
return -1;
}
// Set data
memcpy(dataOut, instEntry->data, objEntry->numBytes);
// Unlock
xSemaphoreGiveRecursive(mutex);
return 0;
return UAVObjSetInstanceData(obj_handle, 0, dataIn);
}
/**
* Get the data of a specific object instance
* Set the object data
* \param[in] obj The object handle
* \param[in] dataIn The object's data structure
* \return 0 if success or -1 if failure
*/
int32_t UAVObjSetDataField(UAVObjHandle obj_handle, const void* dataIn, uint32_t offset, uint32_t size)
{
return UAVObjSetInstanceDataField(obj_handle, 0, dataIn, offset, size);
}
/**
* Get the object data
* \param[in] obj The object handle
* \param[in] instId The object instance ID
* \param[out] dataOut The object's data structure
* \return 0 if success or -1 if failure
*/
int32_t UAVObjGetInstanceDataField(UAVObjHandle obj, uint16_t instId, void* dataOut, uint32_t offset, uint32_t size)
int32_t UAVObjGetData(UAVObjHandle obj_handle, void *dataOut)
{
ObjectList* objEntry;
ObjectInstList* instEntry;
return UAVObjGetInstanceData(obj_handle, 0, dataOut);
}
/**
* Get the object data
* \param[in] obj The object handle
* \param[out] dataOut The object's data structure
* \return 0 if success or -1 if failure
*/
int32_t UAVObjGetDataField(UAVObjHandle obj_handle, void* dataOut, uint32_t offset, uint32_t size)
{
return UAVObjGetInstanceDataField(obj_handle, 0, dataOut, offset, size);
}
/**
* Set the data of a specific object instance
* \param[in] obj The object handle
* \param[in] instId The object instance ID
* \param[in] dataIn The object's data structure
* \return 0 if success or -1 if failure
*/
int32_t UAVObjSetInstanceData(UAVObjHandle obj_handle, uint16_t instId,
const void *dataIn)
{
PIOS_Assert(obj_handle);
// Lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Cast to object info
objEntry = (ObjectList*)obj;
int32_t rc = -1;
// Get instance information
instEntry = getInstance(objEntry, instId);
if ( instEntry == NULL )
{
// Error, unlock and return
xSemaphoreGiveRecursive(mutex);
return -1;
if (UAVObjIsMetaobject(obj_handle)) {
if (instId != 0) {
goto unlock_exit;
}
memcpy(MetaDataPtr((struct UAVOMeta *)obj_handle), dataIn, MetaNumBytes);
} else {
struct UAVOData *obj;
InstanceHandle instEntry;
// Cast to object info
obj = (struct UAVOData *) obj_handle;
// Check access level
if (UAVObjReadOnly(obj_handle)) {
goto unlock_exit;
}
// Get instance information
instEntry = getInstance(obj, instId);
if (instEntry == NULL) {
goto unlock_exit;
}
// Set data
memcpy(InstanceData(instEntry), dataIn, obj->instance_size);
}
// return if we request too much of what we can give
if ( (size + offset) > objEntry->numBytes)
{
// Error, unlock and return
xSemaphoreGiveRecursive(mutex);
return -1;
}
// Set data
memcpy(dataOut, instEntry->data + offset, size);
// Fire event
sendEvent((struct UAVOBase *)obj_handle, instId, EV_UPDATED);
rc = 0;
// Unlock
unlock_exit:
xSemaphoreGiveRecursive(mutex);
return 0;
return rc;
}
/**
* Set the data of a specific object instance
* \param[in] obj The object handle
* \param[in] instId The object instance ID
* \param[in] dataIn The object's data structure
* \return 0 if success or -1 if failure
*/
int32_t UAVObjSetInstanceDataField(UAVObjHandle obj_handle, uint16_t instId, const void* dataIn, uint32_t offset, uint32_t size)
{
PIOS_Assert(obj_handle);
// Lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
int32_t rc = -1;
if (UAVObjIsMetaobject(obj_handle)) {
// Get instance information
if (instId != 0) {
goto unlock_exit;
}
// Check for overrun
if ((size + offset) > MetaNumBytes) {
goto unlock_exit;
}
// Set data
memcpy(MetaDataPtr((struct UAVOMeta *)obj_handle) + offset, dataIn, size);
} else {
struct UAVOData * obj;
InstanceHandle instEntry;
// Cast to object info
obj = (struct UAVOData *)obj_handle;
// Check access level
if (UAVObjReadOnly(obj_handle)) {
goto unlock_exit;
}
// Get instance information
instEntry = getInstance(obj, instId);
if (instEntry == NULL) {
goto unlock_exit;
}
// Check for overrun
if ((size + offset) > obj->instance_size) {
goto unlock_exit;
}
// Set data
memcpy(InstanceData(instEntry) + offset, dataIn, size);
}
// Fire event
sendEvent((struct UAVOBase *)obj_handle, instId, EV_UPDATED);
rc = 0;
unlock_exit:
xSemaphoreGiveRecursive(mutex);
return rc;
}
/**
* Get the data of a specific object instance
* \param[in] obj The object handle
* \param[in] instId The object instance ID
* \param[out] dataOut The object's data structure
* \return 0 if success or -1 if failure
*/
int32_t UAVObjGetInstanceData(UAVObjHandle obj_handle, uint16_t instId,
void *dataOut)
{
PIOS_Assert(obj_handle);
// Lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
int32_t rc = -1;
if (UAVObjIsMetaobject(obj_handle)) {
// Get instance information
if (instId != 0) {
goto unlock_exit;
}
// Set data
memcpy(dataOut, MetaDataPtr((struct UAVOMeta *)obj_handle), MetaNumBytes);
} else {
struct UAVOData *obj;
InstanceHandle instEntry;
// Cast to object info
obj = (struct UAVOData *) obj_handle;
// Get instance information
instEntry = getInstance(obj, instId);
if (instEntry == NULL) {
goto unlock_exit;
}
// Set data
memcpy(dataOut, InstanceData(instEntry), obj->instance_size);
}
rc = 0;
unlock_exit:
xSemaphoreGiveRecursive(mutex);
return rc;
}
/**
* Get the data of a specific object instance
* \param[in] obj The object handle
* \param[in] instId The object instance ID
* \param[out] dataOut The object's data structure
* \return 0 if success or -1 if failure
*/
int32_t UAVObjGetInstanceDataField(UAVObjHandle obj_handle, uint16_t instId, void* dataOut, uint32_t offset, uint32_t size)
{
PIOS_Assert(obj_handle);
// Lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
int32_t rc = -1;
if (UAVObjIsMetaobject(obj_handle)) {
// Get instance information
if (instId != 0) {
goto unlock_exit;
}
// Check for overrun
if ((size + offset) > MetaNumBytes) {
goto unlock_exit;
}
// Set data
memcpy(dataOut, MetaDataPtr((struct UAVOMeta *)obj_handle) + offset, size);
} else {
struct UAVOData * obj;
InstanceHandle instEntry;
// Cast to object info
obj = (struct UAVOData *)obj_handle;
// Get instance information
instEntry = getInstance(obj, instId);
if (instEntry == NULL) {
goto unlock_exit;
}
// Check for overrun
if ((size + offset) > obj->instance_size) {
goto unlock_exit;
}
// Set data
memcpy(dataOut, InstanceData(instEntry) + offset, size);
}
rc = 0;
unlock_exit:
xSemaphoreGiveRecursive(mutex);
return rc;
}
/**
@ -1217,25 +1470,21 @@ int32_t UAVObjGetInstanceDataField(UAVObjHandle obj, uint16_t instId, void* data
* \param[in] dataIn The object's metadata structure
* \return 0 if success or -1 if failure
*/
int32_t UAVObjSetMetadata(UAVObjHandle obj, const UAVObjMetadata * dataIn)
int32_t UAVObjSetMetadata(UAVObjHandle obj_handle, const UAVObjMetadata * dataIn)
{
ObjectList *objEntry;
PIOS_Assert(obj_handle);
// Lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Set metadata (metadata of metaobjects can not be modified)
if (UAVObjIsMetaobject(obj_handle)) {
return -1;
}
// Set metadata (metadata of metaobjects can not be modified)
objEntry = (ObjectList *) obj;
if (!OLGetIsMetaobject(objEntry)) {
UAVObjSetData((UAVObjHandle) objEntry->linkedObj,
dataIn);
} else {
return -1;
}
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Unlock
xSemaphoreGiveRecursive(mutex);
return 0;
UAVObjSetData((UAVObjHandle) MetaObjectPtr((struct UAVOData *)obj_handle), dataIn);
xSemaphoreGiveRecursive(mutex);
return 0;
}
/**
@ -1244,44 +1493,29 @@ int32_t UAVObjSetMetadata(UAVObjHandle obj, const UAVObjMetadata * dataIn)
* \param[out] dataOut The object's metadata structure
* \return 0 if success or -1 if failure
*/
int32_t UAVObjGetMetadata(UAVObjHandle obj, UAVObjMetadata * dataOut)
int32_t UAVObjGetMetadata(UAVObjHandle obj_handle, UAVObjMetadata * dataOut)
{
ObjectList *objEntry;
PIOS_Assert(obj_handle);
// Lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Get metadata
objEntry = (ObjectList *) obj;
if (OLGetIsMetaobject(objEntry)) {
memcpy(dataOut, &defMetadata, sizeof(UAVObjMetadata));
} else {
UAVObjGetData((UAVObjHandle) objEntry->linkedObj,
dataOut);
}
// Get metadata
if (UAVObjIsMetaobject(obj_handle)) {
memcpy(dataOut, &defMetadata, sizeof(UAVObjMetadata));
} else {
UAVObjGetData((UAVObjHandle) MetaObjectPtr( (struct UAVOData *)obj_handle ),
dataOut);
}
// Unlock
xSemaphoreGiveRecursive(mutex);
return 0;
// Unlock
xSemaphoreGiveRecursive(mutex);
return 0;
}
/**
* Initialize a UAVObjMetadata object.
* \param[in] metadata The metadata object
*/
void UAVObjMetadataInitialize(UAVObjMetadata* metadata)
{
metadata->flags =
ACCESS_READWRITE << UAVOBJ_ACCESS_SHIFT |
ACCESS_READWRITE << UAVOBJ_GCS_ACCESS_SHIFT |
1 << UAVOBJ_TELEMETRY_ACKED_SHIFT |
1 << UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT |
UPDATEMODE_ONCHANGE << UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT |
UPDATEMODE_ONCHANGE << UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT;
metadata->telemetryUpdatePeriod = 0;
metadata->gcsTelemetryUpdatePeriod = 0;
metadata->loggingUpdatePeriod = 0;
}
/*******************************
* Object Metadata Manipulation
******************************/
/**
* Get the UAVObject metadata access member
@ -1290,6 +1524,7 @@ void UAVObjMetadataInitialize(UAVObjMetadata* metadata)
*/
UAVObjAccessType UAVObjGetAccess(const UAVObjMetadata* metadata)
{
PIOS_Assert(metadata);
return (metadata->flags >> UAVOBJ_ACCESS_SHIFT) & 1;
}
@ -1300,6 +1535,7 @@ UAVObjAccessType UAVObjGetAccess(const UAVObjMetadata* metadata)
*/
void UAVObjSetAccess(UAVObjMetadata* metadata, UAVObjAccessType mode)
{
PIOS_Assert(metadata);
SET_BITS(metadata->flags, UAVOBJ_ACCESS_SHIFT, mode, 1);
}
@ -1310,6 +1546,7 @@ void UAVObjSetAccess(UAVObjMetadata* metadata, UAVObjAccessType mode)
*/
UAVObjAccessType UAVObjGetGcsAccess(const UAVObjMetadata* metadata)
{
PIOS_Assert(metadata);
return (metadata->flags >> UAVOBJ_GCS_ACCESS_SHIFT) & 1;
}
@ -1319,6 +1556,7 @@ UAVObjAccessType UAVObjGetGcsAccess(const UAVObjMetadata* metadata)
* \param[in] mode The access mode
*/
void UAVObjSetGcsAccess(UAVObjMetadata* metadata, UAVObjAccessType mode) {
PIOS_Assert(metadata);
SET_BITS(metadata->flags, UAVOBJ_GCS_ACCESS_SHIFT, mode, 1);
}
@ -1328,6 +1566,7 @@ void UAVObjSetGcsAccess(UAVObjMetadata* metadata, UAVObjAccessType mode) {
* \return the telemetry acked boolean
*/
uint8_t UAVObjGetTelemetryAcked(const UAVObjMetadata* metadata) {
PIOS_Assert(metadata);
return (metadata->flags >> UAVOBJ_TELEMETRY_ACKED_SHIFT) & 1;
}
@ -1337,6 +1576,7 @@ uint8_t UAVObjGetTelemetryAcked(const UAVObjMetadata* metadata) {
* \param[in] val The telemetry acked boolean
*/
void UAVObjSetTelemetryAcked(UAVObjMetadata* metadata, uint8_t val) {
PIOS_Assert(metadata);
SET_BITS(metadata->flags, UAVOBJ_TELEMETRY_ACKED_SHIFT, val, 1);
}
@ -1346,6 +1586,7 @@ void UAVObjSetTelemetryAcked(UAVObjMetadata* metadata, uint8_t val) {
* \return the telemetry acked boolean
*/
uint8_t UAVObjGetGcsTelemetryAcked(const UAVObjMetadata* metadata) {
PIOS_Assert(metadata);
return (metadata->flags >> UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT) & 1;
}
@ -1355,6 +1596,7 @@ uint8_t UAVObjGetGcsTelemetryAcked(const UAVObjMetadata* metadata) {
* \param[in] val The GCS telemetry acked boolean
*/
void UAVObjSetGcsTelemetryAcked(UAVObjMetadata* metadata, uint8_t val) {
PIOS_Assert(metadata);
SET_BITS(metadata->flags, UAVOBJ_GCS_TELEMETRY_ACKED_SHIFT, val, 1);
}
@ -1364,6 +1606,7 @@ void UAVObjSetGcsTelemetryAcked(UAVObjMetadata* metadata, uint8_t val) {
* \return the telemetry update mode
*/
UAVObjUpdateMode UAVObjGetTelemetryUpdateMode(const UAVObjMetadata* metadata) {
PIOS_Assert(metadata);
return (metadata->flags >> UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT) & UAVOBJ_UPDATE_MODE_MASK;
}
@ -1373,6 +1616,7 @@ UAVObjUpdateMode UAVObjGetTelemetryUpdateMode(const UAVObjMetadata* metadata) {
* \param[in] val The telemetry update mode
*/
void UAVObjSetTelemetryUpdateMode(UAVObjMetadata* metadata, UAVObjUpdateMode val) {
PIOS_Assert(metadata);
SET_BITS(metadata->flags, UAVOBJ_TELEMETRY_UPDATE_MODE_SHIFT, val, UAVOBJ_UPDATE_MODE_MASK);
}
@ -1382,6 +1626,7 @@ void UAVObjSetTelemetryUpdateMode(UAVObjMetadata* metadata, UAVObjUpdateMode val
* \return the GCS telemetry update mode
*/
UAVObjUpdateMode UAVObjGetGcsTelemetryUpdateMode(const UAVObjMetadata* metadata) {
PIOS_Assert(metadata);
return (metadata->flags >> UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT) & UAVOBJ_UPDATE_MODE_MASK;
}
@ -1391,6 +1636,7 @@ UAVObjUpdateMode UAVObjGetGcsTelemetryUpdateMode(const UAVObjMetadata* metadata)
* \param[in] val The GCS telemetry update mode
*/
void UAVObjSetGcsTelemetryUpdateMode(UAVObjMetadata* metadata, UAVObjUpdateMode val) {
PIOS_Assert(metadata);
SET_BITS(metadata->flags, UAVOBJ_GCS_TELEMETRY_UPDATE_MODE_SHIFT, val, UAVOBJ_UPDATE_MODE_MASK);
}
@ -1403,22 +1649,13 @@ void UAVObjSetGcsTelemetryUpdateMode(UAVObjMetadata* metadata, UAVObjUpdateMode
* \arg 1 if read only
* \arg -1 if unable to get meta data
*/
int8_t UAVObjReadOnly(UAVObjHandle obj)
int8_t UAVObjReadOnly(UAVObjHandle obj_handle)
{
ObjectList *objEntry;
UAVObjMetadata *mdata;
// Cast to object info
objEntry = (ObjectList *) obj;
// Check access level
if (!OLGetIsMetaobject(objEntry)) {
mdata =
(UAVObjMetadata *) (objEntry->linkedObj->instances.
data);
return UAVObjGetAccess(mdata) == ACCESS_READONLY;
}
return -1;
PIOS_Assert(obj_handle);
if (!UAVObjIsMetaobject(obj_handle)) {
return UAVObjGetAccess(LinkedMetaDataPtr( (struct UAVOData *)obj_handle)) == ACCESS_READONLY;
}
return -1;
}
/**
@ -1429,14 +1666,16 @@ int8_t UAVObjReadOnly(UAVObjHandle obj)
* \param[in] eventMask The event mask, if EV_MASK_ALL then all events are enabled (e.g. EV_UPDATED | EV_UPDATED_MANUAL)
* \return 0 if success or -1 if failure
*/
int32_t UAVObjConnectQueue(UAVObjHandle obj, xQueueHandle queue,
uint8_t eventMask)
int32_t UAVObjConnectQueue(UAVObjHandle obj_handle, xQueueHandle queue,
uint8_t eventMask)
{
int32_t res;
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
res = connectObj(obj, queue, 0, eventMask);
xSemaphoreGiveRecursive(mutex);
return res;
PIOS_Assert(obj_handle);
PIOS_Assert(queue);
int32_t res;
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
res = connectObj(obj_handle, queue, 0, eventMask);
xSemaphoreGiveRecursive(mutex);
return res;
}
/**
@ -1445,13 +1684,15 @@ int32_t UAVObjConnectQueue(UAVObjHandle obj, xQueueHandle queue,
* \param[in] queue The event queue
* \return 0 if success or -1 if failure
*/
int32_t UAVObjDisconnectQueue(UAVObjHandle obj, xQueueHandle queue)
int32_t UAVObjDisconnectQueue(UAVObjHandle obj_handle, xQueueHandle queue)
{
int32_t res;
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
res = disconnectObj(obj, queue, 0);
xSemaphoreGiveRecursive(mutex);
return res;
PIOS_Assert(obj_handle);
PIOS_Assert(queue);
int32_t res;
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
res = disconnectObj(obj_handle, queue, 0);
xSemaphoreGiveRecursive(mutex);
return res;
}
/**
@ -1462,14 +1703,15 @@ int32_t UAVObjDisconnectQueue(UAVObjHandle obj, xQueueHandle queue)
* \param[in] eventMask The event mask, if EV_MASK_ALL then all events are enabled (e.g. EV_UPDATED | EV_UPDATED_MANUAL)
* \return 0 if success or -1 if failure
*/
int32_t UAVObjConnectCallback(UAVObjHandle obj, UAVObjEventCallback cb,
uint8_t eventMask)
int32_t UAVObjConnectCallback(UAVObjHandle obj_handle, UAVObjEventCallback cb,
uint8_t eventMask)
{
int32_t res;
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
res = connectObj(obj, 0, cb, eventMask);
xSemaphoreGiveRecursive(mutex);
return res;
PIOS_Assert(obj_handle);
int32_t res;
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
res = connectObj(obj_handle, 0, cb, eventMask);
xSemaphoreGiveRecursive(mutex);
return res;
}
/**
@ -1478,13 +1720,14 @@ int32_t UAVObjConnectCallback(UAVObjHandle obj, UAVObjEventCallback cb,
* \param[in] cb The event callback
* \return 0 if success or -1 if failure
*/
int32_t UAVObjDisconnectCallback(UAVObjHandle obj, UAVObjEventCallback cb)
int32_t UAVObjDisconnectCallback(UAVObjHandle obj_handle, UAVObjEventCallback cb)
{
int32_t res;
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
res = disconnectObj(obj, 0, cb);
xSemaphoreGiveRecursive(mutex);
return res;
PIOS_Assert(obj_handle);
int32_t res;
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
res = disconnectObj(obj_handle, 0, cb);
xSemaphoreGiveRecursive(mutex);
return res;
}
/**
@ -1492,9 +1735,9 @@ int32_t UAVObjDisconnectCallback(UAVObjHandle obj, UAVObjEventCallback cb)
* will be generated as soon as the object is updated.
* \param[in] obj The object handle
*/
void UAVObjRequestUpdate(UAVObjHandle obj)
void UAVObjRequestUpdate(UAVObjHandle obj_handle)
{
UAVObjRequestInstanceUpdate(obj, UAVOBJ_ALL_INSTANCES);
UAVObjRequestInstanceUpdate(obj_handle, UAVOBJ_ALL_INSTANCES);
}
/**
@ -1503,20 +1746,21 @@ void UAVObjRequestUpdate(UAVObjHandle obj)
* \param[in] obj The object handle
* \param[in] instId Object instance ID to update
*/
void UAVObjRequestInstanceUpdate(UAVObjHandle obj, uint16_t instId)
void UAVObjRequestInstanceUpdate(UAVObjHandle obj_handle, uint16_t instId)
{
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
sendEvent((ObjectList *) obj, instId, EV_UPDATE_REQ);
xSemaphoreGiveRecursive(mutex);
PIOS_Assert(obj_handle);
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
sendEvent((struct UAVOBase *) obj_handle, instId, EV_UPDATE_REQ);
xSemaphoreGiveRecursive(mutex);
}
/**
* Send the object's data to the GCS (triggers a EV_UPDATED_MANUAL event on this object).
* \param[in] obj The object handle
*/
void UAVObjUpdated(UAVObjHandle obj)
void UAVObjUpdated(UAVObjHandle obj_handle)
{
UAVObjInstanceUpdated(obj, UAVOBJ_ALL_INSTANCES);
UAVObjInstanceUpdated(obj_handle, UAVOBJ_ALL_INSTANCES);
}
/**
@ -1524,11 +1768,12 @@ void UAVObjUpdated(UAVObjHandle obj)
* \param[in] obj The object handle
* \param[in] instId The object instance ID
*/
void UAVObjInstanceUpdated(UAVObjHandle obj, uint16_t instId)
void UAVObjInstanceUpdated(UAVObjHandle obj_handle, uint16_t instId)
{
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
sendEvent((ObjectList *) obj, instId, EV_UPDATED_MANUAL);
xSemaphoreGiveRecursive(mutex);
PIOS_Assert(obj_handle);
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
sendEvent((struct UAVOBase *) obj_handle, instId, EV_UPDATED_MANUAL);
xSemaphoreGiveRecursive(mutex);
}
/**
@ -1538,133 +1783,152 @@ void UAVObjInstanceUpdated(UAVObjHandle obj, uint16_t instId)
*/
void UAVObjIterate(void (*iterator) (UAVObjHandle obj))
{
ObjectList *objEntry;
PIOS_Assert(iterator);
// Get lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Get lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Iterate through the list and invoke iterator for each object
LL_FOREACH(objList, objEntry) {
(*iterator) ((UAVObjHandle) objEntry);
}
// Iterate through the list and invoke iterator for each object
struct UAVOData *obj;
LL_FOREACH(uavo_list, obj) {
(*iterator) ((UAVObjHandle) obj);
(*iterator) ((UAVObjHandle) &obj->metaObj);
}
// Release lock
xSemaphoreGiveRecursive(mutex);
// Release lock
xSemaphoreGiveRecursive(mutex);
}
/**
* Send an event to all event queues registered on the object.
* Send a triggered event to all event queues registered on the object.
*/
static int32_t sendEvent(ObjectList * obj, uint16_t instId,
UAVObjEventType event)
static int32_t sendEvent(struct UAVOBase * obj, uint16_t instId,
UAVObjEventType triggered_event)
{
ObjectEventList *eventEntry;
UAVObjEvent msg;
/* Set up the message that will be sent to all registered listeners */
UAVObjEvent msg = {
.obj = (UAVObjHandle) obj,
.event = triggered_event,
.instId = instId,
};
// Setup event
msg.obj = (UAVObjHandle) obj;
msg.event = event;
msg.instId = instId;
// Go through each object and push the event message in the queue (if event is activated for the queue)
struct ObjectEventEntry *event;
LL_FOREACH(obj->next_event, event) {
if (event->eventMask == 0
|| (event->eventMask & triggered_event) != 0) {
// Send to queue if a valid queue is registered
if (event->queue) {
// will not block
if (xQueueSend(event->queue, &msg, 0) != pdTRUE) {
stats.lastQueueErrorID = UAVObjGetID(obj);
++stats.eventQueueErrors;
}
}
// Go through each object and push the event message in the queue (if event is activated for the queue)
LL_FOREACH(obj->events, eventEntry) {
if (eventEntry->eventMask == 0
|| (eventEntry->eventMask & event) != 0) {
// Send to queue if a valid queue is registered
if (eventEntry->queue != 0) {
if (xQueueSend(eventEntry->queue, &msg, 0) != pdTRUE) // will not block
{
stats.lastQueueErrorID = UAVObjGetID(obj);
++stats.eventQueueErrors;
}
}
// Invoke callback (from event task) if a valid one is registered
if (eventEntry->cb != 0) {
if (EventCallbackDispatch(&msg, eventEntry->cb) != pdTRUE) // invoke callback from the event task, will not block
{
++stats.eventCallbackErrors;
stats.lastCallbackErrorID = UAVObjGetID(obj);
}
}
}
}
// Invoke callback (from event task) if a valid one is registered
if (event->cb) {
// invoke callback from the event task, will not block
if (EventCallbackDispatch(&msg, event->cb) != pdTRUE) {
++stats.eventCallbackErrors;
stats.lastCallbackErrorID = UAVObjGetID(obj);
}
}
}
}
// Done
return 0;
return 0;
}
/**
* Create a new object instance, return the instance info or NULL if failure.
*/
static ObjectInstList *createInstance(ObjectList * obj, uint16_t instId)
static InstanceHandle createInstance(struct UAVOData * obj, uint16_t instId)
{
ObjectInstList *instEntry;
int32_t n;
struct UAVOMultiInst *instEntry;
// For single instance objects, only instance zero is allowed
if (OLGetIsSingleInstance(obj) && instId != 0) {
return NULL;
}
// Make sure that the instance ID is within limits
if (instId >= UAVOBJ_MAX_INSTANCES) {
return NULL;
}
// Check if the instance already exists
if (getInstance(obj, instId) != NULL) {
return NULL;
}
// Create any missing instances (all instance IDs must be sequential)
for (n = obj->numInstances; n < instId; ++n) {
if (createInstance(obj, n) == NULL) {
return NULL;
}
}
/* Don't allow more than one instance for single instance objects */
if (UAVObjIsSingleInstance(&(obj->base))) {
PIOS_Assert(0);
return NULL;
}
if (instId == 0) { /* Instance 0 ObjectInstList allocated with ObjectList element */
instEntry = &obj->instances;
instEntry->data = pvPortMalloc(obj->numBytes);
if (instEntry->data == NULL)
return NULL;
memset(instEntry->data, 0, obj->numBytes);
instEntry->instId = instId;
} else {
// Create the actual instance
instEntry =
(ObjectInstList *)
pvPortMalloc(sizeof(ObjectInstList));
if (instEntry == NULL)
return NULL;
instEntry->data = pvPortMalloc(obj->numBytes);
if (instEntry->data == NULL)
return NULL;
memset(instEntry->data, 0, obj->numBytes);
instEntry->instId = instId;
LL_APPEND(obj->instances.next, instEntry);
}
++obj->numInstances;
/* Don't create more than the allowed number of instances */
if (instId >= UAVOBJ_MAX_INSTANCES) {
return NULL;
}
// Fire event
UAVObjInstanceUpdated((UAVObjHandle) obj, instId);
/* Don't allow duplicate instances */
if (instId < UAVObjGetNumInstances(&(obj->base))) {
return NULL;
}
// Done
return instEntry;
// Create any missing instances (all instance IDs must be sequential)
for (uint16_t n = UAVObjGetNumInstances(&(obj->base)); n < instId; ++n) {
if (createInstance(obj, n) == NULL) {
return NULL;
}
}
/* Create the actual instance */
instEntry = (struct UAVOMultiInst *) pvPortMalloc(sizeof(struct UAVOMultiInst)+obj->instance_size);
if (!instEntry)
return NULL;
memset(InstanceDataOffset(instEntry), 0, obj->instance_size);
LL_APPEND(( (struct UAVOMulti*)obj )->instance0.next, instEntry);
( (struct UAVOMulti*)obj )->num_instances++;
// Fire event
UAVObjInstanceUpdated((UAVObjHandle) obj, instId);
// Done
return InstanceDataOffset(instEntry);
}
/**
* Get the instance information or NULL if the instance does not exist
*/
static ObjectInstList *getInstance(ObjectList * obj, uint16_t instId)
static InstanceHandle getInstance(struct UAVOData * obj, uint16_t instId)
{
ObjectInstList *instEntry;
if (UAVObjIsMetaobject(&obj->base)) {
/* Metadata Instance */
// Look for specified instance ID
LL_FOREACH(&(obj->instances), instEntry) {
if (instEntry->instId == instId) {
return instEntry;
}
}
// If this point is reached then instance id was not found
return NULL;
if (instId != 0)
return NULL;
/* Augment our pointer to reflect the proper type */
struct UAVOMeta * uavo_meta = (struct UAVOMeta *) obj;
return (&(uavo_meta->instance0));
} else if (UAVObjIsSingleInstance(&(obj->base))) {
/* Single Instance */
if (instId != 0)
return NULL;
/* Augment our pointer to reflect the proper type */
struct UAVOSingle * uavo_single = (struct UAVOSingle *) obj;
return (&(uavo_single->instance0));
} else {
/* Multi Instance */
/* Augment our pointer to reflect the proper type */
struct UAVOMulti * uavo_multi = (struct UAVOMulti *) obj;
if (instId >= uavo_multi->num_instances)
return NULL;
// Look for specified instance ID
uint16_t instance = 0;
struct UAVOMultiInst *instEntry;
LL_FOREACH(&(uavo_multi->instance0), instEntry) {
if (instance++ == instId) {
/* Found it */
return &(instEntry->instance);
}
}
/* Instance was not found */
return NULL;
}
}
/**
@ -1675,35 +1939,34 @@ static ObjectInstList *getInstance(ObjectList * obj, uint16_t instId)
* \param[in] eventMask The event mask, if EV_MASK_ALL then all events are enabled (e.g. EV_UPDATED | EV_UPDATED_MANUAL)
* \return 0 if success or -1 if failure
*/
static int32_t connectObj(UAVObjHandle obj, xQueueHandle queue,
UAVObjEventCallback cb, uint8_t eventMask)
static int32_t connectObj(UAVObjHandle obj_handle, xQueueHandle queue,
UAVObjEventCallback cb, uint8_t eventMask)
{
ObjectEventList *eventEntry;
ObjectList *objEntry;
struct ObjectEventEntry *event;
struct UAVOBase *obj;
// Check that the queue is not already connected, if it is simply update event mask
objEntry = (ObjectList *) obj;
LL_FOREACH(objEntry->events, eventEntry) {
if (eventEntry->queue == queue && eventEntry->cb == cb) {
// Already connected, update event mask and return
eventEntry->eventMask = eventMask;
return 0;
}
}
// Check that the queue is not already connected, if it is simply update event mask
obj = (struct UAVOBase *) obj_handle;
LL_FOREACH(obj->next_event, event) {
if (event->queue == queue && event->cb == cb) {
// Already connected, update event mask and return
event->eventMask = eventMask;
return 0;
}
}
// Add queue to list
eventEntry =
(ObjectEventList *) pvPortMalloc(sizeof(ObjectEventList));
if (eventEntry == NULL) {
return -1;
}
eventEntry->queue = queue;
eventEntry->cb = cb;
eventEntry->eventMask = eventMask;
LL_APPEND(objEntry->events, eventEntry);
// Add queue to list
event = (struct ObjectEventEntry *) pvPortMalloc(sizeof(struct ObjectEventEntry));
if (event == NULL) {
return -1;
}
event->queue = queue;
event->cb = cb;
event->eventMask = eventMask;
LL_APPEND(obj->next_event, event);
// Done
return 0;
// Done
return 0;
}
/**
@ -1713,25 +1976,25 @@ static int32_t connectObj(UAVObjHandle obj, xQueueHandle queue,
* \param[in] cb The event callback
* \return 0 if success or -1 if failure
*/
static int32_t disconnectObj(UAVObjHandle obj, xQueueHandle queue,
UAVObjEventCallback cb)
static int32_t disconnectObj(UAVObjHandle obj_handle, xQueueHandle queue,
UAVObjEventCallback cb)
{
ObjectEventList *eventEntry;
ObjectList *objEntry;
struct ObjectEventEntry *event;
struct UAVOBase *obj;
// Find queue and remove it
objEntry = (ObjectList *) obj;
LL_FOREACH(objEntry->events, eventEntry) {
if ((eventEntry->queue == queue
&& eventEntry->cb == cb)) {
LL_DELETE(objEntry->events, eventEntry);
vPortFree(eventEntry);
return 0;
}
}
// Find queue and remove it
obj = (struct UAVOBase *) obj_handle;
LL_FOREACH(obj->next_event, event) {
if ((event->queue == queue
&& event->cb == cb)) {
LL_DELETE(obj->next_event, event);
vPortFree(event);
return 0;
}
}
// If this point is reached the queue was not found
return -1;
// If this point is reached the queue was not found
return -1;
}
#if defined(PIOS_INCLUDE_SDCARD)
@ -1740,16 +2003,16 @@ static int32_t disconnectObj(UAVObjHandle obj, xQueueHandle queue,
*/
static void customSPrintf(uint8_t * buffer, uint8_t * format, ...)
{
va_list args;
va_start(args, format);
vsprintf((char *)buffer, (char *)format, args);
va_list args;
va_start(args, format);
vsprintf((char *)buffer, (char *)format, args);
}
/**
* Get an 8 character (plus extension) filename for the object.
*/
static void objectFilename(ObjectList * obj, uint8_t * filename)
static void objectFilename(UAVObjHandle obj_handle, uint8_t * filename)
{
customSPrintf(filename, (uint8_t *) "%X.obj", obj->id);
customSPrintf(filename, (uint8_t *) "%X.obj", UAVObjGetID(obj_handle));
}
#endif /* PIOS_INCLUDE_SDCARD */

2
flight/UAVObjects/uavobjecttemplate.c
View File

@ -54,7 +54,7 @@ int32_t $(NAME)Initialize(void)
return -2;
// Register object with the object manager
handle = UAVObjRegister($(NAMEUC)_OBJID, $(NAMEUC)_NAME, $(NAMEUC)_METANAME, 0,
handle = UAVObjRegister($(NAMEUC)_OBJID,
$(NAMEUC)_ISSINGLEINST, $(NAMEUC)_ISSETTINGS, $(NAMEUC)_NUMBYTES, &$(NAME)SetDefaults);
// Done

2
ground/openpilotgcs/src/plugins/config/airframe.ui
View File

@ -2788,7 +2788,7 @@ p, li { white-space: pre-wrap; }
&lt;table border=&quot;0&quot; style=&quot;-qt-table-type: root; margin-top:4px; margin-bottom:4px; margin-left:4px; margin-right:4px;&quot;&gt;
&lt;tr&gt;
&lt;td style=&quot;border: none;&quot;&gt;
&lt;p align=&quot;center&quot; style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-family:'Ubuntu'; font-size:14pt; font-weight:600; color:#ff0000;&quot;&gt;SETTING UP FEED FORWARD IS DANGEROUS&lt;/span&gt;&lt;/p&gt;
&lt;p align=&quot;center&quot; style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-family:'Ubuntu'; font-size:14pt; font-weight:600; color:#ff0000;&quot;&gt;SETTING UP FEED FORWARD NEEDS CAUTION&lt;/span&gt;&lt;/p&gt;
&lt;p style=&quot;-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'Ubuntu'; font-size:11pt;&quot;&gt;&lt;/p&gt;
&lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;Beware: Feed Forward Tuning will launch all engines around mid-throttle, you have been warned!&lt;/p&gt;
&lt;p style=&quot; margin-top:12px; margin-bottom:12px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;Remove your props initially, and for fine-tuning, make sure your airframe is safely held in place. Wear glasses and protect your face and body.&lt;/p&gt;&lt;/td&gt;&lt;/tr&gt;&lt;/table&gt;&lt;/body&gt;&lt;/html&gt;</string>

2
ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.cpp
View File

@ -274,6 +274,8 @@ QStringList ConfigMultiRotorWidget::getChannelDescriptions()
channelDesc[multi.VTOLMotorW-1] = QString("VTOLMotorW");
if (multi.VTOLMotorE > 0 && multi.VTOLMotorE < ConfigMultiRotorWidget::CHANNEL_NUMELEM)
channelDesc[multi.VTOLMotorE-1] = QString("VTOLMotorE");
if (multi.TRIYaw > 0 && multi.TRIYaw <= ConfigMultiRotorWidget::CHANNEL_NUMELEM)
channelDesc[multi.TRIYaw-1] = QString("Tri-Yaw");
return channelDesc;
}

27
ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp
View File

@ -71,6 +71,8 @@ ConfigPipXtremeWidget::ConfigPipXtremeWidget(QWidget *parent) : ConfigTaskWidget
addUAVObjectToWidgetRelation("PipXStatus", "MinFrequency", m_pipx->MinFrequency);
addUAVObjectToWidgetRelation("PipXStatus", "MaxFrequency", m_pipx->MaxFrequency);
addUAVObjectToWidgetRelation("PipXStatus", "FrequencyStepSize", m_pipx->FrequencyStepSize);
addUAVObjectToWidgetRelation("PipXStatus", "FrequencyBand", m_pipx->FreqBand);
addUAVObjectToWidgetRelation("PipXStatus", "RSSI", m_pipx->RSSI);
addUAVObjectToWidgetRelation("PipXStatus", "AFC", m_pipx->RxAFC);
addUAVObjectToWidgetRelation("PipXStatus", "Retries", m_pipx->Retries);
addUAVObjectToWidgetRelation("PipXStatus", "Errors", m_pipx->Errors);
@ -81,6 +83,7 @@ ConfigPipXtremeWidget::ConfigPipXtremeWidget(QWidget *parent) : ConfigTaskWidget
addUAVObjectToWidgetRelation("PipXStatus", "TXRate", m_pipx->TXRate);
// Connect to the pair ID radio buttons.
connect(m_pipx->PairSelectB, SIGNAL(toggled(bool)), this, SLOT(pairBToggled(bool)));
connect(m_pipx->PairSelect1, SIGNAL(toggled(bool)), this, SLOT(pair1Toggled(bool)));
connect(m_pipx->PairSelect2, SIGNAL(toggled(bool)), this, SLOT(pair2Toggled(bool)));
connect(m_pipx->PairSelect3, SIGNAL(toggled(bool)), this, SLOT(pair3Toggled(bool)));
@ -89,6 +92,7 @@ ConfigPipXtremeWidget::ConfigPipXtremeWidget(QWidget *parent) : ConfigTaskWidget
//Add scroll bar when necessary
QScrollArea *scroll = new QScrollArea;
scroll->setWidget(m_pipx->frame_3);
scroll->setWidgetResizable(true);
m_pipx->verticalLayout_3->addWidget(scroll);
// Request and update of the setting object.
@ -148,7 +152,7 @@ void ConfigPipXtremeWidget::updateStatus(UAVObject *object)
PipXSettings *pipxSettings = PipXSettings::GetInstance(getObjectManager());
quint32 pairID = 0;
if (pipxSettings)
pipxSettings->getPairID();
pairID = pipxSettings->getPairID();
// Update the detected devices.
UAVObjectField* pairIdField = object->getField("PairIDs");
@ -245,6 +249,9 @@ void ConfigPipXtremeWidget::updateStatus(UAVObject *object)
qDebug() << "PipXtremeGadgetWidget: Count not read DeviceID field.";
}
// Update the PairID field
m_pipx->PairID->setText(QString::number(pairID, 16).toUpper());
// Update the link state
UAVObjectField* linkField = object->getField("LinkState");
if (linkField) {
@ -284,9 +291,16 @@ void ConfigPipXtremeWidget::pairIDToggled(bool checked, quint8 idx)
if (pipxStatus && pipxSettings)
{
quint32 pairID = pipxStatus->getPairIDs(idx);
if (pairID)
pipxSettings->setPairID(pairID);
if (idx == 4)
{
pipxSettings->setPairID(0);
}
else
{
quint32 pairID = pipxStatus->getPairIDs(idx);
if (pairID)
pipxSettings->setPairID(pairID);
}
}
}
}
@ -311,6 +325,11 @@ void ConfigPipXtremeWidget::pair4Toggled(bool checked)
pairIDToggled(checked, 3);
}
void ConfigPipXtremeWidget::pairBToggled(bool checked)
{
pairIDToggled(checked, 4);
}
/**
@}
@}

1
ground/openpilotgcs/src/plugins/config/configpipxtremewidget.h
View File

@ -63,6 +63,7 @@ private slots:
void pair2Toggled(bool checked);
void pair3Toggled(bool checked);
void pair4Toggled(bool checked);
void pairBToggled(bool checked);
};
#endif // CONFIGTXPIDWIDGET_H

2
ground/openpilotgcs/src/plugins/config/input.ui
View File

@ -17,7 +17,7 @@
<item>
<widget class="QTabWidget" name="tabWidget">
<property name="currentIndex">
<number>1</number>
<number>0</number>
</property>
<widget class="QWidget" name="RCInput">
<attribute name="title">

792
ground/openpilotgcs/src/plugins/config/pipxtreme.ui
View File

@ -6,8 +6,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>840</width>
<height>834</height>
<width>834</width>
<height>772</height>
</rect>
</property>
<property name="windowTitle">
@ -81,7 +81,7 @@
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
<height>5</height>
</size>
</property>
</spacer>
@ -135,16 +135,56 @@
</property>
<layout class="QGridLayout" name="gridLayout_3">
<item row="0" column="0">
<widget class="QRadioButton" name="PairSelectB">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLineEdit" name="PairIDB">
<property name="enabled">
<bool>false</bool>
</property>
<property name="maximumSize">
<size>
<width>100</width>
<height>16777215</height>
</size>
</property>
<property name="text">
<string>Broadcast</string>
</property>
</widget>
</item>
<item row="0" column="2">
<widget class="QLabel" name="PaidIDBLabel">
<property name="text">
<string>Broadcast Address</string>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QRadioButton" name="PairSelect1">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="0" column="1" colspan="2">
<widget class="QLineEdit" name="PairID1"/>
<item row="1" column="1">
<widget class="QLineEdit" name="PairID1">
<property name="maximumSize">
<size>
<width>100</width>
<height>16777215</height>
</size>
</property>
<property name="placeholderText">
<string>12345678</string>
</property>
</widget>
</item>
<item row="0" column="3">
<item row="1" column="2">
<widget class="QProgressBar" name="PairSignalStrengthBar1">
<property name="minimum">
<number>-127</number>
@ -163,17 +203,31 @@
</property>
</widget>
</item>
<item row="1" column="0">
<item row="1" column="3">
<widget class="QLabel" name="PairSignalStrengthLabel1">
<property name="text">
<string>-100dB</string>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QRadioButton" name="PairSelect2">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="1" column="1" colspan="2">
<widget class="QLineEdit" name="PairID2"/>
<item row="2" column="1">
<widget class="QLineEdit" name="PairID2">
<property name="maximumSize">
<size>
<width>100</width>
<height>16777215</height>
</size>
</property>
</widget>
</item>
<item row="1" column="3">
<item row="2" column="2">
<widget class="QProgressBar" name="PairSignalStrengthBar2">
<property name="minimum">
<number>-127</number>
@ -192,17 +246,31 @@
</property>
</widget>
</item>
<item row="2" column="0">
<item row="2" column="3">
<widget class="QLabel" name="PairSignalStrengthLabel2">
<property name="text">
<string>-100dB</string>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QRadioButton" name="PairSelect3">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="2" column="1" colspan="2">
<widget class="QLineEdit" name="PairID3"/>
<item row="3" column="1">
<widget class="QLineEdit" name="PairID3">
<property name="maximumSize">
<size>
<width>100</width>
<height>16777215</height>
</size>
</property>
</widget>
</item>
<item row="2" column="3">
<item row="3" column="2">
<widget class="QProgressBar" name="PairSignalStrengthBar3">
<property name="minimum">
<number>-127</number>
@ -221,17 +289,31 @@
</property>
</widget>
</item>
<item row="3" column="0">
<item row="3" column="3">
<widget class="QLabel" name="PairSignalStrengthLabel3">
<property name="text">
<string>-100dB</string>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QRadioButton" name="PairSelect4">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="3" column="1" colspan="2">
<widget class="QLineEdit" name="PairID4"/>
<item row="4" column="1">
<widget class="QLineEdit" name="PairID4">
<property name="maximumSize">
<size>
<width>100</width>
<height>16777215</height>
</size>
</property>
</widget>
</item>
<item row="3" column="3">
<item row="4" column="2">
<widget class="QProgressBar" name="PairSignalStrengthBar4">
<property name="minimum">
<number>-127</number>
@ -250,28 +332,7 @@
</property>
</widget>
</item>
<item row="0" column="4">
<widget class="QLabel" name="PairSignalStrengthLabel1">
<property name="text">
<string>-100dB</string>
</property>
</widget>
</item>
<item row="1" column="4">
<widget class="QLabel" name="PairSignalStrengthLabel2">
<property name="text">
<string>-100dB</string>
</property>
</widget>
</item>
<item row="2" column="4">
<widget class="QLabel" name="PairSignalStrengthLabel3">
<property name="text">
<string>-100dB</string>
</property>
</widget>
</item>
<item row="3" column="4">
<item row="4" column="3">
<widget class="QLabel" name="PairSignalStrengthLabel4">
<property name="text">
<string>-100dB</string>
@ -285,7 +346,7 @@
<widget class="QGroupBox" name="groupBox_3">
<property name="minimumSize">
<size>
<width>400</width>
<width>430</width>
<height>0</height>
</size>
</property>
@ -302,15 +363,27 @@
<item row="0" column="0">
<widget class="QLabel" name="label_9">
<property name="text">
<string>Firmware Version</string>
<string>Firmware Ver.</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
</widget>
</item>
<item row="0" column="1">
<item row="0" column="1" colspan="3">
<widget class="QLineEdit" name="FirmwareVersion">
<property name="minimumSize">
<size>
<width>0</width>
<height>0</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<height>16777215</height>
</size>
</property>
<property name="font">
<font>
<weight>75</weight>
@ -321,7 +394,7 @@
<string notr="true">QLineEdit {
border: none;
border-radius: 1px;
padding: 0 8px;
padding: 0 4px;
background: rgba(0, 0, 0, 16);
/* background: transparent; */
/* selection-background-color: darkgray;*/
@ -345,7 +418,7 @@
</property>
</widget>
</item>
<item row="2" column="1">
<item row="2" column="1" colspan="3">
<widget class="QLineEdit" name="SerialNumber">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Fixed">
@ -353,6 +426,12 @@
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>0</width>
<height>0</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
@ -376,7 +455,7 @@
<string notr="true">QLineEdit {
border: none;
border-radius: 1px;
padding: 0 8px;
padding: 0 4px;
background: rgba(0, 0, 0, 16);
/* background: transparent; */
/* selection-background-color: darkgray;*/
@ -393,6 +472,108 @@
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QLabel" name="DeviceIDLabel">
<property name="text">
<string>Device ID</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLineEdit" name="DeviceID">
<property name="minimumSize">
<size>
<width>0</width>
<height>0</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>101</width>
<height>16777215</height>
</size>
</property>
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="styleSheet">
<string notr="true">QLineEdit {
border: none;
border-radius: 1px;
padding: 0 4px;
background: rgba(0, 0, 0, 16);
/* background: transparent; */
/* selection-background-color: darkgray;*/
}</string>
</property>
<property name="frame">
<bool>false</bool>
</property>
<property name="alignment">
<set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
</property>
<property name="readOnly">
<bool>true</bool>
</property>
<property name="placeholderText">
<string>12345678</string>
</property>
</widget>
</item>
<item row="3" column="2">
<widget class="QLabel" name="PairIDLabel">
<property name="text">
<string>Pair ID</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
</widget>
</item>
<item row="3" column="3">
<widget class="QLineEdit" name="PairID">
<property name="maximumSize">
<size>
<width>101</width>
<height>16777215</height>
</size>
</property>
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="styleSheet">
<string notr="true">QLineEdit {
border: none;
border-radius: 1px;
padding: 0 4px;
background: rgba(0, 0, 0, 16);
/* background: transparent; */
/* selection-background-color: darkgray;*/
}</string>
</property>
<property name="frame">
<bool>false</bool>
</property>
<property name="alignment">
<set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
</property>
<property name="readOnly">
<bool>true</bool>
</property>
<property name="placeholderText">
<string>90ABCDEF</string>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QLabel" name="label_3">
<property name="text">
@ -411,9 +592,15 @@
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>0</width>
<height>0</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<width>101</width>
<height>16777215</height>
</size>
</property>
@ -430,7 +617,7 @@
<string notr="true">QLineEdit {
border: none;
border-radius: 1px;
padding: 0 8px;
padding: 0 4px;
background: rgba(0, 0, 0, 16);
/* background: transparent; */
/* selection-background-color: darkgray;*/
@ -444,7 +631,7 @@
</property>
</widget>
</item>
<item row="5" column="0">
<item row="4" column="2">
<widget class="QLabel" name="label_4">
<property name="text">
<string>Max Frequency</string>
@ -454,7 +641,7 @@
</property>
</widget>
</item>
<item row="5" column="1">
<item row="4" column="3">
<widget class="QLineEdit" name="MaxFrequency">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Fixed">
@ -464,7 +651,7 @@
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<width>101</width>
<height>16777215</height>
</size>
</property>
@ -481,7 +668,7 @@
<string notr="true">QLineEdit {
border: none;
border-radius: 1px;
padding: 0 8px;
padding: 0 4px;
background: rgba(0, 0, 0, 16);
/* background: transparent; */
/* selection-background-color: darkgray;*/
@ -495,7 +682,7 @@
</property>
</widget>
</item>
<item row="6" column="0">
<item row="5" column="0">
<widget class="QLabel" name="label_12">
<property name="text">
<string>Freq. Step Size</string>
@ -505,7 +692,7 @@
</property>
</widget>
</item>
<item row="6" column="1">
<item row="5" column="1">
<widget class="QLineEdit" name="FrequencyStepSize">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Fixed">
@ -513,9 +700,15 @@
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>0</width>
<height>0</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<width>101</width>
<height>16777215</height>
</size>
</property>
@ -532,7 +725,7 @@
<string notr="true">QLineEdit {
border: none;
border-radius: 1px;
padding: 0 8px;
padding: 0 4px;
background: rgba(0, 0, 0, 16);
/* background: transparent; */
/* selection-background-color: darkgray;*/
@ -546,7 +739,232 @@
</property>
</widget>
</item>
<item row="5" column="2">
<widget class="QLabel" name="FreqBandLabel">
<property name="text">
<string>Freq. Band</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
</widget>
</item>
<item row="5" column="3">
<widget class="QLineEdit" name="FreqBand">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="maximumSize">
<size>
<width>101</width>
<height>16777215</height>
</size>
</property>
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="toolTip">
<string>The current frequency band</string>
</property>
<property name="styleSheet">
<string notr="true">QLineEdit {
border: none;
border-radius: 1px;
padding: 0 4px;
background: rgba(0, 0, 0, 16);
/* background: transparent; */
/* selection-background-color: darkgray;*/
}</string>
</property>
<property name="alignment">
<set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
</property>
<property name="readOnly">
<bool>true</bool>
</property>
</widget>
</item>
<item row="6" column="0">
<widget class="QLabel" name="RSSILabel">
<property name="text">
<string>RSSI</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QLineEdit" name="RSSI">
<property name="minimumSize">
<size>
<width>0</width>
<height>0</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>101</width>
<height>16777215</height>
</size>
</property>
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="styleSheet">
<string notr="true">QLineEdit {
border: none;
border-radius: 1px;
padding: 0 4px;
background: rgba(0, 0, 0, 16);
/* background: transparent; */
/* selection-background-color: darkgray;*/
}</string>
</property>
<property name="readOnly">
<bool>true</bool>
</property>
</widget>
</item>
<item row="6" column="2">
<widget class="QLabel" name="RxAFCLabel">
<property name="text">
<string>Rx AFC</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
</widget>
</item>
<item row="6" column="3">
<widget class="QLineEdit" name="RxAFC">
<property name="maximumSize">
<size>
<width>101</width>
<height>16777215</height>
</size>
</property>
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="styleSheet">
<string notr="true">QLineEdit {
border: none;
border-radius: 1px;
padding: 0 4px;
background: rgba(0, 0, 0, 16);
/* background: transparent; */
/* selection-background-color: darkgray;*/
}</string>
</property>
<property name="readOnly">
<bool>true</bool>
</property>
</widget>
</item>
<item row="7" column="0">
<widget class="QLabel" name="TXRateLabel">
<property name="text">
<string>TX Rate (B/s)</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
</widget>
</item>
<item row="7" column="1">
<widget class="QLineEdit" name="TXRate">
<property name="minimumSize">
<size>
<width>0</width>
<height>0</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>101</width>
<height>16777215</height>
</size>
</property>
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="styleSheet">
<string notr="true">QLineEdit {
border: none;
border-radius: 1px;
padding: 0 4px;
background: rgba(0, 0, 0, 16);
/* background: transparent; */
/* selection-background-color: darkgray;*/
}</string>
</property>
<property name="frame">
<bool>false</bool>
</property>
<property name="readOnly">
<bool>true</bool>
</property>
</widget>
</item>
<item row="7" column="2">
<widget class="QLabel" name="RXRateLabel">
<property name="text">
<string>RX Rate (B/s)</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
</widget>
</item>
<item row="7" column="3">
<widget class="QLineEdit" name="RXRate">
<property name="maximumSize">
<size>
<width>101</width>
<height>16777215</height>
</size>
</property>
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="styleSheet">
<string notr="true">QLineEdit {
border: none;
border-radius: 1px;
padding: 0 4px;
background: rgba(0, 0, 0, 16);
/* background: transparent; */
/* selection-background-color: darkgray;*/
}</string>
</property>
<property name="frame">
<bool>false</bool>
</property>
<property name="readOnly">
<bool>true</bool>
</property>
</widget>
</item>
<item row="8" column="0">
<widget class="QLabel" name="label_11">
<property name="text">
<string>Link State</string>
@ -556,17 +974,23 @@
</property>
</widget>
</item>
<item row="7" column="1">
<item row="8" column="1">
<widget class="QLineEdit" name="LinkState">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Fixed">
<sizepolicy hsizetype="Expanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>0</width>
<height>0</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>16777215</width>
<width>101</width>
<height>16777215</height>
</size>
</property>
@ -583,7 +1007,7 @@
<string notr="true">QLineEdit {
border: none;
border-radius: 1px;
padding: 0 8px;
padding: 0 3px;
background: rgba(0, 0, 0, 16);
/* background: transparent; */
/* selection-background-color: darkgray;*/
@ -595,20 +1019,29 @@
<property name="readOnly">
<bool>true</bool>
</property>
<property name="placeholderText">
<string>Disconnected</string>
</property>
</widget>
</item>
<item row="8" column="0">
<widget class="QLabel" name="label_17">
<item row="8" column="2">
<widget class="QLabel" name="label_19">
<property name="text">
<string>Rx AFC</string>
<string>Errors</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
</widget>
</item>
<item row="8" column="1">
<widget class="QLineEdit" name="RxAFC">
<item row="8" column="3">
<widget class="QLineEdit" name="Errors">
<property name="maximumSize">
<size>
<width>101</width>
<height>16777215</height>
</size>
</property>
<property name="font">
<font>
<weight>75</weight>
@ -619,12 +1052,15 @@
<string notr="true">QLineEdit {
border: none;
border-radius: 1px;
padding: 0 8px;
padding: 0 4px;
background: rgba(0, 0, 0, 16);
/* background: transparent; */
/* selection-background-color: darkgray;*/
}</string>
</property>
<property name="frame">
<bool>false</bool>
</property>
<property name="readOnly">
<bool>true</bool>
</property>
@ -642,6 +1078,18 @@
</item>
<item row="9" column="1">
<widget class="QLineEdit" name="Retries">
<property name="minimumSize">
<size>
<width>0</width>
<height>0</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>101</width>
<height>16777215</height>
</size>
</property>
<property name="font">
<font>
<weight>75</weight>
@ -652,7 +1100,7 @@
<string notr="true">QLineEdit {
border: none;
border-radius: 1px;
padding: 0 8px;
padding: 0 4px;
background: rgba(0, 0, 0, 16);
/* background: transparent; */
/* selection-background-color: darkgray;*/
@ -666,8 +1114,24 @@
</property>
</widget>
</item>
<item row="10" column="1">
<widget class="QLineEdit" name="Errors">
<item row="9" column="2">
<widget class="QLabel" name="UAVTalkErrorsLabel">
<property name="text">
<string>UAVTalk Errors</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
</widget>
</item>
<item row="9" column="3">
<widget class="QLineEdit" name="UAVTalkErrors">
<property name="maximumSize">
<size>
<width>101</width>
<height>16777215</height>
</size>
</property>
<property name="font">
<font>
<weight>75</weight>
@ -678,7 +1142,7 @@
<string notr="true">QLineEdit {
border: none;
border-radius: 1px;
padding: 0 8px;
padding: 0 4px;
background: rgba(0, 0, 0, 16);
/* background: transparent; */
/* selection-background-color: darkgray;*/
@ -693,52 +1157,6 @@
</widget>
</item>
<item row="10" column="0">
<widget class="QLabel" name="label_19">
<property name="text">
<string>Errors</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
</widget>
</item>
<item row="11" column="0">
<widget class="QLabel" name="UAVTalkErrorsLabel">
<property name="text">
<string>UAVTalk Errors</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
</widget>
</item>
<item row="11" column="1">
<widget class="QLineEdit" name="UAVTalkErrors">
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="styleSheet">
<string notr="true">QLineEdit {
border: none;
border-radius: 1px;
padding: 0 8px;
background: rgba(0, 0, 0, 16);
/* background: transparent; */
/* selection-background-color: darkgray;*/
}</string>
</property>
<property name="frame">
<bool>false</bool>
</property>
<property name="readOnly">
<bool>true</bool>
</property>
</widget>
</item>
<item row="12" column="0">
<widget class="QLabel" name="ResetsLabel">
<property name="text">
<string>Resets</string>
@ -748,8 +1166,20 @@
</property>
</widget>
</item>
<item row="12" column="1">
<item row="10" column="1">
<widget class="QLineEdit" name="Resets">
<property name="minimumSize">
<size>
<width>0</width>
<height>0</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>101</width>
<height>16777215</height>
</size>
</property>
<property name="font">
<font>
<weight>75</weight>
@ -760,7 +1190,7 @@
<string notr="true">QLineEdit {
border: none;
border-radius: 1px;
padding: 0 8px;
padding: 0 4px;
background: rgba(0, 0, 0, 16);
/* background: transparent; */
/* selection-background-color: darkgray;*/
@ -774,7 +1204,7 @@
</property>
</widget>
</item>
<item row="13" column="0">
<item row="10" column="2">
<widget class="QLabel" name="DroppedLabel">
<property name="text">
<string>Dropped</string>
@ -784,8 +1214,14 @@
</property>
</widget>
</item>
<item row="13" column="1">
<item row="10" column="3">
<widget class="QLineEdit" name="Dropped">
<property name="maximumSize">
<size>
<width>101</width>
<height>16777215</height>
</size>
</property>
<property name="font">
<font>
<weight>75</weight>
@ -796,7 +1232,7 @@
<string notr="true">QLineEdit {
border: none;
border-radius: 1px;
padding: 0 8px;
padding: 0 4px;
background: rgba(0, 0, 0, 16);
/* background: transparent; */
/* selection-background-color: darkgray;*/
@ -810,114 +1246,6 @@
</property>
</widget>
</item>
<item row="14" column="0">
<widget class="QLabel" name="TXRateLabel">
<property name="text">
<string>TX Rate (B/s)</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
</widget>
</item>
<item row="14" column="1">
<widget class="QLineEdit" name="TXRate">
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="styleSheet">
<string notr="true">QLineEdit {
border: none;
border-radius: 1px;
padding: 0 8px;
background: rgba(0, 0, 0, 16);
/* background: transparent; */
/* selection-background-color: darkgray;*/
}</string>
</property>
<property name="frame">
<bool>false</bool>
</property>
<property name="readOnly">
<bool>true</bool>
</property>
</widget>
</item>
<item row="15" column="0">
<widget class="QLabel" name="RXRateLabel">
<property name="text">
<string>RX Rate (B/s)</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
</widget>
</item>
<item row="15" column="1">
<widget class="QLineEdit" name="RXRate">
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="styleSheet">
<string notr="true">QLineEdit {
border: none;
border-radius: 1px;
padding: 0 8px;
background: rgba(0, 0, 0, 16);
/* background: transparent; */
/* selection-background-color: darkgray;*/
}</string>
</property>
<property name="frame">
<bool>false</bool>
</property>
<property name="readOnly">
<bool>true</bool>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLineEdit" name="DeviceID">
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="styleSheet">
<string notr="true">QLineEdit {
border: none;
border-radius: 1px;
padding: 0 8px;
background: rgba(0, 0, 0, 16);
/* background: transparent; */
/* selection-background-color: darkgray;*/
}</string>
</property>
<property name="frame">
<bool>false</bool>
</property>
<property name="readOnly">
<bool>true</bool>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QLabel" name="label_20">
<property name="text">
<string>Device ID</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
</widget>
</item>
</layout>
</widget>
</item>
@ -926,6 +1254,12 @@
</item>
<item row="0" column="1">
<widget class="QGroupBox" name="groupBox">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="font">
<font>
<weight>75</weight>

48
ground/openpilotgcs/src/plugins/gcscontrol/Matlab/GCSControl.m Normal file
View File

@ -0,0 +1,48 @@
% GCSCONTROL
% This class allows the user to send 4-axis stick commands to OpenPilot
% GCS.
%
% Create class by
% control = GCSControl
%
% Open connection by
% control.connect('01.23.45.67', 89)
% where the first value is the IP address of the computer running GCS and
% the second value is the port on which GCS is listening.
%
% Send command by
% control.command(pitch, yaw, roll, throttle)
% where all variables are between [-1,1]
%
% Close connection by
% control.close()
classdef GCSControl < handle
properties
udpObj;
isConnected=false;
end
methods
function obj=GCSControl()
obj.isConnected = false;
end
function obj=connect(obj,rhost,rport)
obj.udpObj = udp(rhost,rport);
fopen(obj.udpObj);
obj.isConnected = true;
end
function obj=command(obj,pitch,yaw,roll,throttle)
if(obj.isConnected)
fwrite(obj.udpObj,[42,pitch,yaw,roll,throttle,36],'double')
end
end
function obj=close(obj)
if(obj.isConnected)
fclose(obj.udpObj);
obj.isConnected = false;
end
end
end
end

1
ground/openpilotgcs/src/plugins/gcscontrol/gcscontrol.pro
View File

@ -2,6 +2,7 @@ TEMPLATE = lib
TARGET = GCSControl
QT += svg
QT += opengl
QT += network
include(../../openpilotgcsplugin.pri)
include(../../plugins/coreplugin/coreplugin.pri)

13
ground/openpilotgcs/src/plugins/gcscontrol/gcscontrol.ui
View File

@ -41,8 +41,21 @@
</property>
</widget>
</item>
<item>
<widget class="QCheckBox" name="checkBoxUDPControl">
<property name="enabled">
<bool>false</bool>
</property>
<property name="text">
<string>UDP Control</string>
</property>
</widget>
</item>
<item>
<widget class="QCheckBox" name="checkBoxArmed">
<property name="enabled">
<bool>true</bool>
</property>
<property name="text">
<string>Armed</string>
</property>

106
ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp
View File

@ -44,6 +44,10 @@ GCSControlGadget::GCSControlGadget(QString classId, GCSControlGadgetWidget *widg
manualControlCommandUpdated(getManualControlCommand());
control_sock = new QUdpSocket(this);
connect(control_sock,SIGNAL(readyRead()),this,SLOT(readUDPCommand()));
joystickTime.start();
GCSControlPlugin *pl = dynamic_cast<GCSControlPlugin*>(plugin);
connect(pl->sdlGamepad,SIGNAL(gamepads(quint8)),this,SLOT(gamepads(quint8)));
@ -67,6 +71,12 @@ void GCSControlGadget::loadConfiguration(IUAVGadgetConfiguration* config)
yawChannel = ql.at(2);
throttleChannel = ql.at(3);
// if(control_sock->isOpen())
// control_sock->close();
control_sock->bind(GCSControlConfig->getUDPControlHost(), GCSControlConfig->getUDPControlPort(),QUdpSocket::ShareAddress);
controlsMode = GCSControlConfig->getControlsMode();
int i;
@ -174,7 +184,8 @@ void GCSControlGadget::sticksChangedLocally(double leftX, double leftY, double r
}
//if we are not in local gcs control mode, ignore the joystick input
if (((GCSControlGadgetWidget *)m_widget)->getGCSControl()==false)return;
if (((GCSControlGadgetWidget *)m_widget)->getGCSControl()==false || ((GCSControlGadgetWidget *)m_widget)->getUDPControl())
return;
if((newThrottle != oldThrottle) || (newPitch != oldPitch) || (newYaw != oldYaw) || (newRoll != oldRoll)) {
if (buttonRollControl==0)obj->getField("Roll")->setDouble(newRoll);
@ -191,6 +202,93 @@ void GCSControlGadget::gamepads(quint8 count)
// sdlGamepad.setTickRate(JOYSTICK_UPDATE_RATE);
}
void GCSControlGadget::readUDPCommand()
{
double pitch, yaw, roll, throttle;
while (control_sock->hasPendingDatagrams()) {
QByteArray datagram;
datagram.resize(control_sock->pendingDatagramSize());
control_sock->readDatagram(datagram.data(), datagram.size());
QDataStream readData(datagram);
bool badPack = false;
int state = 0;
while(!readData.atEnd() && !badPack)
{
double buffer;
readData >> buffer;
switch(state)
{
case 0:
if(buffer == 42){
state = 1;
}else{
state = 0;
badPack = true;
}
break;
case 1:
pitch = buffer;
state = 2;
break;
case 2:
yaw = buffer;
state = 3;
break;
case 3:
roll = buffer;
state = 4;
break;
case 4:
throttle = buffer;
state = 5;
break;
case 5:
if(buffer != 36 || !readData.atEnd())
badPack=true;
break;
}
}
if(!badPack && ((GCSControlGadgetWidget *)m_widget)->getUDPControl())
{
ManualControlCommand * obj = getManualControlCommand();
bool update = false;
if(pitch != obj->getField("Pitch")->getDouble()){
obj->getField("Pitch")->setDouble(constrain(pitch));
update = true;
}
if(yaw != obj->getField("Yaw")->getDouble()){
obj->getField("Yaw")->setDouble(constrain(yaw));
update = true;
}
if(roll != obj->getField("Roll")->getDouble()){
obj->getField("Roll")->setDouble(constrain(roll));
update = true;
}
if(throttle != obj->getField("Throttle")->getDouble()){
obj->getField("Throttle")->setDouble(constrain(throttle));
update = true;
}
if(update)
obj->updated();
}
}
qDebug() << "Pitch: " << pitch << " Yaw: " << yaw << " Roll: " << roll << " Throttle: " << throttle;
}
double GCSControlGadget::constrain(double value)
{
if(value < -1)
return -1;
if(value > 1)
return 1;
return value;
}
void GCSControlGadget::buttonState(ButtonNumber number, bool pressed)
{
int state;
@ -200,6 +298,7 @@ void GCSControlGadget::buttonState(ButtonNumber number, bool pressed)
UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
UAVDataObject* obj = dynamic_cast<UAVDataObject*>( objManager->getObject(QString("ManualControlCommand")) );
bool currentCGSControl = ((GCSControlGadgetWidget *)m_widget)->getGCSControl();
bool currentUDPControl = ((GCSControlGadgetWidget *)m_widget)->getUDPControl();
switch (buttonSettings[number].ActionID)
{
@ -268,6 +367,11 @@ void GCSControlGadget::buttonState(ButtonNumber number, bool pressed)
((GCSControlGadgetWidget *)m_widget)->setGCSControl(!currentCGSControl);
break;
case 3: //UDP Control
if(currentCGSControl)
((GCSControlGadgetWidget *)m_widget)->setUDPControl(!currentUDPControl);
break;
}
break;

7
ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.h
View File

@ -34,6 +34,9 @@
#include "sdlgamepad/sdlgamepad.h"
#include <QTime>
#include "gcscontrolplugin.h"
#include <QUdpSocket>
#include <QHostAddress>
namespace Core {
class IUAVGadget;
@ -59,6 +62,7 @@ public:
private:
ManualControlCommand* getManualControlCommand();
double constrain(double value);
QTime joystickTime;
QWidget *m_widget;
QList<int> m_context;
@ -72,6 +76,8 @@ private:
double bound(double input);
double wrap(double input);
bool channelReverse[8];
QUdpSocket *control_sock;
signals:
void sticksChangedRemotely(double leftX, double leftY, double rightX, double rightY);
@ -79,6 +85,7 @@ signals:
protected slots:
void manualControlCommandUpdated(UAVObject *);
void sticksChangedLocally(double leftX, double leftY, double rightX, double rightY);
void readUDPCommand();
// signals from joystick
void gamepads(quint8 count);

24
ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetconfiguration.cpp
View File

@ -54,6 +54,9 @@ GCSControlGadgetConfiguration::GCSControlGadgetConfiguration(QString classId, QS
yawChannel = qSettings->value("yawChannel").toInt();
throttleChannel = qSettings->value("throttleChannel").toInt();
udp_port = qSettings->value("controlPortUDP").toUInt();
udp_host = QHostAddress(qSettings->value("controlHostUDP").toString());
int i;
for (i=0;i<8;i++)
{
@ -66,6 +69,21 @@ GCSControlGadgetConfiguration::GCSControlGadgetConfiguration(QString classId, QS
}
void GCSControlGadgetConfiguration::setUDPControlSettings(int port, QString host)
{
udp_port = port;
udp_host = QHostAddress(host);
}
int GCSControlGadgetConfiguration::getUDPControlPort()
{
return udp_port;
}
QHostAddress GCSControlGadgetConfiguration::getUDPControlHost()
{
return udp_host;
}
void GCSControlGadgetConfiguration::setRPYTchannels(int roll, int pitch, int yaw, int throttle) {
rollChannel = roll;
pitchChannel = pitch;
@ -102,6 +120,9 @@ IUAVGadgetConfiguration *GCSControlGadgetConfiguration::clone()
m->yawChannel = yawChannel;
m->throttleChannel = throttleChannel;
m->udp_host = udp_host;
m->udp_port = udp_port;
int i;
for (i=0;i<8;i++)
{
@ -126,6 +147,9 @@ void GCSControlGadgetConfiguration::saveConfig(QSettings* settings) const {
settings->setValue("yawChannel", yawChannel);
settings->setValue("throttleChannel", throttleChannel);
settings->setValue("controlPortUDP",QString::number(udp_port));
settings->setValue("controlHostUDP",udp_host.toString());
int i;
for (i=0;i<8;i++)
{

11
ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetconfiguration.h
View File

@ -29,6 +29,7 @@
#define GCSCONTROLGADGETCONFIGURATION_H
#include <coreplugin/iuavgadgetconfiguration.h>
#include <QtNetwork/QHostAddress>
typedef struct{
int ActionID;
@ -36,6 +37,11 @@ typedef struct{
double Amount;
}buttonSettingsStruct;
typedef struct{
int port;
QHostAddress address;
}portSettingsStruct;
using namespace Core;
@ -49,6 +55,9 @@ class GCSControlGadgetConfiguration : public IUAVGadgetConfiguration
void setControlsMode(int mode) { controlsMode = mode; }
void setRPYTchannels(int roll, int pitch, int yaw, int throttle);
void setUDPControlSettings(int port, QString host);
int getUDPControlPort();
QHostAddress getUDPControlHost();
int getControlsMode() { return controlsMode; }
QList<int> getChannelsMapping();
QList<bool> getChannelsReverse();
@ -72,6 +81,8 @@ class GCSControlGadgetConfiguration : public IUAVGadgetConfiguration
int throttleChannel;
buttonSettingsStruct buttonSettings[8];
bool channelReverse[8];
int udp_port;
QHostAddress udp_host;
};

11
ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.cpp
View File

@ -137,7 +137,7 @@ QWidget *GCSControlGadgetOptionsPage::createPage(QWidget *parent)
options_page->buttonFunction4 << options_page->buttonFunction5 <<
options_page->buttonFunction6 << options_page->buttonFunction7;
QStringList buttonOptions;
buttonOptions <<"-" << "Roll" << "Pitch" << "Yaw" << "Throttle" << "Armed" << "GCS Control" ;
buttonOptions <<"-" << "Roll" << "Pitch" << "Yaw" << "Throttle" << "Armed" << "GCS Control"; //added UDP control to action list
foreach (QComboBox* qb, buttonFunctionList) {
qb->addItems(buttonOptions);
}
@ -187,6 +187,9 @@ QWidget *GCSControlGadgetOptionsPage::createPage(QWidget *parent)
//updateButtonFunction();
options_page->udp_host->setText(m_config->getUDPControlHost().toString());
options_page->udp_port->setText(QString::number(m_config->getUDPControlPort()));
// Controls mode are from 1 to 4.
if (m_config->getControlsMode()>0 && m_config->getControlsMode() < 5)
@ -262,6 +265,9 @@ void GCSControlGadgetOptionsPage::apply()
}
m_config->setRPYTchannels(roll,pitch,yaw,throttle);
m_config->setUDPControlSettings(options_page->udp_port->text().toInt(),options_page->udp_host->text());
int j;
for (j=0;j<8;j++)
{
@ -271,6 +277,7 @@ void GCSControlGadgetOptionsPage::apply()
m_config->setChannelReverse(j,chRevList.at(j)->isChecked());
}
}
void GCSControlGadgetOptionsPage::finish()
@ -369,7 +376,7 @@ void GCSControlGadgetOptionsPage::updateButtonAction(int controlID)
if (buttonActionList.at(i)->currentText().compare("Toggles")==0)
{
disconnect(buttonFunctionList.at(i),SIGNAL(currentIndexChanged(int)),this,SLOT(updateButtonFunction()));
buttonOptions <<"-" << "Armed" << "GCS Control" ;
buttonOptions <<"-" << "Armed" << "GCS Control" << "UDP Control";
buttonFunctionList.at(i)->clear();
buttonFunctionList.at(i)->insertItems(-1,buttonOptions);

62
ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.ui
View File

@ -141,7 +141,7 @@
</widget>
</item>
<item>
<widget class="QTabWidget" name="tabWidget">
<widget class="QTabWidget" name="UDPSetup">
<property name="currentIndex">
<number>0</number>
</property>
@ -1011,6 +1011,66 @@
</item>
</layout>
</widget>
<widget class="QWidget" name="UDPSetup_2">
<attribute name="title">
<string>UDP Setup</string>
</attribute>
<widget class="QGroupBox" name="groupBox">
<property name="geometry">
<rect>
<x>20</x>
<y>20</y>
<width>301</width>
<height>71</height>
</rect>
</property>
<property name="title">
<string>UDP Port Configuration</string>
</property>
<layout class="QHBoxLayout" name="horizontalLayout_2">
<item>
<widget class="QLabel" name="label_7">
<property name="text">
<string>Host:</string>
</property>
</widget>
</item>
<item>
<widget class="QLineEdit" name="udp_host">
<property name="maximumSize">
<size>
<width>120</width>
<height>16777215</height>
</size>
</property>
<property name="text">
<string>127.0.0.1</string>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label_6">
<property name="text">
<string>Port:</string>
</property>
</widget>
</item>
<item>
<widget class="QLineEdit" name="udp_port">
<property name="maximumSize">
<size>
<width>50</width>
<height>16777215</height>
</size>
</property>
<property name="text">
<string>2323</string>
</property>
</widget>
</item>
</layout>
</widget>
</widget>
</widget>
</item>
</layout>

33
ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.cpp
View File

@ -34,6 +34,7 @@
#include <QtGui/QVBoxLayout>
#include <QtGui/QPushButton>
#include "uavobject.h"
#include "uavobjectmanager.h"
#include "manualcontrolcommand.h"
@ -64,9 +65,14 @@ GCSControlGadgetWidget::GCSControlGadgetWidget(QWidget *parent) : QLabel(parent)
connect(m_gcscontrol->checkBoxArmed, SIGNAL(stateChanged(int)), this, SLOT(toggleArmed(int)));
connect(m_gcscontrol->comboBoxFlightMode, SIGNAL(currentIndexChanged(int)), this, SLOT(selectFlightMode(int)));
connect(m_gcscontrol->checkBoxUDPControl, SIGNAL(stateChanged(int)),this,SLOT(toggleUDPControl(int))); //UDP control checkbox
// Connect object updated event from UAVObject to also update check boxes and dropdown
connect(obj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(mccChanged(UAVObject*)));
leftX = 0;
leftY = 0;
rightX = 0;
@ -122,11 +128,14 @@ void GCSControlGadgetWidget::toggleControl(int state)
UAVObject::SetGcsTelemetryAcked(mdata, false);
UAVObject::SetGcsTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_ONCHANGE);
mdata.gcsTelemetryUpdatePeriod = 100;
m_gcscontrol->checkBoxUDPControl->setEnabled(true);
}
else
{
mdata = mccInitialData;
toggleUDPControl(false);
m_gcscontrol->checkBoxUDPControl->setEnabled(false);
}
obj->setMetadata(mdata);
}
@ -152,6 +161,16 @@ void GCSControlGadgetWidget::mccChanged(UAVObject * obj)
m_gcscontrol->checkBoxArmed->setChecked(flightStatus->getField("Armed")->getValue() == "Armed");
}
void GCSControlGadgetWidget::toggleUDPControl(int state)
{
if(state)
{
setUDPControl(true);
}else{
setUDPControl(false);
}
}
/*!
\brief Called when the flight mode drop down is changed and sets the ManualControlCommand->FlightMode accordingly
*/
@ -168,11 +187,21 @@ void GCSControlGadgetWidget::selectFlightMode(int state)
void GCSControlGadgetWidget::setGCSControl(bool newState)
{
m_gcscontrol->checkBoxGcsControl->setChecked(newState);
};
}
bool GCSControlGadgetWidget::getGCSControl(void)
{
return m_gcscontrol->checkBoxGcsControl->isChecked();
};
}
void GCSControlGadgetWidget::setUDPControl(bool newState)
{
m_gcscontrol->checkBoxUDPControl->setChecked(newState);
}
bool GCSControlGadgetWidget::getUDPControl(void)
{
return m_gcscontrol->checkBoxUDPControl->isChecked();
}
/**

5
ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.h
View File

@ -31,6 +31,8 @@
#include <QtGui/QLabel>
#include "manualcontrolcommand.h"
#define UDP_PORT 2323
class Ui_GCSControl;
class GCSControlGadgetWidget : public QLabel
@ -42,6 +44,8 @@ public:
~GCSControlGadgetWidget();
void setGCSControl(bool newState);
bool getGCSControl(void);
void setUDPControl(bool newState);
bool getUDPControl(void);
signals:
void sticksChanged(double leftX, double leftY, double rightX, double rightY);
@ -59,6 +63,7 @@ protected slots:
void toggleArmed(int state);
void selectFlightMode(int state);
void mccChanged(UAVObject *);
void toggleUDPControl(int state);
private:
Ui_GCSControl *m_gcscontrol;

2
ground/openpilotgcs/src/plugins/gpsdisplay/gpsconstellationwidget.cpp
View File

@ -123,7 +123,7 @@ void GpsConstellationWidget::updateSat(int index, int prn, int elevation, int az
satellites[index][2] = azimuth;
satellites[index][3] = snr;
if (prn) {
if (prn && elevation >= 0) {
QPointF opd = polarToCoord(elevation,azimuth);
opd += QPointF(-satIcons[index]->boundingRect().center().x(),
-satIcons[index]->boundingRect().center().y());

27
ground/openpilotgcs/src/plugins/hitlv2/aerosimrc.cpp
View File

@ -1,3 +1,30 @@
/**
******************************************************************************
*
* @file aerosimrc.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
* @addtogroup GCSPlugins GCS Plugins
* @{
* @addtogroup HITLPlugin HITLv2 Plugin
* @{
* @brief The Hardware In The Loop plugin version 2
*****************************************************************************/
/*
* 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 "aerosimrc.h"
#include <extensionsystem/pluginmanager.h>
#include <coreplugin/icore.h>

27
ground/openpilotgcs/src/plugins/hitlv2/aerosimrc.h
View File

@ -1,3 +1,30 @@
/**
******************************************************************************
*
* @file aerosimrc.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
* @addtogroup GCSPlugins GCS Plugins
* @{
* @addtogroup HITLPlugin HITLv2 Plugin
* @{
* @brief The Hardware In The Loop plugin version 2
*****************************************************************************/
/*
* 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 AEROSIMRC_H
#define AEROSIMRC_H

10
ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/aerosimrc.pro Normal file
View File

@ -0,0 +1,10 @@
TEMPLATE = subdirs
win32 {
SUBDIRS += plugin
}
SUBDIRS += udptest
plugin.file = src/plugin.pro
udptest.file = src/udptest.pro

215
ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/aerosimrcdatastruct.h Normal file
View File

@ -0,0 +1,215 @@
/**
******************************************************************************
*
* @file aerosimrcdatastruct.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
* @addtogroup 3rdParty Third-party integration
* @{
* @addtogroup AeroSimRC AeroSimRC proxy plugin
* @{
* @brief AeroSimRC simulator to HITL proxy plugin
*****************************************************************************/
/*
* 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 AEROSIMRCDATASTRUCT_H
#define AEROSIMRCDATASTRUCT_H
#include <QtCore>
const quint8 AEROSIMRC_MAX_CHANNELS = 39;
const quint16 DBG_BUFFER_MAX_SIZE = 4096;
#define MAX_DLL_USER_MENU_ITEMS 16
#define OBSOLETE_MIT_COMMAND (1 << 0)
#define OBSOLETE_MIT_CHECKBOX (1 << 1)
#define OBSOLETE_MIT_SEPARATOR (1 << 7)
#if defined(Q_CC_MSVC)
#define PACK_STRUCT
#define MAX_PATH 260
#pragma pack (push, r1, 1)
#elif defined(Q_CC_GNU)
#define PACK_STRUCT __attribute__((packed))
#endif
struct simToPlugin
{
quint16 structSize;
float simTimeStep;
float chSimTX[AEROSIMRC_MAX_CHANNELS];
float chSimRX[AEROSIMRC_MAX_CHANNELS];
uchar *OSDVideoBuf;
quint32 simMenuStatus;
float initPosX;
float initPosY;
float initPosZ;
float initHeading;
float initPitch;
float initRoll;
float wpHomeX;
float wpHomeY;
float wpHomeLat;
float wpHomeLong;
const char *wpHomeDesc; // (m, deg, string)
float wpAX;
float wpAY;
float wpALat;
float wpALong;
const char *wpADesc; // (m, deg, string)
float wpBX;
float wpBY;
float wpBLat;
float wpBLong;
const char *wpBDesc; // (m, deg, string)
float wpCX;
float wpCY;
float wpCLat;
float wpCLong;
const char *wpCDesc; // (m, deg, string)
float wpDX;
float wpDY;
float wpDLat;
float wpDLong;
const char *wpDDesc; // (m, deg, string)
float posX;
float posY;
float posZ;
float velX;
float velY;
float velZ;
float angVelX;
float angVelY;
float angVelZ;
float accelX;
float accelY;
float accelZ;
qreal latitude;
qreal longitude;
float AGL;
float heading;
float pitch;
float roll;
float windVelX;
float windVelY;
float windVelZ;
float eng1RPM;
float eng2RPM;
float eng3RPM;
float eng4RPM;
float voltage; // V
float current; // A
float consumedCharge; // Ah
float capacity; // Ah
float fuelConsumed; // l
float fuelTankCapacity; // l
// ver 3.83
qint16 screenW;
qint16 screenH;
// Model Orientation Matrix (X=Right, Y=Front, Z=Up)
float axisXx;
float axisXy;
float axisXz;
float axisYx;
float axisYy;
float axisYz;
float axisZx;
float axisZy;
float axisZz;
// Model data in body frame coordinates (X=Right, Y=Front, Z=Up)
float velXm; // m/s Model velocity in body coordinates
float velYm;
float velZm;
float angVelXm; // rad/s Model angular velocity in body coordinates
float angVelYm;
float angVelZm;
float accelXm; // m/s/s Model acceleration in body coordinates
float accelYm;
float accelZm;
// ver 3.90
quint32 OSDVideoBufSize;
} PACK_STRUCT ; // normal - 592, packed - 582 OK (3.81)
// normal - ???, packed - 658 OK (3.83)
// normal - ???, packed - 662 OK (3.90)
struct pluginToSim
{
quint16 structSize;
const char *dbgInfoText;
uchar chOverTX[AEROSIMRC_MAX_CHANNELS];
float chNewTX[AEROSIMRC_MAX_CHANNELS];
uchar chOverRX[AEROSIMRC_MAX_CHANNELS];
float chNewRX[AEROSIMRC_MAX_CHANNELS];
float newPosX; // m
float newPosY;
float newPosZ;
float newVelX; // m/s
float newVelY;
float newVelZ;
float newAngVelX; // rad/s
float newAngVelY;
float newAngVelZ;
float newHeading; // rad
float newPitch;
float newRoll;
quint32 modelOverrideFlags;
quint32 newMenuStatus;
quint8 isOSDShow;
quint8 isOSDChanged;
quint16 OSDWindow_DX;
quint16 OSDWindow_DY;
float OSDScale;
float newWindVelX;
float newWindVelY;
float newWindVelZ;
float newEng1RPM;
float newEng2RPM;
float newEng3RPM;
float newEng4RPM;
float newVoltage;
float newCurrent;
float newConsumedCharge;
float newFuelConsumed;
quint8 modelCrashInhibit;
// ver 3.83
qint16 newScreenW; // Simulator window position and size on screen
qint16 newScreenH;
qint16 newScreenX;
qint16 newScreenY;
} PACK_STRUCT ; // normal 516, packed 507 OK (3.81)
// normal ???, packed 515 OK (3.83 & 3.90)
struct TPluginMenuItem
{
quint32 OBSOLETE_eType;
char *OBSOLETE_strName;
} PACK_STRUCT ;
struct pluginInit
{
quint32 nStructSize;
char *OBSOLETE_strMenuTitle;
TPluginMenuItem OBSOLETE_atMenuItem[MAX_DLL_USER_MENU_ITEMS];
const char *strPluginFolder;
const char *strOutputFolder;
} PACK_STRUCT ; // normal - 144, packed - 144 OK (3.81 & 3.83 & 3.90)
#ifdef Q_CC_MSVC
#pragma pack (pop, r1)
#endif
#undef PACK_STRUCT
#endif // AEROSIMRCDATASTRUCT_H

102
ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/enums.h Normal file
View File

@ -0,0 +1,102 @@
/**
******************************************************************************
*
* @file enums.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
* @addtogroup 3rdParty Third-party integration
* @{
* @addtogroup AeroSimRC AeroSimRC proxy plugin
* @{
* @brief AeroSimRC simulator to HITL proxy plugin
*****************************************************************************/
/*
* 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 ENUMS_H
#define ENUMS_H
// Custom Menu Item masks
enum MenuMasks {
MenuEnable = (1 << 0),
MenuTx = (1 << 1),
MenuRx = (1 << 2),
MenuScreen = (1 << 3),
MenuNextWpt = (1 << 4),
MenuCmdReset = (1 << 5),
MenuLedBlue = (1 << 6),
MenuLedGreen = (1 << 7),
MenuFMode1 = (1 << 8),
MenuFMode2 = (1 << 9),
MenuFMode3 = (1 << 10)
};
enum EOverrideFlags
{
OVR_POS = (1 << 0),
OVR_VEL = (1 << 1),
OVR_ANG_VEL = (1 << 2),
OVR_HPR = (1 << 3), // Override Heading, Pitch and Roll
OVR_WIND_VEL = (1 << 4), // Override Wind velocity at model
OVR_ENGINE_RPM = (1 << 5), // Override RPM of all Engines or Motors
OVR_BAT_VOLT = (1 << 6), // Override motor Battery Voltage
OVR_BAT_AMP = (1 << 7), // Override motor Battery current
OVR_BAT_AH_CONSUMED = (1 << 8), // Override motor Battery AmpsHour consumed
OVR_FUEL_CONSUMED = (1 << 9) // Override Fuel consumed (gas & jet engines)
};
enum Channels {
Ch1Aileron,
Ch2Elevator,
Ch3Throttle,
Ch4Rudder,
Ch5,
Ch6,
Ch7,
Ch8,
Ch9,
Ch10Retracts,
Ch11Flaps,
Ch12FPVCamPan,
Ch13FPVCamTilt,
Ch14Brakes,
Ch15Spoilers,
Ch16Smoke,
Ch17Fire,
Ch18FlightMode,
Ch19ALTHold,
Ch20FPVTiltHold,
Ch21ResetModel,
Ch22MouseTX,
Ch23Plugin1,
Ch24Plugin2,
Ch25ThrottleHold,
Ch26CareFree,
Ch27FPVCamRoll,
Ch28LMotorDual,
Ch29RMotorDual,
Ch30Mix,
Ch31Mix,
Ch32Mix,
Ch33Mix,
Ch34Mix,
Ch35Mix,
Ch36Mix,
Ch37Mix,
Ch38Mix,
Ch39Mix
};
#endif // ENUMS_H

394
ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/plugin.cpp Normal file
View File

@ -0,0 +1,394 @@
/**
******************************************************************************
*
* @file plugin.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
* @addtogroup 3rdParty Third-party integration
* @{
* @addtogroup AeroSimRC AeroSimRC proxy plugin
* @{
* @brief AeroSimRC simulator to HITL proxy plugin
*****************************************************************************/
/*
* 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 "plugin.h"
#include "udpconnect.h"
#include "qdebughandler.h"
#include "enums.h"
#include "settings.h"
bool isFirstRun = true;
QString debugInfo(DBG_BUFFER_MAX_SIZE, ' ');
QString pluginFolder(MAX_PATH, ' ');
QString outputFolder(MAX_PATH, ' ');
QList<quint16> videoModes;
QTime ledTimer;
UdpSender *sndr;
UdpReceiver *rcvr;
const float RAD2DEG = (float)(180.0 / M_PI);
const float DEG2RAD = (float)(M_PI / 180.0);
//extern "C" int __stdcall DllMain(HINSTANCE hinstDLL, DWORD fdwReason, LPVOID lpvReserved)
extern "C" int __stdcall DllMain(void*, quint32 fdwReason, void*)
{
switch (fdwReason) {
case 0:
// qDebug() << hinstDLL << "DLL_PROCESS_DETACH " << lpvReserved;
// free resources here
rcvr->stop();
rcvr->wait(500);
delete rcvr;
delete sndr;
qDebug("------");
break;
case 1:
// qDebug() << hinstDLL << " DLL_PROCESS_ATTACH " << lpvReserved;
break;
case 2:
// qDebug() << hinstDLL << "DLL_THREAD_ATTACH " << lpvReserved;
break;
case 3:
// qDebug() << hinstDLL << "DLL_THREAD_DETACH " << lpvReserved;
break;
}
return true;
}
SIM_DLL_EXPORT void AeroSIMRC_Plugin_ReportStructSizes(quint32 *sizeSimToPlugin,
quint32 *sizePluginToSim,
quint32 *sizePluginInit)
{
// debug redirection
qInstallMsgHandler(myQDebugHandler);
qDebug() << "AeroSIMRC_Plugin_ReportStructSizes";
*sizeSimToPlugin = sizeof(simToPlugin);
*sizePluginToSim = sizeof(pluginToSim);
*sizePluginInit = sizeof(pluginInit);
qDebug() << "sizeSimToPlugin = " << *sizeSimToPlugin;
qDebug() << "sizePluginToSim = " << *sizePluginToSim;
qDebug() << "sizePluginInit = " << *sizePluginInit;
}
SIM_DLL_EXPORT void AeroSIMRC_Plugin_Init(pluginInit *p)
{
qDebug() << "AeroSIMRC_Plugin_Init begin";
pluginFolder = p->strPluginFolder;
outputFolder = p->strOutputFolder;
ledTimer.restart();
Settings *ini = new Settings(pluginFolder);
ini->read();
videoModes = ini->getVideoModes();
sndr = new UdpSender(ini->getOutputMap(), ini->isFromTX());
sndr->init(ini->remoteHost(), ini->remotePort());
rcvr = new UdpReceiver(ini->getInputMap(), ini->isToRX());
rcvr->init(ini->localHost(), ini->localPort());
// run thread
rcvr->start();
delete ini;
qDebug() << "AeroSIMRC_Plugin_Init done";
}
//-----------------------------------------------------------------------------
void Run_Command_Reset(/*const simToPlugin *stp,
pluginToSim *pts*/)
{
// Print some debug info, although it will only be seen during one frame
debugInfo.append("\nRESET");
}
void Run_Command_WindowSizeAndPos(const simToPlugin *stp,
pluginToSim *pts)
{
static quint8 snSequence = 0;
quint8 idx = snSequence * 4;
if (snSequence >= videoModes.at(0)) { // set fullscreen
pts->newScreenX = 0;
pts->newScreenY = 0;
pts->newScreenW = stp->screenW;
pts->newScreenH = stp->screenH;
snSequence = 0;
} else { // set video mode from config
pts->newScreenX = videoModes.at(idx + 1);
pts->newScreenY = videoModes.at(idx + 2);
pts->newScreenW = videoModes.at(idx + 3);
pts->newScreenH = videoModes.at(idx + 4);
snSequence++;
}
}
void Run_Command_MoveToNextWaypoint(const simToPlugin *stp,
pluginToSim *pts)
{
static quint8 snSequence = 0;
switch(snSequence) {
case 0:
pts->newPosX = stp->wpAX;
pts->newPosY = stp->wpAY;
pts->newPosZ = 100.0;
break;
case 1:
pts->newPosX = stp->wpBX;
pts->newPosY = stp->wpBY;
pts->newPosZ = 100.0;
break;
case 2:
pts->newPosX = stp->wpCX;
pts->newPosY = stp->wpCY;
pts->newPosZ = 100.0;
break;
case 3:
pts->newPosX = stp->wpDX;
pts->newPosY = stp->wpDY;
pts->newPosZ = 100.0;
break;
case 4:
pts->newPosX = stp->wpHomeX;
pts->newPosY = stp->wpHomeY;
pts->newPosZ = 100.0;
break;
default:
qFatal("Run_Command_MoveToNextWaypoint switch error");
}
pts->modelOverrideFlags = 0;
pts->modelOverrideFlags |= OVR_POS;
snSequence++;
if(snSequence > 4)
snSequence = 0;
}
void Run_BlinkLEDs(const simToPlugin *stp,
pluginToSim *pts)
{
if ((stp->simMenuStatus & MenuEnable) != 0) {
pts->newMenuStatus |= MenuLedGreen;
int timeout;
quint8 armed;
quint8 mode;
rcvr->getFlighStatus(armed, mode);
debugInfo.append(QString("armed: %1, mode: %2\n").arg(armed).arg(mode));
if (armed == 0) // disarm
timeout = 1000;
else if (armed == 1) // arming
timeout = 40;
else if (armed == 2) // armed
timeout = 100;
else // unknown
timeout = 2000;
if (ledTimer.elapsed() > timeout) {
ledTimer.restart();
pts->newMenuStatus ^= MenuLedBlue;
}
if (mode == 6) {
pts->newMenuStatus |= MenuFMode3;
pts->newMenuStatus |= MenuFMode2;
pts->newMenuStatus |= MenuFMode1;
} else if (mode == 5) {
pts->newMenuStatus |= MenuFMode3;
pts->newMenuStatus |= MenuFMode2;
pts->newMenuStatus &= ~MenuFMode1;
} else if (mode == 4) {
pts->newMenuStatus |= MenuFMode3;
pts->newMenuStatus &= ~MenuFMode2;
pts->newMenuStatus |= MenuFMode1;
} else if (mode == 3) {
pts->newMenuStatus |= MenuFMode3;
pts->newMenuStatus &= ~MenuFMode2;
pts->newMenuStatus &= ~MenuFMode1;
} else if (mode == 2) {
pts->newMenuStatus &= ~MenuFMode3;
pts->newMenuStatus |= MenuFMode2;
pts->newMenuStatus &= ~MenuFMode1;
} else if (mode == 1) {
pts->newMenuStatus &= ~MenuFMode3;
pts->newMenuStatus &= ~MenuFMode2;
pts->newMenuStatus |= MenuFMode1;
} else /*(mode == 0)*/ {
pts->newMenuStatus &= ~MenuFMode3;
pts->newMenuStatus &= ~MenuFMode2;
pts->newMenuStatus &= ~MenuFMode1;
}
} else {
pts->newMenuStatus = 0;
}
}
void InfoText(const simToPlugin *stp,
pluginToSim *pts)
{
debugInfo.append(
QString(
"Plugin Folder = %1\n"
"Output Folder = %2\n"
"nStructSize = %3 "
"fIntegrationTimeStep = %4\n"
"\n"
"Aileron TX = %5 RX = %6 RCMD TX = %7 RX = %8\n"
"Elevator TX = %9 RX = %10 RCMD TX = %11 RX = %12\n"
"Throttle TX = %13 RX = %14 RCMD TX = %15 RX = %16\n"
"Rudder TX = %17 RX = %18 RCMD TX = %19 RX = %20\n"
"Channel5 TX = %21 RX = %22 RCMD TX = %23 RX = %24\n"
"Channel6 TX = %25 RX = %26 RCMD TX = %27 RX = %28\n"
"Channel7 TX = %29 RX = %30 RCMD TX = %31 RX = %32\n"
"PluginCh1 TX = %33 RX = %34 RCMD TX = %35 RX = %36\n"
"PluginCh2 TX = %37 RX = %38 RCMD TX = %39 RX = %40\n"
"FPVCamPan TX = %41 RX = %42 RCMD TX = %43 RX = %44\n"
"FPVCamTil TX = %45 RX = %46 RCMD TX = %47 RX = %48\n"
"\n"
"MenuItems = %49\n"
// Model data
"\n"
"fPosX,Y,Z = (%50, %51, %52)\n"
"fVelX,Y,Z = (%53, %54, %55)\n"
"fAngVelX,Y,Z = (%56, %57, %58)\n"
"fAccelX,Y,Z = (%59, %60, %61)\n"
"\n"
"Lat, Long = %62, %63\n"
"fHeightAboveTerrain = %64\n"
"\n"
"fHeading = %65 fPitch = %66 fRoll = %67\n"
)
.arg(pluginFolder)
.arg(outputFolder)
.arg(stp->structSize)
.arg(1.0 / stp->simTimeStep, 4, 'f', 1)
.arg(stp->chSimTX[Ch1Aileron], 5, 'f', 2)
.arg(stp->chSimRX[Ch1Aileron], 5, 'f', 2)
.arg(pts->chNewTX[Ch1Aileron], 5, 'f', 2)
.arg(pts->chNewRX[Ch1Aileron], 5, 'f', 2)
.arg(stp->chSimTX[Ch2Elevator], 5, 'f', 2)
.arg(stp->chSimRX[Ch2Elevator], 5, 'f', 2)
.arg(pts->chNewTX[Ch2Elevator], 5, 'f', 2)
.arg(pts->chNewRX[Ch2Elevator], 5, 'f', 2)
.arg(stp->chSimTX[Ch3Throttle], 5, 'f', 2)
.arg(stp->chSimRX[Ch3Throttle], 5, 'f', 2)
.arg(pts->chNewTX[Ch3Throttle], 5, 'f', 2)
.arg(pts->chNewRX[Ch3Throttle], 5, 'f', 2)
.arg(stp->chSimTX[Ch4Rudder], 5, 'f', 2)
.arg(stp->chSimRX[Ch4Rudder], 5, 'f', 2)
.arg(pts->chNewTX[Ch4Rudder], 5, 'f', 2)
.arg(pts->chNewRX[Ch4Rudder], 5, 'f', 2)
.arg(stp->chSimTX[Ch5], 5, 'f', 2)
.arg(stp->chSimRX[Ch5], 5, 'f', 2)
.arg(pts->chNewTX[Ch5], 5, 'f', 2)
.arg(pts->chNewRX[Ch5], 5, 'f', 2)
.arg(stp->chSimTX[Ch6], 5, 'f', 2)
.arg(stp->chSimRX[Ch6], 5, 'f', 2)
.arg(pts->chNewTX[Ch6], 5, 'f', 2)
.arg(pts->chNewRX[Ch6], 5, 'f', 2)
.arg(stp->chSimTX[Ch7], 5, 'f', 2)
.arg(stp->chSimRX[Ch7], 5, 'f', 2)
.arg(pts->chNewTX[Ch7], 5, 'f', 2)
.arg(pts->chNewRX[Ch7], 5, 'f', 2)
.arg(stp->chSimTX[Ch23Plugin1], 5, 'f', 2)
.arg(stp->chSimRX[Ch23Plugin1], 5, 'f', 2)
.arg(pts->chNewTX[Ch23Plugin1], 5, 'f', 2)
.arg(pts->chNewRX[Ch23Plugin1], 5, 'f', 2)
.arg(stp->chSimTX[Ch24Plugin2], 5, 'f', 2)
.arg(stp->chSimRX[Ch24Plugin2], 5, 'f', 2)
.arg(pts->chNewTX[Ch24Plugin2], 5, 'f', 2)
.arg(pts->chNewRX[Ch24Plugin2], 5, 'f', 2)
.arg(stp->chSimTX[Ch12FPVCamPan], 5, 'f', 2)
.arg(stp->chSimRX[Ch12FPVCamPan], 5, 'f', 2)
.arg(pts->chNewTX[Ch12FPVCamPan], 5, 'f', 2)
.arg(pts->chNewRX[Ch12FPVCamPan], 5, 'f', 2)
.arg(stp->chSimTX[Ch13FPVCamTilt], 5, 'f', 2)
.arg(stp->chSimRX[Ch13FPVCamTilt], 5, 'f', 2)
.arg(pts->chNewTX[Ch13FPVCamTilt], 5, 'f', 2)
.arg(pts->chNewRX[Ch13FPVCamTilt], 5, 'f', 2)
.arg(stp->simMenuStatus)
.arg(stp->posX, 5, 'f', 2)
.arg(stp->posY, 5, 'f', 2)
.arg(stp->posZ, 5, 'f', 2)
.arg(stp->velX, 5, 'f', 2)
.arg(stp->velY, 5, 'f', 2)
.arg(stp->velZ, 5, 'f', 2)
.arg(stp->angVelXm, 5, 'f', 2)
.arg(stp->angVelYm, 5, 'f', 2)
.arg(stp->angVelZm, 5, 'f', 2)
.arg(stp->accelXm, 5, 'f', 2)
.arg(stp->accelYm, 5, 'f', 2)
.arg(stp->accelZm, 5, 'f', 2)
.arg(stp->latitude, 5, 'f', 2)
.arg(stp->longitude, 5, 'f', 2)
.arg(stp->AGL, 5, 'f', 2)
.arg(stp->heading*RAD2DEG, 5, 'f', 2)
.arg(stp->pitch*RAD2DEG, 5, 'f', 2)
.arg(stp->roll*RAD2DEG, 5, 'f', 2)
);
}
SIM_DLL_EXPORT void AeroSIMRC_Plugin_Run(const simToPlugin *stp,
pluginToSim *pts)
{
debugInfo = "---\n";
// By default do not change the Menu Items of type CheckBox
pts->newMenuStatus = stp->simMenuStatus;
// Extract Menu Commands from Flags
bool isReset = (stp->simMenuStatus & MenuCmdReset) != 0;
bool isEnable = (stp->simMenuStatus & MenuEnable) != 0;
bool isTxON = (stp->simMenuStatus & MenuTx) != 0;
bool isRxON = (stp->simMenuStatus & MenuRx) != 0;
bool isScreen = (stp->simMenuStatus & MenuScreen) != 0;
bool isNextWp = (stp->simMenuStatus & MenuNextWpt) != 0;
// Run commands
if (isReset) {
Run_Command_Reset(/*stp, pts*/);
} else if (isScreen) {
Run_Command_WindowSizeAndPos(stp, pts);
} else if (isNextWp) {
Run_Command_MoveToNextWaypoint(stp, pts);
} else {
Run_BlinkLEDs(stp, pts);
if (isEnable) {
if (isTxON)
sndr->sendDatagram(stp);
if (isRxON)
rcvr->setChannels(pts);
}
// network lag
debugInfo.append(QString("out: %1, inp: %2, delta: %3\n")
.arg(sndr->pcks() - 1)
.arg(rcvr->pcks())
.arg(sndr->pcks() - rcvr->pcks() - 1)
);
}
// debug info is shown on the screen
InfoText(stp, pts);
pts->dbgInfoText = debugInfo.toAscii();
isFirstRun = false;
}

53
ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/plugin.h Normal file
View File

@ -0,0 +1,53 @@
/**
******************************************************************************
*
* @file plugin.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
* @addtogroup 3rdParty Third-party integration
* @{
* @addtogroup AeroSimRC AeroSimRC proxy plugin
* @{
* @brief AeroSimRC simulator to HITL proxy plugin
*****************************************************************************/
/*
* 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 PLUGIN_H
#define PLUGIN_H
#include <QtCore>
#include <QTime>
#include <QList>
#include "aerosimrcdatastruct.h"
#define SIM_DLL_EXPORT extern "C" __declspec(dllexport)
SIM_DLL_EXPORT void AeroSIMRC_Plugin_ReportStructSizes(
quint32 *sizeSimToPlugin,
quint32 *sizePluginToSim,
quint32 *sizePluginInit
);
SIM_DLL_EXPORT void AeroSIMRC_Plugin_Init(
pluginInit *p
);
SIM_DLL_EXPORT void AeroSIMRC_Plugin_Run(
const simToPlugin *stp,
pluginToSim *pts
);
#endif // PLUGIN_H

71
ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/plugin.pro Normal file
View File

@ -0,0 +1,71 @@
!win32 {
error("AeroSimRC plugin is only available for win32 platform")
}
include(../../../../../openpilotgcs.pri)
QT += network
QT -= gui
TEMPLATE = lib
TARGET = plugin_AeroSIMRC
RES_DIR = $${PWD}/resources
SIM_DIR = $$GCS_BUILD_TREE/../AeroSIM-RC
PLUGIN_DIR = $$SIM_DIR/Plugin/CopterControl
DLLDESTDIR = $$PLUGIN_DIR
# Don't depend on MSVRT*.dll
win32-msvc* {
QMAKE_CXXFLAGS_RELEASE -= -MD
QMAKE_CXXFLAGS_MT_DLL += -MT
}
HEADERS = \
aerosimrcdatastruct.h \
enums.h \
plugin.h \
qdebughandler.h \
udpconnect.h \
settings.h
SOURCES = \
qdebughandler.cpp \
plugin.cpp \
udpconnect.cpp \
settings.cpp
# Resemble the AeroSimRC directory structure and copy plugin files and resources
equals(copydata, 1) {
# Windows release only
win32:CONFIG(release, debug|release) {
data_copy.commands += -@$(MKDIR) $$targetPath(\"$$PLUGIN_DIR\") $$addNewline()
# resources and sample configuration
PLUGIN_RESOURCES = \
cc_off.tga \
cc_off_hover.tga \
cc_on.tga \
cc_on_hover.tga \
cc_plugin.ini \
plugin.txt
for(res, PLUGIN_RESOURCES) {
data_copy.commands += $(COPY_FILE) $$targetPath(\"$$RES_DIR/$$res\") $$targetPath(\"$$PLUGIN_DIR/$$res\") $$addNewline()
}
# Qt DLLs
QT_DLLS = \
libgcc_s_dw2-1.dll \
mingwm10.dll \
QtCore4.dll \
QtNetwork4.dll
for(dll, QT_DLLS) {
data_copy.commands += $(COPY_FILE) $$targetPath(\"$$[QT_INSTALL_BINS]/$$dll\") $$targetPath(\"$$SIM_DIR/$$dll\") $$addNewline()
}
data_copy.target = FORCE
QMAKE_EXTRA_TARGETS += data_copy
}
}

64
ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/qdebughandler.cpp Normal file
View File

@ -0,0 +1,64 @@
/**
******************************************************************************
*
* @file qdebughandler.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
* @addtogroup 3rdParty Third-party integration
* @{
* @addtogroup AeroSimRC AeroSimRC proxy plugin
* @{
* @brief AeroSimRC simulator to HITL proxy plugin
*****************************************************************************/
/*
* 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 "qdebughandler.h"
void myQDebugHandler(QtMsgType type, const char *msg)
{
static bool firstRun = true;
QString txt;
switch (type) {
case QtDebugMsg:
txt = QString("Debug: %1").arg(msg);
break;
case QtWarningMsg:
txt = QString("Warning: %1").arg(msg);
break;
case QtCriticalMsg:
txt = QString("Critical: %1").arg(msg);
break;
case QtFatalMsg:
txt = QString("Fatal: %1").arg(msg);
break;
}
QFile outFile("dbglog.txt");
outFile.open(QIODevice::WriteOnly | QIODevice::Append);
QTextStream ts(&outFile);
QTime time;
if (firstRun) {
ts << endl << endl;
firstRun = false;
}
ts << time.currentTime().toString("hh:mm:ss.zzz") << " " << txt << endl;
if (type == QtFatalMsg)
abort();
}

37
ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/qdebughandler.h Normal file
View File

@ -0,0 +1,37 @@
/**
******************************************************************************
*
* @file qdebughandler.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
* @addtogroup 3rdParty Third-party integration
* @{
* @addtogroup AeroSimRC AeroSimRC proxy plugin
* @{
* @brief AeroSimRC simulator to HITL proxy plugin
*****************************************************************************/
/*
* 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 QDEBUGHANDLER_H
#define QDEBUGHANDLER_H
#include <QDebug>
#include <QFile>
#include <QTime>
void myQDebugHandler(QtMsgType type, const char *msg);
#endif // QDEBUGHANDLER_H

BIN
ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/resources/cc_off.tga Normal file
View File

Binary file not shown.

BIN
ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/resources/cc_off_hover.tga Normal file
View File

Binary file not shown.

BIN
ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/resources/cc_on.tga Normal file
View File

Binary file not shown.

BIN
ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/resources/cc_on_hover.tga Normal file
View File

Binary file not shown.

55
ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/resources/cc_plugin.ini Normal file
View File

@ -0,0 +1,55 @@
[General]
; Network settings
listen_on_host = 127.0.0.1
listen_on_port = 40200
send_to_host = 127.0.0.1
send_to_port = 40100
; Channels enumerator, applicable for the AeroSIM RC version 3.90+
all_channels = Ch1-Aileron Ch2-Elevator Ch3-Throttle Ch4-Rudder Ch5 Ch6 Ch7 Ch8 Ch9 Ch10-Retracts Ch11-Flaps Ch12-FPV-Pan Ch13-FPV-Tilt Ch14-Brakes Ch15-Spoilers Ch16-Smoke Ch17-Fire Ch18-Flight-Mode Ch19-ALT-Hold Ch20-FPV-Tilt-Hold Ch21-Reset-Model Ch22-MouseTX Ch23-Plugin-1 Ch24-Plugin-2 Ch25-Throttle-Hold Ch26-CareFree Ch27-FPV-Roll Ch28-L-Motor-Dual Ch29-R-Motor-Dual Ch30-Mix Ch31-Mix Ch32-Mix Ch33-Mix Ch34-Mix Ch35-Mix Ch36-Mix Ch37-Mix Ch38-Mix Ch39-Mix
[Input]
; Map CopterControl channels to simulator channels
; To use internal simulator channels just comment the mapping here
cc_channel_1 = Ch1-Aileron
cc_channel_2 = Ch2-Elevator
cc_channel_3 = Ch3-Throttle
cc_channel_4 = Ch4-Rudder
;cc_channel_5 = Ch27-FPV-Roll
cc_channel_6 = Ch13-FPV-Tilt
;cc_channel_7 = Ch12-FPV-Pan
;cc_channel_8 =
;cc_channel_9 =
;cc_channel_10=
; Control TX or RX (before or after mixes)
send_to = RX
[Output]
; Map simulator channels to GCS HiTL/CopterControl channels
; Only mapped channels will be sent to the GCS
sim_channel_1 = Ch1-Aileron
sim_channel_2 = Ch2-Elevator
sim_channel_3 = Ch3-Throttle
sim_channel_4 = Ch4-Rudder
sim_channel_5 = Ch23-Plugin-1
;sim_channel_6 = Ch27-FPV-Roll
sim_channel_7 = Ch13-FPV-Tilt
;sim_channel_8 = Ch12-FPV-Pan
; take values from TX or RX (before or after mixes)
take_from = TX
[Video]
; Windowed simulator mode configurations
; Each resolution defines the upper left corner and width/hight.
; User can cycle through all resolutions using the menu command.
number_of_resolutions = 2
; x, y, width, height
resolution_1 = 50 50 640 720
resolution_2 = 0 0 800 480

86
ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/resources/plugin.txt Normal file
View File

@ -0,0 +1,86 @@
.NAME "CopterControl"
.HELP_EN "\
OpenPilot CopterControl HiTL plugin for AeroSIM RC"
.IMAGE_OFF "cc_off.tga"
.IMAGE_ON "cc_on.tga"
.IMAGE_OFF_MOUSE_HOVER "cc_off_hover.tga"
.IMAGE_ON_MOUSE_HOVER "cc_on_hover.tga"
.MENU_ITEM_00_NAME "Enable"
.MENU_ITEM_00_TYPE CHECKBOX
.MENU_ITEM_00_HAS_SEPARATOR 0
.MENU_ITEM_00_MOUSE_RECT_XY 89 91
.MENU_ITEM_00_MOUSE_RECT_DXDY 48 48
.MENU_ITEM_00_HIDE_MENU_ITEM 0
.MENU_ITEM_01_NAME "Transmit data"
.MENU_ITEM_01_TYPE CHECKBOX
.MENU_ITEM_01_HAS_SEPARATOR 0
.MENU_ITEM_01_MOUSE_RECT_XY 110 166
.MENU_ITEM_01_MOUSE_RECT_DXDY 52 34
.MENU_ITEM_01_HIDE_MENU_ITEM 0
.MENU_ITEM_02_NAME "Receive data"
.MENU_ITEM_02_TYPE CHECKBOX
.MENU_ITEM_02_HAS_SEPARATOR 1
.MENU_ITEM_02_MOUSE_RECT_XY 36 166
.MENU_ITEM_02_MOUSE_RECT_DXDY 52 34
.MENU_ITEM_02_HIDE_MENU_ITEM 0
.MENU_ITEM_03_NAME "Change window size and position"
.MENU_ITEM_03_TYPE COMMAND
.MENU_ITEM_03_HAS_SEPARATOR 0
.MENU_ITEM_03_MOUSE_RECT_XY 0 0
.MENU_ITEM_03_MOUSE_RECT_DXDY 0 0
.MENU_ITEM_03_HIDE_MENU_ITEM 0
.MENU_ITEM_04_NAME "Move to next waypoint"
.MENU_ITEM_04_TYPE COMMAND
.MENU_ITEM_04_HAS_SEPARATOR 0
.MENU_ITEM_04_MOUSE_RECT_XY 0 0
.MENU_ITEM_04_MOUSE_RECT_DXDY 0 0
.MENU_ITEM_04_HIDE_MENU_ITEM 0
.MENU_ITEM_05_NAME "no action"
.MENU_ITEM_05_TYPE COMMAND
.MENU_ITEM_05_HAS_SEPARATOR 0
.MENU_ITEM_05_MOUSE_RECT_XY 0 0
.MENU_ITEM_05_MOUSE_RECT_DXDY 0 0
.MENU_ITEM_05_HIDE_MENU_ITEM 0
.MENU_ITEM_06_NAME "Blue LED"
.MENU_ITEM_06_TYPE CHECKBOX
.MENU_ITEM_06_HAS_SEPARATOR 0
.MENU_ITEM_06_MOUSE_RECT_XY 6 40
.MENU_ITEM_06_MOUSE_RECT_DXDY 15 12
.MENU_ITEM_06_HIDE_MENU_ITEM 1
.MENU_ITEM_07_NAME "Green LED"
.MENU_ITEM_07_TYPE CHECKBOX
.MENU_ITEM_07_HAS_SEPARATOR 0
.MENU_ITEM_07_MOUSE_RECT_XY 6 52
.MENU_ITEM_07_MOUSE_RECT_DXDY 15 12
.MENU_ITEM_07_HIDE_MENU_ITEM 1
.MENU_ITEM_08_NAME "FlightMode 1"
.MENU_ITEM_08_TYPE CHECKBOX
.MENU_ITEM_08_HAS_SEPARATOR 0
.MENU_ITEM_08_MOUSE_RECT_XY 40 19
.MENU_ITEM_08_MOUSE_RECT_DXDY 38 38
.MENU_ITEM_08_HIDE_MENU_ITEM 1
.MENU_ITEM_09_NAME "FlightMode 2"
.MENU_ITEM_09_TYPE CHECKBOX
.MENU_ITEM_09_HAS_SEPARATOR 0
.MENU_ITEM_09_MOUSE_RECT_XY 78 19
.MENU_ITEM_09_MOUSE_RECT_DXDY 38 38
.MENU_ITEM_09_HIDE_MENU_ITEM 1
.MENU_ITEM_10_NAME "FlightMode 3"
.MENU_ITEM_10_TYPE CHECKBOX
.MENU_ITEM_10_HAS_SEPARATOR 0
.MENU_ITEM_10_MOUSE_RECT_XY 115 19
.MENU_ITEM_10_MOUSE_RECT_DXDY 38 38
.MENU_ITEM_10_HIDE_MENU_ITEM 1

98
ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/settings.cpp Normal file
View File

@ -0,0 +1,98 @@
/**
******************************************************************************
*
* @file settings.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
* @addtogroup 3rdParty Third-party integration
* @{
* @addtogroup AeroSimRC AeroSimRC proxy plugin
* @{
* @brief AeroSimRC simulator to HITL proxy plugin
*****************************************************************************/
/*
* 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 "settings.h"
Settings::Settings(QString settingsPath, QObject *parent) :
QObject(parent)
{
settings = new QSettings(settingsPath + "/cc_plugin.ini", QSettings::IniFormat);
// default settings
sendToHost = "127.0.0.1";
sendToPort = 40100;
listenOnHost = "127.0.0.1";
listenOnPort = 40200;
channels.reserve(60);
for (quint8 i = 0; i < 10; ++i)
inputMap << 255;
for (quint8 i = 0; i < 8; ++i)
outputMap << 255;
sendToRX = true;
takeFromTX = true;
videoModes << 1 << 50 << 50 << 800 << 600;
}
void Settings::read()
{
// network
listenOnHost = settings->value("listen_on_host", listenOnHost).toString();
listenOnPort = settings->value("listen_on_port", listenOnPort).toInt();
sendToHost = settings->value("send_to_host", sendToHost).toString();
sendToPort = settings->value("send_to_port", sendToPort).toInt();
QString allChannels = settings->value("all_channels").toString();
QString chan;
int i = 0;
foreach (chan, allChannels.split(" "))
channels.insert(chan, i++);
// inputs
QString num = "";
QString map = "";
for (quint8 i = 0; i < 10; ++i) {
num = QString::number(i+1);
map = settings->value("Input/cc_channel_" + num).toString();
inputMap[i] = channels.value(map, inputMap.at(i));
}
QString sendTo = settings->value("Input/send_to", "RX").toString();
sendToRX = (sendTo == "RX") ? true : false;
// outputs
for (quint8 i = 0; i < 8; ++i) {
num = QString::number(i+1);
map = settings->value("Output/sim_channel_" + num).toString();
outputMap[i] = channels.value(map, outputMap.at(i));
}
QString takeFrom = settings->value("Output/take_from", "TX").toString();
takeFromTX = (takeFrom == "TX") ? true : false;
// video
quint8 resolutionNum = settings->value("Video/number_of_resolutions", 0).toInt();
if (resolutionNum > 0) {
videoModes.clear();
videoModes << resolutionNum;
for (quint8 i = 0; i < resolutionNum; ++i) {
num = QString::number(i+1);
QString modes = settings->value("Video/resolution_" + num, "0, 0, 640, 480").toString();
QString mode;
foreach (mode, modes.split(" "))
videoModes << mode.toInt();
}
}
}

67
ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/settings.h Normal file
View File

@ -0,0 +1,67 @@
/**
******************************************************************************
*
* @file settings.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
* @addtogroup 3rdParty Third-party integration
* @{
* @addtogroup AeroSimRC AeroSimRC proxy plugin
* @{
* @brief AeroSimRC simulator to HITL proxy plugin
*****************************************************************************/
/*
* 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 SETTINGS_H
#define SETTINGS_H
#include <QObject>
#include <QSettings>
#include <QHash>
#include <QList>
#include <QStringList>
#include <QDebug>
class Settings : public QObject
{
public:
explicit Settings(QString settingsPath, QObject *parent = 0);
void read();
QString remoteHost() { return sendToHost; }
quint16 remotePort() { return sendToPort; }
QString localHost() { return listenOnHost; }
quint16 localPort() { return listenOnPort; }
QList<quint8> getInputMap() { return inputMap; }
QList<quint8> getOutputMap() { return outputMap; }
bool isToRX() { return sendToRX; }
bool isFromTX() { return takeFromTX; }
QList<quint16> getVideoModes() { return videoModes; }
private:
QHash<QString, quint8> channels;
QSettings *settings;
QString sendToHost;
quint16 sendToPort;
QString listenOnHost;
quint16 listenOnPort;
QList<quint8> inputMap;
QList<quint8> outputMap;
bool sendToRX;
bool takeFromTX;
QList<quint16> videoModes;
};
#endif // SETTINGS_H

228
ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udpconnect.cpp Normal file
View File

@ -0,0 +1,228 @@
/**
******************************************************************************
*
* @file udpconnect.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
* @addtogroup 3rdParty Third-party integration
* @{
* @addtogroup AeroSimRC AeroSimRC proxy plugin
* @{
* @brief AeroSimRC simulator to HITL proxy plugin
*****************************************************************************/
/*
* 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 "udpconnect.h"
#include "enums.h"
UdpSender::UdpSender(const QList<quint8> map,
bool isTX,
QObject *parent)
: QObject(parent)
{
qDebug() << this << "UdpSender::UdpSender thread:" << thread();
outSocket = NULL;
for (int i = 0; i < 8; ++i)
channels << 0.0;
channelsMap = map;
takeFromTX = isTX;
packetsSended = 0;
}
UdpSender::~UdpSender()
{
qDebug() << this << "UdpSender::~UdpSender";
if (outSocket)
delete outSocket;
}
// public
void UdpSender::init(const QString &remoteHost, quint16 remotePort)
{
qDebug() << this << "UdpSender::init";
outHost = remoteHost;
outPort = remotePort;
outSocket = new QUdpSocket();
}
void UdpSender::sendDatagram(const simToPlugin *stp)
{
QByteArray data;
data.resize(188);
QDataStream out(&data, QIODevice::WriteOnly);
out.setFloatingPointPrecision(QDataStream::SinglePrecision);
// magic header, "AERO"
out << quint32(0x4153494D);
// simulation step
out << stp->simTimeStep;
// home location
out << stp->initPosX << stp->initPosY << stp->initPosZ;
out << stp->wpHomeX << stp->wpHomeY << stp->wpHomeLat << stp->wpHomeLong;
// position
out << stp->posX << stp->posY << stp->posZ;
// velocity (world)
out << stp->velX << stp->velY << stp->velZ;
// angular velocity (model)
out << stp->angVelXm << stp->angVelYm << stp->angVelZm;
// acceleration (model)
out << stp->accelXm << stp->accelYm << stp->accelZm;
// coordinates
out << stp->latitude << stp->longitude;
// sonar
out << stp->AGL;
// attitude
out << stp->heading << stp->pitch << stp->roll;
// electric
out << stp->voltage << stp->current;
// matrix
out << stp->axisXx << stp->axisXy << stp->axisXz;
out << stp->axisYx << stp->axisYy << stp->axisYz;
out << stp->axisZx << stp->axisZy << stp->axisZz;
// channels
for (int i = 0; i < 8; ++i) {
quint8 mapTo = channelsMap.at(i);
if (mapTo == 255) // unused channel
out << 0.0;
else if (takeFromTX) // use values from simulators transmitter
out << stp->chSimTX[mapTo];
else // direct use values from ESC/motors/ailerons/etc
out << stp->chSimRX[mapTo];
}
// packet counter
out << packetsSended;
outSocket->writeDatagram(data, outHost, outPort);
++packetsSended;
}
//-----------------------------------------------------------------------------
UdpReceiver::UdpReceiver(const QList<quint8> map,
bool isRX,
QObject *parent)
: QThread(parent)
{
qDebug() << this << "UdpReceiver::UdpReceiver thread:" << thread();
stopped = false;
inSocket = NULL;
for (int i = 0; i < 10; ++i)
channels << -1.0;
channelsMap = map;
sendToRX = isRX;
armed = 0;
mode = 0;
packetsRecived = 1;
}
UdpReceiver::~UdpReceiver()
{
qDebug() << this << "UdpReceiver::~UdpReceiver";
if (inSocket)
delete inSocket;
}
// public
void UdpReceiver::init(const QString &localHost, quint16 localPort)
{
qDebug() << this << "UdpReceiver::init";
inSocket = new QUdpSocket();
qDebug() << this << "inSocket constructed" << inSocket->thread();
inSocket->bind(QHostAddress(localHost), localPort);
}
void UdpReceiver::run()
{
qDebug() << this << "UdpReceiver::run start";
while (!stopped)
onReadyRead();
qDebug() << this << "UdpReceiver::run ended";
}
void UdpReceiver::stop()
{
qDebug() << this << "UdpReceiver::stop";
stopped = true;
}
void UdpReceiver::setChannels(pluginToSim *pts)
{
QMutexLocker locker(&mutex);
for (int i = 0; i < 10; ++i) {
quint8 mapTo = channelsMap.at(i);
if (mapTo != 255) {
float channelValue = qBound(-1.0f, channels.at(i), 1.0f);
if (sendToRX) {
// direct connect to ESC/motors/ailerons/etc
pts->chNewRX[mapTo] = channelValue;
pts->chOverRX[mapTo] = true;
} else {
// replace simulators transmitter
pts->chNewTX[mapTo] = channelValue;
pts->chOverTX[mapTo] = true;
}
}
}
}
void UdpReceiver::getFlighStatus(quint8 &arm, quint8 &mod)
{
QMutexLocker locker(&mutex);
arm = armed;
mod = mode;
}
// private
void UdpReceiver::onReadyRead()
{
if (!inSocket->waitForReadyRead(8)) // 1/60fps ~= 16.7ms, 1/120fps ~= 8.3ms
return;
// TODO: add failsafe
// if a command not recieved then slowly return all channel to neutral
//
while (inSocket->hasPendingDatagrams()) {
QByteArray datagram;
datagram.resize(inSocket->pendingDatagramSize());
quint64 datagramSize;
datagramSize = inSocket->readDatagram(datagram.data(), datagram.size());
processDatagram(datagram);
}
}
void UdpReceiver::processDatagram(QByteArray &datagram)
{
QDataStream stream(datagram);
stream.setFloatingPointPrecision(QDataStream::SinglePrecision);
// check magic header
quint32 magic;
stream >> magic;
if (magic != 0x52434D44) // "RCMD"
return;
// read channels
for (int i = 0; i < 10; ++i)
stream >> channels[i];
// read flight mode
stream >> armed >> mode;
// read counter
stream >> packetsRecived;
}

90
ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udpconnect.h Normal file
View File

@ -0,0 +1,90 @@
/**
******************************************************************************
*
* @file udpconnect.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
* @addtogroup 3rdParty Third-party integration
* @{
* @addtogroup AeroSimRC AeroSimRC proxy plugin
* @{
* @brief AeroSimRC simulator to HITL proxy plugin
*****************************************************************************/
/*
* 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 UDPCONNECT_H
#define UDPCONNECT_H
#include <QObject>
#include <QUdpSocket>
#include <QList>
#include <QTime>
#include <QMutex>
#include <QMutexLocker>
#include "aerosimrcdatastruct.h"
class UdpSender : public QObject
{
// Q_OBJECT
public:
explicit UdpSender(const QList<quint8> map, bool isTX, QObject *parent = 0);
~UdpSender();
void init(const QString &remoteHost, quint16 remotePort);
void sendDatagram(const simToPlugin *stp);
quint32 pcks() { return packetsSended; }
private:
QUdpSocket *outSocket;
QHostAddress outHost;
quint16 outPort;
QList<float> channels;
QList<quint8> channelsMap;
bool takeFromTX;
quint32 packetsSended;
};
class UdpReceiver : public QThread
{
// Q_OBJECT
public:
explicit UdpReceiver(const QList<quint8> map, bool isRX, QObject *parent = 0);
~UdpReceiver();
void init(const QString &localHost, quint16 localPort);
void run();
void stop();
// function getChannels for other threads
void setChannels(pluginToSim *pts);
void getFlighStatus(quint8 &arm, quint8 &mod);
quint8 getArmed() { return armed; }
quint8 getMode() { return mode; }
quint32 pcks() { return packetsRecived; }
private:
volatile bool stopped;
QMutex mutex;
QUdpSocket *inSocket;
QList<float> channels;
QList<quint8> channelsMap;
bool sendToRX;
quint8 armed;
quint8 mode;
quint32 packetsRecived;
void onReadyRead();
void processDatagram(QByteArray &datagram);
};
#endif // UDPCONNECT_H

17
ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udptest.pro Normal file
View File

@ -0,0 +1,17 @@
include(../../../../../openpilotgcs.pri)
QT += core gui network
TEMPLATE = app
TARGET = udp_test
DESTDIR = $$GCS_APP_PATH
HEADERS += \
udptestwidget.h
SOURCES += \
udptestmain.cpp \
udptestwidget.cpp
FORMS += \
udptestwidget.ui

38
ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udptestmain.cpp Normal file
View File

@ -0,0 +1,38 @@
/**
******************************************************************************
*
* @file udptestmain.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
* @addtogroup 3rdParty Third-party integration
* @{
* @addtogroup AeroSimRC AeroSimRC proxy plugin
* @{
* @brief AeroSimRC simulator to HITL proxy plugin test utility
*****************************************************************************/
/*
* 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 <QApplication>
#include "udptestwidget.h"
int main(int argc, char *argv[])
{
QApplication a(argc, argv);
Widget w;
w.show();
return a.exec();
}

537
ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udptestwidget.cpp Normal file
View File

@ -0,0 +1,537 @@
/**
******************************************************************************
*
* @file udptestwidget.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
* @addtogroup 3rdParty Third-party integration
* @{
* @addtogroup AeroSimRC AeroSimRC proxy plugin
* @{
* @brief AeroSimRC simulator to HITL proxy plugin test utility
*****************************************************************************/
/*
* 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 "udptestwidget.h"
#include "ui_udptestwidget.h"
Widget::Widget(QWidget *parent) :
QWidget(parent),
ui(new Ui::Widget)
{
ui->setupUi(this);
inSocket = NULL;
outSocket = NULL;
screenTimeout.start();
packetCounter = 0;
autoSendTimer = new QTimer(this);
connect(autoSendTimer, SIGNAL(timeout()), this, SLOT(sendDatagram()), Qt::DirectConnection);
}
Widget::~Widget()
{
if(outSocket) {
delete outSocket;
}
if(inSocket) {
delete inSocket;
}
delete ui;
}
// private slots //////////////////////////////////////////////////////////////
void Widget::on_btReciveStart_clicked()
{
on_btReciveStop_clicked();
inSocket = new QUdpSocket();
QString host = ui->localHost->text();
quint16 port = ui->localPort->text().toInt();
if (inSocket->bind(QHostAddress(host), port)) {
connect(inSocket, SIGNAL(readyRead()),
this, SLOT(readDatagram()), Qt::DirectConnection);
ui->listWidget->addItem("bind ok");
ui->btReciveStop->setEnabled(1);
ui->localHost->setDisabled(1);
ui->localPort->setDisabled(1);
ui->btReciveStart->setDisabled(1);
} else {
ui->listWidget->addItem("bind error: " + inSocket->errorString());
}
}
void Widget::on_btReciveStop_clicked()
{
if(inSocket) {
delete inSocket;
inSocket = NULL;
ui->listWidget->addItem("unbind ok");
} else {
ui->listWidget->addItem("socket not found");
}
ui->btReciveStart->setEnabled(1);
ui->localHost->setEnabled(1);
ui->localPort->setEnabled(1);
ui->btReciveStop->setDisabled(1);
}
void Widget::on_btTransmitStart_clicked()
{
on_btTransmitStop_clicked();
outSocket = new QUdpSocket();
outHost = ui->simHost->text();
outPort = ui->simPort->text().toInt();
ui->listWidget->addItem("transmit started");
ui->btTransmitStop->setEnabled(1);
ui->simHost->setDisabled(1);
ui->simPort->setDisabled(1);
ui->btTransmitStart->setDisabled(1);
}
void Widget::on_btTransmitStop_clicked()
{
if(outSocket) {
delete outSocket;
outSocket = NULL;
ui->listWidget->addItem("transmit stopped");
} else {
ui->listWidget->addItem("transmit socket not found");
}
ui->btTransmitStart->setEnabled(1);
ui->simHost->setEnabled(1);
ui->simPort->setEnabled(1);
ui->btTransmitStop->setDisabled(1);
}
void Widget::readDatagram()
{
while (inSocket->hasPendingDatagrams()) {
QByteArray datagram;
datagram.resize(inSocket->pendingDatagramSize());
QHostAddress sender;
quint16 senderPort;
quint64 datagramSize = inSocket->readDatagram(datagram.data(), datagram.size(),
&sender, &senderPort);
Q_UNUSED(datagramSize);
processDatagram(datagram);
if (ui->autoAnswer->isChecked())
sendDatagram();
}
}
// private ////////////////////////////////////////////////////////////////////
void Widget::processDatagram(const QByteArray &data)
{
QByteArray buf = data;
QDataStream stream(&buf, QIODevice::ReadOnly);
stream.setFloatingPointPrecision(QDataStream::SinglePrecision);
// check magic header
quint32 magic;
stream >> magic;
if (magic == 0x4153494D) { // "AERO"
float timeStep,
homeX, homeY, homeZ,
WpHX, WpHY, WpLat, WpLon,
posX, posY, posZ,
velX, velY, velZ,
angX, angY, angZ,
accX, accY, accZ,
lat, lon, alt,
head, pitch, roll,
volt, curr,
rx, ry, rz, fx, fy, fz, ux, uy, uz,
chAil, chEle, chThr, chRud, chPlg1, chPlg2, chFpv1, chFpv2;
stream >> timeStep;
stream >> homeX >> homeY >> homeZ;
stream >> WpHX >> WpHY >> WpLat >> WpLon;
stream >> posX >> posY >> posZ;
stream >> velX >> velY >> velZ;
stream >> angX >> angY >> angZ;
stream >> accX >> accY >> accZ;
stream >> lat >> lon >> alt;
stream >> head >> pitch >> roll;
stream >> volt >> curr;
stream >> rx >> ry >> rz >> fx >> fy >> fz >> ux >> uy >> uz;
stream >> chAil >> chEle >> chThr >> chRud >> chPlg1 >> chPlg2 >> chFpv1 >> chFpv2;
stream >> packetCounter;
if(ui->tabWidget->currentIndex() != 0)
return;
if (screenTimeout.elapsed() < 200)
return;
ui->listWidget->clear();
/*
ui->listWidget->addItem("time step (s)");
ui->listWidget->addItem(QString("%1")
.arg(timeStep);
ui->listWidget->addItem("home location (m)");
ui->listWidget->addItem(QString("%1, %2, %3")
.arg(homeX, 7, 'f', 4)
.arg(homeY, 7, 'f', 4)
.arg(homeZ, 7, 'f', 4));
ui->listWidget->addItem("home waypoint");
ui->listWidget->addItem(QString("%1, %2, %3, %4")
.arg(WpHX, 7, 'f', 4)
.arg(WpHY, 7, 'f', 4)
.arg(WpLat, 7, 'f', 4)
.arg(WpLon, 7, 'f', 4));
ui->listWidget->addItem("model position (m)");
ui->listWidget->addItem(QString("%1, %2, %3")
.arg(posX, 7, 'f', 4)
.arg(posY, 7, 'f', 4)
.arg(posZ, 7, 'f', 4));
ui->listWidget->addItem("model velocity (m/s)");
ui->listWidget->addItem(QString("%1, %2, %3")
.arg(velX, 7, 'f', 4)
.arg(velY, 7, 'f', 4)
.arg(velZ, 7, 'f', 4));
ui->listWidget->addItem("model angular velocity (rad/s)");
ui->listWidget->addItem(QString("%1, %2, %3")
.arg(angX, 7, 'f', 4)
.arg(angY, 7, 'f', 4)
.arg(angZ, 7, 'f', 4));
ui->listWidget->addItem("model acceleration (m/s/s)");
ui->listWidget->addItem(QString("%1, %2, %3")
.arg(accX, 7, 'f', 4)
.arg(accY, 7, 'f', 4)
.arg(accZ, 7, 'f', 4));
ui->listWidget->addItem("model coordinates (deg, deg, m)");
ui->listWidget->addItem(QString("%1, %2, %3")
.arg(lat, 7, 'f', 4)
.arg(lon, 7, 'f', 4)
.arg(alt, 7, 'f', 4));
ui->listWidget->addItem("model electrics");
ui->listWidget->addItem(QString("%1V, %2A")
.arg(volt, 7, 'f', 4)
.arg(curr, 7, 'f', 4));
ui->listWidget->addItem("channels");
ui->listWidget->addItem(QString("%1 %2 %3 %4 %5 %6 %7 %8")
.arg(chAil, 6, 'f', 3)
.arg(chEle, 6, 'f', 3)
.arg(chThr, 6, 'f', 3)
.arg(chRud, 6, 'f', 3)
.arg(chPlg1, 6, 'f', 3)
.arg(chPlg2, 6, 'f', 3)
.arg(chFpv1, 6, 'f', 3)
.arg(chFpv2, 6, 'f', 3));
ui->listWidget->addItem("datagram size (bytes), packet counter");
ui->listWidget->addItem(QString("%1 %2")
.arg(data.size())
.arg(packetCounter));
*/
// matrix calculation start
// model matrix
QMatrix4x4 m = QMatrix4x4( fy, fx, -fz, 0.0,
ry, rx, -rz, 0.0,
-uy, -ux, uz, 0.0,
0.0, 0.0, 0.0, 1.0);
m.optimize();
// world matrix
QMatrix4x4 w = m.inverted();
// model quat
QQuaternion q;
asMatrix2Quat(m, q);
// model roll, pitch, yaw
QVector3D rpy;
asMatrix2RPY(m, rpy);
// sonar
float sAlt = 5.0;
if ((alt < (sAlt * 2.0)) && (roll < 0.35) && (pitch < 0.35)) {
float x = alt * qTan(roll);
float y = alt * qTan(pitch);
float h = QVector3D(x, y, alt).length();
sAlt = qMin(h, sAlt);
}
ui->listWidget->addItem("sonar altitude");
ui->listWidget->addItem(QString("%1")
.arg(sAlt, 8, 'f', 5));
ui->listWidget->addItem("vectors");
ui->listWidget->addItem(QString(" X Y Z"));
ui->listWidget->addItem(QString("R: %1 %2 %3\nF: %4 %5 %6\nU: %7 %8 %9")
.arg(rx, 8, 'f', 5).arg(ry, 8, 'f', 5).arg(rz, 8, 'f', 5)
.arg(fx, 8, 'f', 5).arg(fy, 8, 'f', 5).arg(fz, 8, 'f', 5)
.arg(ux, 8, 'f', 5).arg(uy, 8, 'f', 5).arg(uz, 8, 'f', 5));
ui->listWidget->addItem("CC model matrix");
ui->listWidget->addItem(QString(" %1 %2 %3\n %4 %5 %6\n %7 %8 %9")
.arg(m(0, 0), 8, 'f', 5).arg(m(0, 1), 8, 'f', 5).arg(m(0, 2), 8, 'f', 5)
.arg(m(1, 0), 8, 'f', 5).arg(m(1, 1), 8, 'f', 5).arg(m(1, 2), 8, 'f', 5)
.arg(m(2, 0), 8, 'f', 5).arg(m(2, 1), 8, 'f', 5).arg(m(2, 2), 8, 'f', 5));
ui->listWidget->addItem("CC world matrix");
ui->listWidget->addItem(QString(" %1 %2 %3\n %4 %5 %6\n %7 %8 %9")
.arg(w(0, 0), 8, 'f', 5).arg(w(0, 1), 8, 'f', 5).arg(w(0, 2), 8, 'f', 5)
.arg(w(1, 0), 8, 'f', 5).arg(w(1, 1), 8, 'f', 5).arg(w(1, 2), 8, 'f', 5)
.arg(w(2, 0), 8, 'f', 5).arg(w(2, 1), 8, 'f', 5).arg(w(2, 2), 8, 'f', 5));
ui->listWidget->addItem("CC quaternion");
ui->listWidget->addItem(QString("%1, %2, %3, %4")
.arg(q.x(), 7, 'f', 4)
.arg(q.y(), 7, 'f', 4)
.arg(q.z(), 7, 'f', 4)
.arg(q.scalar(), 7, 'f', 4));
ui->listWidget->addItem("model attitude (deg)");
ui->listWidget->addItem(QString("%1, %2, %3")
.arg(roll*RAD2DEG, 7, 'f', 4)
.arg(pitch*RAD2DEG, 7, 'f', 4)
.arg(head*RAD2DEG, 7, 'f', 4));
ui->listWidget->addItem("CC attitude calculated (deg)");
ui->listWidget->addItem(QString("%1, %2, %3")
.arg(rpy.x(), 7, 'f', 4)
.arg(rpy.y(), 7, 'f', 4)
.arg(rpy.z(), 7, 'f', 4));
screenTimeout.restart();
} else if (magic == 0x52434D44) { // "RCMD"
qreal ch1, ch2, ch3, ch4, ch5, ch6, ch7, ch8, ch9, ch10;
stream >> ch1 >> ch2 >> ch3 >> ch4 >> ch5 >> ch6 >> ch7 >> ch8 >> ch9 >> ch10;
quint8 armed, mode;
stream >> armed >> mode;
if(ui->tabWidget->currentIndex() == 0) {
if (screenTimeout.elapsed() < 200)
return;
ui->listWidget->clear();
ui->listWidget->addItem("channels");
ui->listWidget->addItem("CH1: " + QString::number(ch1));
ui->listWidget->addItem("CH2: " + QString::number(ch2));
ui->listWidget->addItem("CH3: " + QString::number(ch3));
ui->listWidget->addItem("CH4: " + QString::number(ch4));
ui->listWidget->addItem("CH5: " + QString::number(ch5));
ui->listWidget->addItem("CH6: " + QString::number(ch6));
ui->listWidget->addItem("CH7: " + QString::number(ch7));
ui->listWidget->addItem("CH8: " + QString::number(ch8));
ui->listWidget->addItem("CH9: " + QString::number(ch9));
ui->listWidget->addItem("CH10:" + QString::number(ch10));
ui->listWidget->addItem("armed:" + QString::number(armed));
ui->listWidget->addItem("fmode:" + QString::number(mode));
}
screenTimeout.restart();
} else {
qDebug() << "unknown magic:" << magic;
}
}
void Widget::sendDatagram()
{
if(!outSocket)
return;
float ch[10]; // = {0,0,0,0,0,0,0,0,0,0};
quint8 armed;
quint8 fmode;
const float coeff = 1.0 / 512.0;
ch[0] = ui->ch1->value() * coeff;
ch[1] = ui->ch2->value() * coeff;
ch[2] = ui->ch3->value() * coeff;
ch[3] = ui->ch4->value() * coeff;
ch[4] = ui->ch5->value() * coeff;
ch[5] = ui->ch6->value() * coeff;
ch[6] = ui->ch7->value() * coeff;
ch[7] = ui->ch8->value() * coeff;
ch[8] = ui->ch9->value() * coeff;
ch[9] = ui->ch10->value() * coeff;
armed = (ui->disarmed->isChecked()) ? 0 : (ui->arming->isChecked()) ? 1 : 2;
fmode = ui->flightMode->value();
QByteArray data;
// 50 - current size of values, 4(quint32) + 10*4(float) + 2*1(quint8) + 4(quint32)
data.resize(50);
QDataStream stream(&data, QIODevice::WriteOnly);
stream.setFloatingPointPrecision(QDataStream::SinglePrecision);
// magic header, "RCMD"
stream << quint32(0x52434D44);
// send channels
for (int i = 0; i < 10; ++i) {
stream << ch[i];
}
// send armed and mode
stream << armed << fmode;
// send readed counter
stream << packetCounter;
if (outSocket->writeDatagram(data, outHost, outPort) == -1) {
qDebug() << "write failed: outHost" << outHost << " "
<< "outPort " << outPort << " "
<< outSocket->errorString();
}
}
void Widget::on_autoSend_clicked()
{
autoSendTimer->start(100);
qDebug() << "timer start";
}
void Widget::on_autoAnswer_clicked()
{
autoSendTimer->stop();
qDebug() << "timer stop";
}
// transfomations
void Widget::asMatrix2Quat(const QMatrix4x4 &m, QQuaternion &q)
{
qreal w, x, y, z;
// w always >= 0
w = qSqrt(qMax(0.0, 1.0 + m(0, 0) + m(1, 1) + m(2, 2))) / 2.0;
x = qSqrt(qMax(0.0, 1.0 + m(0, 0) - m(1, 1) - m(2, 2))) / 2.0;
y = qSqrt(qMax(0.0, 1.0 - m(0, 0) + m(1, 1) - m(2, 2))) / 2.0;
z = qSqrt(qMax(0.0, 1.0 - m(0, 0) - m(1, 1) + m(2, 2))) / 2.0;
x = copysign(x, (m(1, 2) - m(2, 1)));
y = copysign(y, (m(2, 0) - m(0, 2)));
z = copysign(z, (m(0, 1) - m(1, 0)));
q.setScalar(w);
q.setX(x);
q.setY(y);
q.setZ(z);
}
void Widget::asQuat2RPY(const QQuaternion &q, QVector3D &rpy)
{
qreal roll;
qreal pitch;
qreal yaw;
const qreal d2 = 2.0;
const qreal qss = q.scalar() * q.scalar();
const qreal qxx = q.x() * q.x();
const qreal qyy = q.y() * q.y();
const qreal qzz = q.z() * q.z();
qreal test = -d2 * (q.x() * q.z() - q.scalar() * q.y());
if (qFabs(test) > 0.998) {
// ~86.3°, gimbal lock
qreal R10 = d2 * (q.x() * q.y() - q.scalar() * q.z());
qreal R11 = qss - qxx + qyy - qzz;
roll = 0.0;
pitch = copysign(M_PI_2, test);
yaw = qAtan2(-R10, R11);
} else {
qreal R12 = d2 * (q.y() * q.z() + q.scalar() * q.x());
qreal R22 = qss - qxx - qyy + qzz;
qreal R01 = d2 * (q.x() * q.y() + q.scalar() * q.z());
qreal R00 = qss + qxx - qyy - qzz;
roll = qAtan2(R12, R22);
pitch = qAsin(test);
yaw = qAtan2(R01, R00);
}
rpy.setX(RAD2DEG * roll);
rpy.setY(RAD2DEG * pitch);
rpy.setZ(RAD2DEG * yaw);
}
void Widget::asMatrix2RPY(const QMatrix4x4 &m, QVector3D &rpy)
{
qreal roll;
qreal pitch;
qreal yaw;
if (qFabs(m(0, 2)) > 0.998) {
// ~86.3°, gimbal lock
roll = 0.0;
pitch = copysign(M_PI_2, -m(0, 2));
yaw = qAtan2(-m(1, 0), m(1, 1));
} else {
roll = qAtan2(m(1, 2), m(2, 2));
pitch = qAsin(-m(0, 2));
yaw = qAtan2(m(0, 1), m(0, 0));
}
rpy.setX(roll * RAD2DEG);
rpy.setY(pitch * RAD2DEG);
rpy.setZ(yaw * RAD2DEG);
}
/* // not used
void Widget::ccRPY2Quat(const QVector3D &rpy, QQuaternion &q)
{
float phi, theta, psi;
float cphi, sphi, ctheta, stheta, cpsi, spsi;
phi = rpy.x() / 2;
theta = rpy.y() / 2;
psi = rpy.z() / 2;
cphi = cosf(phi);
sphi = sinf(phi);
ctheta = cosf(theta);
stheta = sinf(theta);
cpsi = cosf(psi);
spsi = sinf(psi);
q.setScalar(cphi * ctheta * cpsi + sphi * stheta * spsi);
q.setX(sphi * ctheta * cpsi - cphi * stheta * spsi);
q.setY(cphi * stheta * cpsi + sphi * ctheta * spsi);
q.setZ(cphi * ctheta * spsi - sphi * stheta * cpsi);
if (q.scalar() < 0) { // q0 always positive for uniqueness
q.setScalar(-q.scalar());
q.setX(-q.x());
q.setY(-q.y());
q.setZ(-q.z());
}
}
void Widget::ccQuat2Matrix(const QQuaternion &q, QMatrix4x4 &m)
{
float q0s = q.scalar() * q.scalar();
float q1s = q.x() * q.x();
float q2s = q.y() * q.y();
float q3s = q.z() * q.z();
float m00 = q0s + q1s - q2s - q3s;
float m01 = 2 * (q.x() * q.y() + q.scalar() * q.z());
float m02 = 2 * (q.x() * q.z() - q.scalar() * q.y());
float m10 = 2 * (q.x() * q.y() - q.scalar() * q.z());
float m11 = q0s - q1s + q2s - q3s;
float m12 = 2 * (q.y() * q.z() + q.scalar() * q.x());
float m20 = 2 * (q.x() * q.z() + q.scalar() * q.y());
float m21 = 2 * (q.y() * q.z() - q.scalar() * q.x());
float m22 = q0s - q1s - q2s + q3s;
m = QMatrix4x4(m00, m01, m02, 0,
m10, m11, m12, 0,
m20, m21, m22, 0,
0, 0, 0, 1);
}
*/

94
ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udptestwidget.h Normal file
View File

@ -0,0 +1,94 @@
/**
******************************************************************************
*
* @file udptestwidget.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
* @addtogroup 3rdParty Third-party integration
* @{
* @addtogroup AeroSimRC AeroSimRC proxy plugin
* @{
* @brief AeroSimRC simulator to HITL proxy plugin test utility
*****************************************************************************/
/*
* 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 UDPTESTWIDGET_H
#define UDPTESTWIDGET_H
#include <QWidget>
#include <QUdpSocket>
#include <QTime>
#if defined(Q_CC_MSVC)
#define _USE_MATH_DEFINES
#endif
#include <qmath.h>
#include <QVector3D>
#include <QMatrix4x4>
#include <QDebug>
#include <QTimer>
namespace Ui {
class Widget;
}
const float RAD2DEG = (float)(180.0/M_PI);
const float DEG2RAD = (float)(M_PI/180.0);
class Widget : public QWidget
{
Q_OBJECT
public:
explicit Widget(QWidget *parent = 0);
~Widget();
private slots:
void on_btReciveStart_clicked();
void on_btReciveStop_clicked();
void on_btTransmitStart_clicked();
void on_btTransmitStop_clicked();
void readDatagram();
void sendDatagram();
void on_autoSend_clicked();
void on_autoAnswer_clicked();
private:
Ui::Widget *ui;
QTime screenTimeout;
QUdpSocket *inSocket;
QUdpSocket *outSocket;
QHostAddress outHost;
quint16 outPort;
quint32 packetCounter;
void processDatagram(const QByteArray &data);
QTimer *autoSendTimer;
void asMatrix2Quat(const QMatrix4x4 &m, QQuaternion &q);
void asMatrix2RPY(const QMatrix4x4 &m, QVector3D &rpy);
void asQuat2RPY(const QQuaternion &q, QVector3D &rpy);
/* // not used
* void ccRPY2Quat(const QVector3D &rpy, QQuaternion &q);
* void ccQuat2Matrix(const QQuaternion &q, QMatrix4x4 &m);
*/
};
#endif // UDPTESTWIDGET_H

940
ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udptestwidget.ui Normal file
View File

@ -0,0 +1,940 @@
<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
<class>Widget</class>
<widget class="QWidget" name="Widget">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>440</width>
<height>525</height>
</rect>
</property>
<property name="windowTitle">
<string notr="true">udp_test</string>
</property>
<property name="windowFilePath">
<string notr="true"/>
</property>
<layout class="QVBoxLayout" name="verticalLayout">
<item>
<layout class="QGridLayout" name="gridLayout">
<item row="0" column="4">
<widget class="QPushButton" name="btReciveStart">
<property name="minimumSize">
<size>
<width>60</width>
<height>0</height>
</size>
</property>
<property name="text">
<string notr="true">Connect</string>
</property>
</widget>
</item>
<item row="0" column="3">
<widget class="QLineEdit" name="localPort">
<property name="enabled">
<bool>true</bool>
</property>
<property name="sizePolicy">
<sizepolicy hsizetype="Minimum" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>50</width>
<height>0</height>
</size>
</property>
<property name="text">
<string notr="true">40100</string>
</property>
<property name="maxLength">
<number>5</number>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLineEdit" name="localHost">
<property name="enabled">
<bool>true</bool>
</property>
<property name="sizePolicy">
<sizepolicy hsizetype="Minimum" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="text">
<string notr="true">127.0.0.1</string>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="lbLocalHost">
<property name="text">
<string notr="true">sim host</string>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QLabel" name="lbSimHost">
<property name="text">
<string notr="true">local host:</string>
</property>
</widget>
</item>
<item row="0" column="2">
<widget class="QLabel" name="lbSimPort">
<property name="text">
<string notr="true">local port:</string>
</property>
</widget>
</item>
<item row="0" column="5">
<widget class="QPushButton" name="btReciveStop">
<property name="enabled">
<bool>false</bool>
</property>
<property name="minimumSize">
<size>
<width>60</width>
<height>0</height>
</size>
</property>
<property name="text">
<string notr="true">Disconnect</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLineEdit" name="simHost">
<property name="enabled">
<bool>true</bool>
</property>
<property name="sizePolicy">
<sizepolicy hsizetype="Minimum" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="text">
<string notr="true">127.0.0.1</string>
</property>
</widget>
</item>
<item row="1" column="2">
<widget class="QLabel" name="lbLocalPort">
<property name="text">
<string notr="true">sim port</string>
</property>
</widget>
</item>
<item row="1" column="3">
<widget class="QLineEdit" name="simPort">
<property name="sizePolicy">
<sizepolicy hsizetype="Minimum" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>50</width>
<height>0</height>
</size>
</property>
<property name="text">
<string notr="true">40200</string>
</property>
<property name="maxLength">
<number>5</number>
</property>
</widget>
</item>
<item row="1" column="4">
<widget class="QPushButton" name="btTransmitStart">
<property name="minimumSize">
<size>
<width>60</width>
<height>0</height>
</size>
</property>
<property name="text">
<string notr="true">Transmit</string>
</property>
</widget>
</item>
<item row="1" column="5">
<widget class="QPushButton" name="btTransmitStop">
<property name="enabled">
<bool>false</bool>
</property>
<property name="minimumSize">
<size>
<width>60</width>
<height>0</height>
</size>
</property>
<property name="text">
<string notr="true">Stop</string>
</property>
</widget>
</item>
</layout>
</item>
<item>
<widget class="QTabWidget" name="tabWidget">
<property name="currentIndex">
<number>0</number>
</property>
<widget class="QWidget" name="tab_1">
<attribute name="title">
<string notr="true">raw data</string>
</attribute>
<layout class="QVBoxLayout" name="verticalLayout_2">
<item>
<widget class="QListWidget" name="listWidget">
<property name="font">
<font>
<family>Terminal</family>
<pointsize>10</pointsize>
</font>
</property>
</widget>
</item>
</layout>
</widget>
<widget class="QWidget" name="tab_2">
<attribute name="title">
<string notr="true">channels</string>
</attribute>
<layout class="QVBoxLayout" name="verticalLayout_3">
<property name="spacing">
<number>0</number>
</property>
<property name="bottomMargin">
<number>9</number>
</property>
<item>
<widget class="QGroupBox" name="groupBox">
<property name="title">
<string>send data</string>
</property>
<layout class="QHBoxLayout" name="horizontalLayout_2">
<property name="spacing">
<number>6</number>
</property>
<property name="leftMargin">
<number>9</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>3</number>
</property>
<item>
<widget class="QRadioButton" name="autoAnswer">
<property name="enabled">
<bool>true</bool>
</property>
<property name="text">
<string notr="true">answer on recieve</string>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
<item>
<widget class="QRadioButton" name="autoSend">
<property name="enabled">
<bool>true</bool>
</property>
<property name="text">
<string notr="true">auto send</string>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
</layout>
</widget>
</item>
<item>
<widget class="QGroupBox" name="groupBox_2">
<property name="title">
<string>Flight mode</string>
</property>
<layout class="QHBoxLayout" name="horizontalLayout">
<property name="spacing">
<number>6</number>
</property>
<property name="leftMargin">
<number>9</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>3</number>
</property>
<item>
<widget class="QSlider" name="flightMode">
<property name="maximum">
<number>6</number>
</property>
<property name="pageStep">
<number>1</number>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="tickPosition">
<enum>QSlider::TicksAbove</enum>
</property>
<property name="tickInterval">
<number>1</number>
</property>
</widget>
</item>
</layout>
</widget>
</item>
<item>
<widget class="QGroupBox" name="groupBox_3">
<property name="title">
<string>Armed state</string>
</property>
<layout class="QHBoxLayout" name="horizontalLayout_3">
<property name="spacing">
<number>6</number>
</property>
<property name="leftMargin">
<number>9</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>3</number>
</property>
<item>
<widget class="QRadioButton" name="disarmed">
<property name="text">
<string>Disarmed</string>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
<item>
<widget class="QRadioButton" name="arming">
<property name="text">
<string>Arming</string>
</property>
</widget>
</item>
<item>
<widget class="QRadioButton" name="armed">
<property name="text">
<string>Armed</string>
</property>
</widget>
</item>
</layout>
</widget>
</item>
<item>
<widget class="QGroupBox" name="groupBox_4">
<property name="title">
<string>Channels</string>
</property>
<layout class="QGridLayout" name="gridLayout_3">
<property name="leftMargin">
<number>9</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>9</number>
</property>
<property name="bottomMargin">
<number>3</number>
</property>
<property name="spacing">
<number>3</number>
</property>
<item row="0" column="0">
<widget class="QLabel" name="lbCH1n">
<property name="text">
<string notr="true">CH1</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QSlider" name="ch1">
<property name="minimum">
<number>-511</number>
</property>
<property name="maximum">
<number>512</number>
</property>
<property name="singleStep">
<number>16</number>
</property>
<property name="pageStep">
<number>32</number>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="tickPosition">
<enum>QSlider::TicksAbove</enum>
</property>
<property name="tickInterval">
<number>128</number>
</property>
</widget>
</item>
<item row="0" column="2">
<widget class="QLabel" name="lbCH1">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>30</width>
<height>0</height>
</size>
</property>
<property name="text">
<string notr="true">0</string>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="lbCH2n">
<property name="text">
<string notr="true">CH2</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QSlider" name="ch2">
<property name="minimum">
<number>-511</number>
</property>
<property name="maximum">
<number>512</number>
</property>
<property name="singleStep">
<number>16</number>
</property>
<property name="pageStep">
<number>32</number>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="tickPosition">
<enum>QSlider::TicksAbove</enum>
</property>
<property name="tickInterval">
<number>128</number>
</property>
</widget>
</item>
<item row="1" column="2">
<widget class="QLabel" name="lbCH2">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>30</width>
<height>0</height>
</size>
</property>
<property name="text">
<string notr="true">0</string>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QLabel" name="lbCH3n">
<property name="text">
<string notr="true">CH3</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QSlider" name="ch3">
<property name="minimum">
<number>-511</number>
</property>
<property name="maximum">
<number>512</number>
</property>
<property name="singleStep">
<number>16</number>
</property>
<property name="pageStep">
<number>32</number>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="tickPosition">
<enum>QSlider::TicksAbove</enum>
</property>
<property name="tickInterval">
<number>128</number>
</property>
</widget>
</item>
<item row="2" column="2">
<widget class="QLabel" name="lbCH3">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>30</width>
<height>0</height>
</size>
</property>
<property name="text">
<string notr="true">0</string>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QLabel" name="lbCH4n">
<property name="text">
<string notr="true">CH4</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QSlider" name="ch4">
<property name="minimum">
<number>-511</number>
</property>
<property name="maximum">
<number>512</number>
</property>
<property name="singleStep">
<number>16</number>
</property>
<property name="pageStep">
<number>32</number>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="tickPosition">
<enum>QSlider::TicksAbove</enum>
</property>
<property name="tickInterval">
<number>128</number>
</property>
</widget>
</item>
<item row="3" column="2">
<widget class="QLabel" name="lbCH4">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>30</width>
<height>0</height>
</size>
</property>
<property name="text">
<string notr="true">0</string>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QLabel" name="lbCH5n">
<property name="text">
<string notr="true">CH5</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QSlider" name="ch5">
<property name="minimum">
<number>-511</number>
</property>
<property name="maximum">
<number>512</number>
</property>
<property name="singleStep">
<number>16</number>
</property>
<property name="pageStep">
<number>32</number>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="tickPosition">
<enum>QSlider::TicksAbove</enum>
</property>
<property name="tickInterval">
<number>128</number>
</property>
</widget>
</item>
<item row="4" column="2">
<widget class="QLabel" name="lbCH5">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>30</width>
<height>0</height>
</size>
</property>
<property name="text">
<string notr="true">0</string>
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QLabel" name="lbCH6n">
<property name="text">
<string notr="true">CH6</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="5" column="2">
<widget class="QLabel" name="lbCH6">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>30</width>
<height>0</height>
</size>
</property>
<property name="text">
<string notr="true">0</string>
</property>
</widget>
</item>
<item row="6" column="0">
<widget class="QLabel" name="lbCH7n">
<property name="text">
<string notr="true">Ch7</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="6" column="2">
<widget class="QLabel" name="lbCH7">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>30</width>
<height>0</height>
</size>
</property>
<property name="text">
<string notr="true">0</string>
</property>
</widget>
</item>
<item row="7" column="0">
<widget class="QLabel" name="lbCH8n">
<property name="text">
<string notr="true">Ch8</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="7" column="2">
<widget class="QLabel" name="lbCH8">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>30</width>
<height>0</height>
</size>
</property>
<property name="text">
<string notr="true">0</string>
</property>
</widget>
</item>
<item row="8" column="0">
<widget class="QLabel" name="lbCH9n">
<property name="text">
<string notr="true">Ch9</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="8" column="1">
<widget class="QSlider" name="ch9">
<property name="minimum">
<number>-511</number>
</property>
<property name="maximum">
<number>512</number>
</property>
<property name="singleStep">
<number>16</number>
</property>
<property name="pageStep">
<number>32</number>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="tickPosition">
<enum>QSlider::TicksAbove</enum>
</property>
<property name="tickInterval">
<number>128</number>
</property>
</widget>
</item>
<item row="8" column="2">
<widget class="QLabel" name="lbCH9">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>30</width>
<height>0</height>
</size>
</property>
<property name="text">
<string notr="true">0</string>
</property>
</widget>
</item>
<item row="9" column="0">
<widget class="QLabel" name="lbCH10n">
<property name="text">
<string notr="true">Ch10</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
<item row="9" column="1">
<widget class="QSlider" name="ch10">
<property name="minimum">
<number>-511</number>
</property>
<property name="maximum">
<number>512</number>
</property>
<property name="singleStep">
<number>16</number>
</property>
<property name="pageStep">
<number>32</number>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="tickPosition">
<enum>QSlider::TicksAbove</enum>
</property>
<property name="tickInterval">
<number>128</number>
</property>
</widget>
</item>
<item row="9" column="2">
<widget class="QLabel" name="lbCH10">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Preferred">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>30</width>
<height>0</height>
</size>
</property>
<property name="text">
<string notr="true">0</string>
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QSlider" name="ch6">
<property name="minimum">
<number>-511</number>
</property>
<property name="maximum">
<number>512</number>
</property>
<property name="singleStep">
<number>16</number>
</property>
<property name="pageStep">
<number>32</number>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="tickPosition">
<enum>QSlider::TicksAbove</enum>
</property>
<property name="tickInterval">
<number>128</number>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QSlider" name="ch7">
<property name="minimum">
<number>-511</number>
</property>
<property name="maximum">
<number>512</number>
</property>
<property name="singleStep">
<number>16</number>
</property>
<property name="pageStep">
<number>32</number>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="tickPosition">
<enum>QSlider::TicksAbove</enum>
</property>
<property name="tickInterval">
<number>128</number>
</property>
</widget>
</item>
<item row="7" column="1">
<widget class="QSlider" name="ch8">
<property name="minimum">
<number>-511</number>
</property>
<property name="maximum">
<number>512</number>
</property>
<property name="singleStep">
<number>16</number>
</property>
<property name="pageStep">
<number>32</number>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="tickPosition">
<enum>QSlider::TicksAbove</enum>
</property>
<property name="tickInterval">
<number>128</number>
</property>
</widget>
</item>
</layout>
</widget>
</item>
<item>
<spacer name="verticalSpacer">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>40</height>
</size>
</property>
</spacer>
</item>
</layout>
</widget>
</widget>
</item>
</layout>
</widget>
<layoutdefault spacing="6" margin="11"/>
<resources/>
<connections/>
</ui>

32
ground/openpilotgcs/src/plugins/hitlv2/hitlv2.pro
View File

@ -1,27 +1,5 @@
TEMPLATE = lib
TARGET = HITLv2
QT += network
include(../../openpilotgcsplugin.pri)
include(hitlv2_dependencies.pri)
HEADERS += \
aerosimrc.h \
hitlv2configuration.h \
hitlv2factory.h \
hitlv2gadget.h \
hitlv2optionspage.h \
hitlv2plugin.h \
hitlv2widget.h \
simulatorv2.h
SOURCES += \
aerosimrc.cpp \
hitlv2configuration.cpp \
hitlv2factory.cpp \
hitlv2gadget.cpp \
hitlv2optionspage.cpp \
hitlv2plugin.cpp \
hitlv2widget.cpp \
simulatorv2.cpp
OTHER_FILES += hitlv2.pluginspec
FORMS += \
hitlv2optionspage.ui \
hitlv2widget.ui
TEMPLATE = subdirs
SUBDIRS = plugin aerosimrc
plugin.file = plugin.pro

8
ground/openpilotgcs/src/plugins/hitlv2/hitlv2configuration.cpp
View File

@ -1,13 +1,13 @@
/**
******************************************************************************
*
* @file hitlconfiguration.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @file hitlv2configuration.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
* @addtogroup GCSPlugins GCS Plugins
* @{
* @addtogroup HITLPlugin HITL Plugin
* @addtogroup HITLPlugin HITLv2 Plugin
* @{
* @brief The Hardware In The Loop plugin
* @brief The Hardware In The Loop plugin version 2
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify

8
ground/openpilotgcs/src/plugins/hitlv2/hitlv2configuration.h
View File

@ -1,13 +1,13 @@
/**
******************************************************************************
*
* @file hitlconfiguration.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @file hitlv2configuration.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
* @addtogroup GCSPlugins GCS Plugins
* @{
* @addtogroup HITLPlugin HITL Plugin
* @addtogroup HITLPlugin HITLv2 Plugin
* @{
* @brief The Hardware In The Loop plugin
* @brief The Hardware In The Loop plugin version 2
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify

9
ground/openpilotgcs/src/plugins/hitlv2/hitlv2factory.cpp
View File

@ -1,13 +1,13 @@
/**
******************************************************************************
*
* @file hitlfactory.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @file hitlv2factory.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
* @addtogroup GCSPlugins GCS Plugins
* @{
* @addtogroup HITLPlugin HITL Plugin
* @addtogroup HITLPlugin HITLv2 Plugin
* @{
* @brief The Hardware In The Loop plugin
* @brief The Hardware In The Loop plugin version 2
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
@ -24,6 +24,7 @@
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "hitlv2factory.h"
#include "hitlv2widget.h"
#include "hitlv2gadget.h"

8
ground/openpilotgcs/src/plugins/hitlv2/hitlv2factory.h
View File

@ -1,13 +1,13 @@
/**
******************************************************************************
*
* @file hitlfactory.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @file hitlv2factory.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
* @addtogroup GCSPlugins GCS Plugins
* @{
* @addtogroup HITLPlugin HITL Plugin
* @addtogroup HITLPlugin HITLv2 Plugin
* @{
* @brief The Hardware In The Loop plugin
* @brief The Hardware In The Loop plugin version 2
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify

9
ground/openpilotgcs/src/plugins/hitlv2/hitlv2gadget.cpp
View File

@ -1,13 +1,13 @@
/**
******************************************************************************
*
* @file hitl.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @file hitlv2gadget.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
* @addtogroup GCSPlugins GCS Plugins
* @{
* @addtogroup HITLPlugin HITL Plugin
* @addtogroup HITLPlugin HITLv2 Plugin
* @{
* @brief The Hardware In The Loop plugin
* @brief The Hardware In The Loop plugin version 2
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
@ -24,6 +24,7 @@
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "hitlv2gadget.h"
#include "hitlv2widget.h"
#include "hitlv2configuration.h"

8
ground/openpilotgcs/src/plugins/hitlv2/hitlv2gadget.h
View File

@ -1,13 +1,13 @@
/**
******************************************************************************
*
* @file hitl.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @file hitlv2gadget.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
* @addtogroup GCSPlugins GCS Plugins
* @{
* @addtogroup HITLPlugin HITL Plugin
* @addtogroup HITLPlugin HITLv2 Plugin
* @{
* @brief The Hardware In The Loop plugin
* @brief The Hardware In The Loop plugin version 2
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify

8
ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.cpp
View File

@ -1,13 +1,13 @@
/**
******************************************************************************
*
* @file hitloptionspage.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @file hitlv2optionspage.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
* @addtogroup GCSPlugins GCS Plugins
* @{
* @addtogroup HITLPlugin HITL Plugin
* @addtogroup HITLPlugin HITLv2 Plugin
* @{
* @brief The Hardware In The Loop plugin
* @brief The Hardware In The Loop plugin version 2
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify

8
ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.h
View File

@ -1,13 +1,13 @@
/**
******************************************************************************
*
* @file hitloptionspage.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @file hitlv2optionspage.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
* @addtogroup GCSPlugins GCS Plugins
* @{
* @addtogroup HITLPlugin HITL Plugin
* @addtogroup HITLPlugin HITLv2 Plugin
* @{
* @brief The Hardware In The Loop plugin
* @brief The Hardware In The Loop plugin version 2
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify

9
ground/openpilotgcs/src/plugins/hitlv2/hitlv2plugin.cpp
View File

@ -1,13 +1,13 @@
/**
******************************************************************************
*
* @file mapplugin.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @file hitlv2plugin.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
* @addtogroup GCSPlugins GCS Plugins
* @{
* @addtogroup HITLPlugin HITL Plugin
* @addtogroup HITLPlugin HITLv2 Plugin
* @{
* @brief The Hardware In The Loop plugin
* @brief The Hardware In The Loop plugin version 2
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
@ -24,6 +24,7 @@
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "hitlv2plugin.h"
#include "hitlv2factory.h"
#include <QtPlugin>

8
ground/openpilotgcs/src/plugins/hitlv2/hitlv2plugin.h
View File

@ -1,13 +1,13 @@
/**
******************************************************************************
*
* @file browserplugin.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @file hitlv2plugin.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
* @addtogroup GCSPlugins GCS Plugins
* @{
* @addtogroup HITLPlugin HITL Plugin
* @addtogroup HITLPlugin HITLv2 Plugin
* @{
* @brief The Hardware In The Loop plugin
* @brief The Hardware In The Loop plugin version 2
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify

9
ground/openpilotgcs/src/plugins/hitlv2/hitlv2widget.cpp
View File

@ -1,13 +1,13 @@
/**
******************************************************************************
*
* @file hitlwidget.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @file hitlv2widget.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
* @addtogroup GCSPlugins GCS Plugins
* @{
* @addtogroup HITLPlugin HITL Plugin
* @addtogroup HITLPlugin HITLv2 Plugin
* @{
* @brief The Hardware In The Loop plugin
* @brief The Hardware In The Loop plugin version 2
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
@ -24,6 +24,7 @@
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "hitlv2widget.h"
#include "ui_hitlv2widget.h"
#include <QDebug>

8
ground/openpilotgcs/src/plugins/hitlv2/hitlv2widget.h
View File

@ -1,13 +1,13 @@
/**
******************************************************************************
*
* @file hitlwidget.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @file hitlv2widget.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
* @addtogroup GCSPlugins GCS Plugins
* @{
* @addtogroup HITLPlugin HITL Plugin
* @addtogroup HITLPlugin HITLv2 Plugin
* @{
* @brief The Hardware In The Loop plugin
* @brief The Hardware In The Loop plugin version 2
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify

32
ground/openpilotgcs/src/plugins/hitlv2/plugin.pro Normal file
View File

@ -0,0 +1,32 @@
TEMPLATE = lib
TARGET = HITLv2
QT += network
include(../../openpilotgcsplugin.pri)
include(hitlv2_dependencies.pri)
HEADERS += \
aerosimrc.h \
hitlv2configuration.h \
hitlv2factory.h \
hitlv2gadget.h \
hitlv2optionspage.h \
hitlv2plugin.h \
hitlv2widget.h \
simulatorv2.h
SOURCES += \
aerosimrc.cpp \
hitlv2configuration.cpp \
hitlv2factory.cpp \
hitlv2gadget.cpp \
hitlv2optionspage.cpp \
hitlv2plugin.cpp \
hitlv2widget.cpp \
simulatorv2.cpp
FORMS += \
hitlv2optionspage.ui \
hitlv2widget.ui
OTHER_FILES += hitlv2.pluginspec

8
ground/openpilotgcs/src/plugins/hitlv2/simulatorv2.cpp
View File

@ -1,13 +1,13 @@
/**
******************************************************************************
*
* @file simulator.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @file simulatorv2.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
* @addtogroup GCSPlugins GCS Plugins
* @{
* @addtogroup HITLPlugin HITL Plugin
* @addtogroup HITLPlugin HITLv2 Plugin
* @{
* @brief The Hardware In The Loop plugin
* @brief The Hardware In The Loop plugin version 2
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify

8
ground/openpilotgcs/src/plugins/hitlv2/simulatorv2.h
View File

@ -1,13 +1,13 @@
/**
******************************************************************************
*
* @file simulator.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @file simulatorv2.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
* @addtogroup GCSPlugins GCS Plugins
* @{
* @addtogroup HITLPlugin HITL Plugin
* @addtogroup HITLPlugin HITLv2 Plugin
* @{
* @brief The Hardware In The Loop plugin
* @brief The Hardware In The Loop plugin version 2
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify

2
ground/openpilotgcs/src/plugins/rawhid/rawhid.cpp
View File

@ -41,7 +41,7 @@ class IConnection;
static const int READ_TIMEOUT = 200;
static const int READ_SIZE = 64;
static const int WRITE_TIMEOUT = 200;
static const int WRITE_TIMEOUT = 1000;
static const int WRITE_SIZE = 64;

13
ground/openpilotgcs/src/plugins/systemhealth/html/AlarmOK.html Normal file
View File

@ -0,0 +1,13 @@
<html>
<head>
<title></title>
<meta content="">
<style></style>
</head>
<body>
<h1>Alarm: OK</h1>
<p>
There are no problems with this alarm.
</p>
</body>
</html>

23
ground/openpilotgcs/src/plugins/systemhealth/systemhealth.qrc
View File

@ -3,5 +3,28 @@
<file>html/Actuator-Critical.html</file>
<file>html/ManualControl-Critical.html</file>
<file>html/ManualControl-Warning.html</file>
<file>html/CPU-Critical.html</file>
<file>html/CPU-Warning.html</file>
<file>html/FlightTime-Error.html</file>
<file>html/Battery-Warning.html</file>
<file>html/BootFault-Critical.html</file>
<file>html/EventSystem-Warning.html</file>
<file>html/FlightTime-Critical.html</file>
<file>html/AlarmOK.html</file>
<file>html/Attitude-Critical.html</file>
<file>html/Attitude-Error.html</file>
<file>html/Battery-Critical.html</file>
<file>html/Battery-Error.html</file>
<file>html/FlightTime-Warning.html</file>
<file>html/GPS-Critical.html</file>
<file>html/GPS-Error.html</file>
<file>html/GPS-Warning.html</file>
<file>html/Guidance-Warning.html</file>
<file>html/Memory-Critical.html</file>
<file>html/Memory-Warning.html</file>
<file>html/Sensors-Critical.html</file>
<file>html/Stabilization-Warning.html</file>
<file>html/Stack-Critical.html</file>
<file>html/Telemetry-Error.html</file>
</qresource>
</RCC>

62
ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetwidget.cpp
View File

@ -63,6 +63,7 @@ SystemHealthGadgetWidget::SystemHealthGadgetWidget(QWidget *parent) : QGraphicsV
connect(telMngr, SIGNAL(connected()), this, SLOT(onAutopilotConnect()));
connect(telMngr, SIGNAL(disconnected()), this, SLOT(onAutopilotDisconnect()));
setToolTip(tr("Displays flight system errors. Click on an alarm for more information."));
}
/**
@ -202,16 +203,67 @@ void SystemHealthGadgetWidget::mousePressEvent ( QMouseEvent * event )
QGraphicsScene *graphicsScene = scene();
if(graphicsScene){
QPoint point = event->pos();
bool haveAlarmItem = false;
foreach(QGraphicsItem* sceneItem, items(point)){
QGraphicsSvgItem *clickedItem = dynamic_cast<QGraphicsSvgItem*>(sceneItem);
if(clickedItem && (clickedItem != foreground) && clickedItem != background){
QFile alarmDescription(":/systemhealth/html/" + clickedItem->elementId() + ".html");
if(alarmDescription.open(QIODevice::ReadOnly | QIODevice::Text)){
QTextStream textStream(&alarmDescription);
QWhatsThis::showText(event->globalPos(), textStream.readAll());
if(clickedItem){
if((clickedItem != foreground) && (clickedItem != background)){
// Clicked an actual alarm. We need to set haveAlarmItem to true
// as two of the items in this loop will always be foreground and
// background. Without this flag, at some point in the loop we
// would always call showAllAlarmDescriptions...
haveAlarmItem = true;
QString itemId = clickedItem->elementId();
if(itemId.contains("OK")){
// No alarm set for this item
showAlarmDescriptionForItemId("AlarmOK", event->globalPos());
}else{
// Warning, error or critical alarm
showAlarmDescriptionForItemId(itemId, event->globalPos());
}
}else if(!haveAlarmItem){
// Clicked foreground or background
showAllAlarmDescriptions(event->globalPos());
}
}
}
}
}
void SystemHealthGadgetWidget::showAlarmDescriptionForItemId(const QString itemId, const QPoint& location){
QFile alarmDescription(":/systemhealth/html/" + itemId + ".html");
if(alarmDescription.open(QIODevice::ReadOnly | QIODevice::Text)){
QTextStream textStream(&alarmDescription);
QWhatsThis::showText(location, textStream.readAll());
}
}
void SystemHealthGadgetWidget::showAllAlarmDescriptions(const QPoint& location){
QGraphicsScene *graphicsScene = scene();
if(graphicsScene){
QString alarmsText;
// Loop through all items in the scene looking for svg items that represent alarms
foreach(QGraphicsItem* curItem, graphicsScene->items()){
QGraphicsSvgItem* curSvgItem = dynamic_cast<QGraphicsSvgItem*>(curItem);
if(curSvgItem && (curSvgItem != foreground) && (curSvgItem != background)){
QString elementId = curSvgItem->elementId();
if(!elementId.contains("OK")){
// Found an alarm, get its corresponding alarm html file contents
// and append to the cumulative string for all alarms.
QFile alarmDescription(":/systemhealth/html/" + elementId + ".html");
if(alarmDescription.open(QIODevice::ReadOnly | QIODevice::Text)){
QTextStream textStream(&alarmDescription);
alarmsText.append(textStream.readAll());
}
}
}
}
// Show alarms text if we have any
if(alarmsText.length() > 0){
QWhatsThis::showText(location, alarmsText);
}
}
}

3
ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetwidget.h
View File

@ -69,5 +69,8 @@ private:
// Simple flag to skip rendering if the
bool fgenabled; // layer does not exist.
void showAlarmDescriptionForItemId(const QString itemId, const QPoint& location);
void showAllAlarmDescriptions(const QPoint &location);
};
#endif /* SYSTEMHEALTHGADGETWIDGET_H_ */

13
ground/openpilotgcs/src/plugins/uavobjectbrowser/treeitem.cpp
View File

@ -129,21 +129,23 @@ void TreeItem::appendChild(TreeItem *child)
child->setParentTree(this);
}
void TreeItem::insert(int index, TreeItem *child)
void TreeItem::insertChild(TreeItem *child)
{
int index = nameIndex(child->data(0).toString());
m_children.insert(index, child);
child->setParentTree(this);
}
TreeItem *TreeItem::child(int row)
TreeItem *TreeItem::getChild(int index)
{
return m_children.value(row);
return m_children.value(index);
}
int TreeItem::childCount() const
{
return m_children.count();
}
int TreeItem::row() const
{
if (m_parent)
@ -224,3 +226,8 @@ QTime TreeItem::getHiglightExpires()
{
return m_highlightExpires;
}
QList<MetaObjectTreeItem *> TopTreeItem::getMetaObjectItems()
{
return m_metaObjectTreeItemsPerObjectIds.values();
}

54
ground/openpilotgcs/src/plugins/uavobjectbrowser/treeitem.h
View File

@ -33,6 +33,7 @@
#include "uavobjectfield.h"
#include <QtCore/QList>
#include <QtCore/QLinkedList>
#include <QtCore/QMap>
#include <QtCore/QVariant>
#include <QtCore/QTime>
#include <QtCore/QTimer>
@ -94,9 +95,9 @@ public:
virtual ~TreeItem();
void appendChild(TreeItem *child);
void insert(int index, TreeItem *child);
void insertChild(TreeItem *child);
TreeItem *child(int row);
TreeItem *getChild(int index);
inline QList<TreeItem*> treeChildren() const { return m_children; }
int childCount() const;
int columnCount() const;
@ -131,6 +132,24 @@ public:
virtual void removeHighlight();
int nameIndex(QString name) {
for (int i = 0; i < childCount(); ++i) {
if (name < getChild(i)->data(0).toString())
return i;
}
return childCount();
}
TreeItem* findChildByName(QString name)
{
foreach (TreeItem* child, m_children) {
if (name == child->data(0).toString()) {
return child;
}
}
return 0;
}
signals:
void updateHighlight(TreeItem*);
@ -152,6 +171,9 @@ private:
static int m_highlightTimeMs;
};
class DataObjectTreeItem;
class MetaObjectTreeItem;
class TopTreeItem : public TreeItem
{
Q_OBJECT
@ -159,19 +181,27 @@ public:
TopTreeItem(const QList<QVariant> &data, TreeItem *parent = 0) : TreeItem(data, parent) { }
TopTreeItem(const QVariant &data, TreeItem *parent = 0) : TreeItem(data, parent) { }
QList<quint32> objIds() { return m_objIds; }
void addObjId(quint32 objId) { m_objIds.append(objId); }
void insertObjId(int index, quint32 objId) { m_objIds.insert(index, objId); }
int nameIndex(QString name) {
for (int i = 0; i < childCount(); ++i) {
if (name < child(i)->data(0).toString())
return i;
}
return childCount();
void addObjectTreeItem(quint32 objectId, DataObjectTreeItem* oti) {
m_objectTreeItemsPerObjectIds[objectId] = oti;
}
DataObjectTreeItem* findDataObjectTreeItemByObjectId(quint32 objectId) {
return m_objectTreeItemsPerObjectIds.contains(objectId) ? m_objectTreeItemsPerObjectIds[objectId] : 0;
}
void addMetaObjectTreeItem(quint32 objectId, MetaObjectTreeItem* oti) {
m_metaObjectTreeItemsPerObjectIds[objectId] = oti;
}
MetaObjectTreeItem* findMetaObjectTreeItemByObjectId(quint32 objectId) {
return m_metaObjectTreeItemsPerObjectIds.contains(objectId) ? m_metaObjectTreeItemsPerObjectIds[objectId] : 0;
}
QList<MetaObjectTreeItem*> getMetaObjectItems();
private:
QList<quint32> m_objIds;
QMap<quint32, DataObjectTreeItem*> m_objectTreeItemsPerObjectIds;
QMap<quint32, MetaObjectTreeItem*> m_metaObjectTreeItemsPerObjectIds;
};
class ObjectTreeItem : public TreeItem

1
ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowser.cpp
View File

@ -45,5 +45,6 @@ void UAVObjectBrowser::loadConfiguration(IUAVGadgetConfiguration* config)
m_widget->setRecentlyUpdatedColor(m->recentlyUpdatedColor());
m_widget->setManuallyChangedColor(m->manuallyChangedColor());
m_widget->setRecentlyUpdatedTimeout(m->recentlyUpdatedTimeout());
m_widget->setOnlyHilightChangedValues(m->onlyHighlightChangedValues());
}

13
ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowser.ui
View File

@ -202,6 +202,19 @@
</property>
</widget>
</item>
<item>
<widget class="QCheckBox" name="categorizeCheckbox">
<property name="toolTip">
<string>Select to sort objects in to categories.</string>
</property>
<property name="text">
<string>Categorize Objects</string>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
<item>
<spacer name="horizontalSpacer_3">
<property name="orientation">

Some files were not shown because too many files have changed in this diff Show More