diff --git a/.gitignore b/.gitignore index b57f248a9..88d97efdd 100644 --- a/.gitignore +++ b/.gitignore @@ -20,6 +20,9 @@ /flight/OpenPilot/Build /flight/OpenPilot/Build.win32 +#flight/Project/OpenPilotOSX +flight/Project/OpenPilotOSX/build + # /flight/PipBee/ /flight/PipBee/Build diff --git a/HISTORY.txt b/HISTORY.txt index 4d22602df..838f25d05 100644 --- a/HISTORY.txt +++ b/HISTORY.txt @@ -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 diff --git a/flight/CopterControl/Makefile b/flight/CopterControl/Makefile index 82334e56a..7c5498806 100644 --- a/flight/CopterControl/Makefile +++ b/flight/CopterControl/Makefile @@ -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 diff --git a/flight/CopterControl/System/inc/pios_config.h b/flight/CopterControl/System/inc/pios_config.h index e4b70e931..bcf0b0b23 100644 --- a/flight/CopterControl/System/inc/pios_config.h +++ b/flight/CopterControl/System/inc/pios_config.h @@ -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 diff --git a/flight/Libraries/inc/packet_handler.h b/flight/Libraries/inc/packet_handler.h index b0bd8315c..1e9f60807 100644 --- a/flight/Libraries/inc/packet_handler.h +++ b/flight/Libraries/inc/packet_handler.h @@ -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); diff --git a/flight/Libraries/packet_handler.c b/flight/Libraries/packet_handler.c index 59a9a187f..f4e8e28d7 100644 --- a/flight/Libraries/packet_handler.c +++ b/flight/Libraries/packet_handler.c @@ -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); diff --git a/flight/Modules/Attitude/attitude.c b/flight/Modules/Attitude/attitude.c index 02ba45744..baa0c5453 100644 --- a/flight/Modules/Attitude/attitude.c +++ b/flight/Modules/Attitude/attitude.c @@ -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(); diff --git a/flight/Modules/FlightPlan/lib/uavobject.py b/flight/Modules/FlightPlan/lib/uavobject.py index aafe76a43..bf2d622e8 100644 --- a/flight/Modules/FlightPlan/lib/uavobject.py +++ b/flight/Modules/FlightPlan/lib/uavobject.py @@ -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__ diff --git a/flight/Modules/GPS/GPS.c b/flight/Modules/GPS/GPS.c index 9d02c06e4..5851ac10d 100644 --- a/flight/Modules/GPS/GPS.c +++ b/flight/Modules/GPS/GPS.c @@ -33,13 +33,12 @@ #include "openpilot.h" #include "GPS.h" -#include - #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); } } diff --git a/flight/Modules/GPS/NMEA.c b/flight/Modules/GPS/NMEA.c index a2b4c562c..6258484e7 100644 --- a/flight/Modules/GPS/NMEA.c +++ b/flight/Modules/GPS/NMEA.c @@ -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 diff --git a/flight/Modules/GPS/UBX.c b/flight/Modules/GPS/UBX.c index 180834281..92d5e08bf 100644 --- a/flight/Modules/GPS/UBX.c +++ b/flight/Modules/GPS/UBX.c @@ -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 diff --git a/flight/Modules/GPS/inc/GPS.h b/flight/Modules/GPS/inc/GPS.h index 86a92d285..6e243c21f 100644 --- a/flight/Modules/GPS/inc/GPS.h +++ b/flight/Modules/GPS/inc/GPS.h @@ -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 diff --git a/flight/Modules/GPS/inc/NMEA.h b/flight/Modules/GPS/inc/NMEA.h index 297e07cad..49213e716 100644 --- a/flight/Modules/GPS/inc/NMEA.h +++ b/flight/Modules/GPS/inc/NMEA.h @@ -33,8 +33,12 @@ #include #include +#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 */ diff --git a/flight/Modules/GPS/inc/UBX.h b/flight/Modules/GPS/inc/UBX.h index 21cd60ce4..dc27fa798 100644 --- a/flight/Modules/GPS/inc/UBX.h +++ b/flight/Modules/GPS/inc/UBX.h @@ -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 */ diff --git a/flight/Modules/RadioComBridge/RadioComBridge.c b/flight/Modules/RadioComBridge/RadioComBridge.c index ceb547fd1..eb674cbdd 100644 --- a/flight/Modules/RadioComBridge/RadioComBridge.c +++ b/flight/Modules/RadioComBridge/RadioComBridge.c @@ -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) { diff --git a/flight/OSD/Makefile b/flight/OSD/Makefile index cfed11dfe..b86f5ef65 100644 --- a/flight/OSD/Makefile +++ b/flight/OSD/Makefile @@ -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 diff --git a/flight/OSD/System/inc/pios_config.h b/flight/OSD/System/inc/pios_config.h index 0d08f4a7a..5d3004f30 100644 --- a/flight/OSD/System/inc/pios_config.h +++ b/flight/OSD/System/inc/pios_config.h @@ -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 diff --git a/flight/PiOS.posix/inc/pios_struct_helper.h b/flight/PiOS.posix/inc/pios_struct_helper.h new file mode 100644 index 000000000..6196d9b26 --- /dev/null +++ b/flight/PiOS.posix/inc/pios_struct_helper.h @@ -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) );}) diff --git a/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h b/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h index 868bba722..71426d594 100755 --- a/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h +++ b/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h @@ -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) diff --git a/flight/PiOS/STM32F10x/Libraries/CMSIS/Core/CM3/core_cm3.c b/flight/PiOS/STM32F10x/Libraries/CMSIS/Core/CM3/core_cm3.c index 56fddc52b..d202e369b 100755 --- a/flight/PiOS/STM32F10x/Libraries/CMSIS/Core/CM3/core_cm3.c +++ b/flight/PiOS/STM32F10x/Libraries/CMSIS/Core/CM3/core_cm3.c @@ -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); } diff --git a/flight/PiOS/STM32F10x/link_STM32103CB_PIPXTREME_sections.ld b/flight/PiOS/STM32F10x/link_STM32103CB_PIPXTREME_sections.ld index fbb858118..074dad236 100644 --- a/flight/PiOS/STM32F10x/link_STM32103CB_PIPXTREME_sections.ld +++ b/flight/PiOS/STM32F10x/link_STM32103CB_PIPXTREME_sections.ld @@ -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). diff --git a/flight/PiOS/inc/pios_struct_helper.h b/flight/PiOS/inc/pios_struct_helper.h new file mode 100644 index 000000000..6196d9b26 --- /dev/null +++ b/flight/PiOS/inc/pios_struct_helper.h @@ -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) );}) diff --git a/flight/PipXtreme/System/pios_board.c b/flight/PipXtreme/System/pios_board.c index 6b577609e..3a2dd0544 100644 --- a/flight/PipXtreme/System/pios_board.c +++ b/flight/PipXtreme/System/pios_board.c @@ -33,26 +33,27 @@ #include #include -#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; diff --git a/flight/Project/gdb/simposix b/flight/Project/gdb/simposix new file mode 100644 index 000000000..877eb2920 --- /dev/null +++ b/flight/Project/gdb/simposix @@ -0,0 +1 @@ +handle SIGUSR1 noprint nostop \ No newline at end of file diff --git a/flight/Revolution/System/inc/pios_config.h b/flight/Revolution/System/inc/pios_config.h index e0cea289b..ce5db7f85 100644 --- a/flight/Revolution/System/inc/pios_config.h +++ b/flight/Revolution/System/inc/pios_config.h @@ -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 */ +/** + * @} + * @} + */ diff --git a/flight/Revolution/UAVObjects.inc b/flight/Revolution/UAVObjects.inc index d5b952c11..490e29d36 100644 --- a/flight/Revolution/UAVObjects.inc +++ b/flight/Revolution/UAVObjects.inc @@ -48,6 +48,7 @@ UAVOBJSRCFILENAMES += flighttelemetrystats UAVOBJSRCFILENAMES += gcstelemetrystats UAVOBJSRCFILENAMES += gpsposition UAVOBJSRCFILENAMES += gpssatellites +UAVOBJSRCFILENAMES += gpssettings UAVOBJSRCFILENAMES += gpstime UAVOBJSRCFILENAMES += gpsvelocity UAVOBJSRCFILENAMES += fixedwingpathfollowersettings diff --git a/flight/SimPosix/Makefile b/flight/SimPosix/Makefile index 14c8eba51..857022c16 100644 --- a/flight/SimPosix/Makefile +++ b/flight/SimPosix/Makefile @@ -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 diff --git a/flight/SimPosix/UAVObjects.inc b/flight/SimPosix/UAVObjects.inc index d7449a0d0..00956951d 100644 --- a/flight/SimPosix/UAVObjects.inc +++ b/flight/SimPosix/UAVObjects.inc @@ -48,6 +48,7 @@ UAVOBJSRCFILENAMES += gcstelemetrystats UAVOBJSRCFILENAMES += gpsposition UAVOBJSRCFILENAMES += gpssatellites UAVOBJSRCFILENAMES += gpstime +UAVOBJSRCFILENAMES += gpssettings UAVOBJSRCFILENAMES += gpsvelocity UAVOBJSRCFILENAMES += vtolpathfollowersettings UAVOBJSRCFILENAMES += fixedwingpathfollowersettings diff --git a/flight/UAVObjects/inc/uavobjectmanager.h b/flight/UAVObjects/inc/uavobjectmanager.h index fe7c21d07..62aff543d 100644 --- a/flight/UAVObjects/inc/uavobjectmanager.h +++ b/flight/UAVObjects/inc/uavobjectmanager.h @@ -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 diff --git a/flight/UAVObjects/inc/uavobjecttemplate.h b/flight/UAVObjects/inc/uavobjecttemplate.h index f106dd8ba..2938c218f 100644 --- a/flight/UAVObjects/inc/uavobjecttemplate.h +++ b/flight/UAVObjects/inc/uavobjecttemplate.h @@ -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) diff --git a/flight/UAVObjects/uavobjectmanager.c b/flight/UAVObjects/uavobjectmanager.c index 8dc077967..2b2b69018 100644 --- a/flight/UAVObjects/uavobjectmanager.c +++ b/flight/UAVObjects/uavobjectmanager.c @@ -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 */ diff --git a/flight/UAVObjects/uavobjecttemplate.c b/flight/UAVObjects/uavobjecttemplate.c index ad37eef83..f8db8a024 100644 --- a/flight/UAVObjects/uavobjecttemplate.c +++ b/flight/UAVObjects/uavobjecttemplate.c @@ -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 diff --git a/ground/openpilotgcs/src/plugins/config/airframe.ui b/ground/openpilotgcs/src/plugins/config/airframe.ui index 47006a789..ba3396ac7 100644 --- a/ground/openpilotgcs/src/plugins/config/airframe.ui +++ b/ground/openpilotgcs/src/plugins/config/airframe.ui @@ -2788,7 +2788,7 @@ p, li { white-space: pre-wrap; } <table border="0" style="-qt-table-type: root; margin-top:4px; margin-bottom:4px; margin-left:4px; margin-right:4px;"> <tr> <td style="border: none;"> -<p align="center" style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:14pt; font-weight:600; color:#ff0000;">SETTING UP FEED FORWARD IS DANGEROUS</span></p> +<p align="center" style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:14pt; font-weight:600; color:#ff0000;">SETTING UP FEED FORWARD NEEDS CAUTION</span></p> <p style="-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;"></p> <p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Beware: Feed Forward Tuning will launch all engines around mid-throttle, you have been warned!</p> <p style=" margin-top:12px; margin-bottom:12px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">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.</p></td></tr></table></body></html> diff --git a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.cpp b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.cpp index d27f4c530..bfa4fe7a2 100644 --- a/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/cfg_vehicletypes/configmultirotorwidget.cpp @@ -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; } diff --git a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp index fb1223607..7808b6dfa 100644 --- a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp @@ -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); +} + /** @} @} diff --git a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.h b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.h index b702326e8..28db67b74 100644 --- a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.h +++ b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.h @@ -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 diff --git a/ground/openpilotgcs/src/plugins/config/input.ui b/ground/openpilotgcs/src/plugins/config/input.ui index e00f4baf3..199f52620 100644 --- a/ground/openpilotgcs/src/plugins/config/input.ui +++ b/ground/openpilotgcs/src/plugins/config/input.ui @@ -17,7 +17,7 @@ - 1 + 0 diff --git a/ground/openpilotgcs/src/plugins/config/pipxtreme.ui b/ground/openpilotgcs/src/plugins/config/pipxtreme.ui index 8a5c2dec4..9de1a7120 100644 --- a/ground/openpilotgcs/src/plugins/config/pipxtreme.ui +++ b/ground/openpilotgcs/src/plugins/config/pipxtreme.ui @@ -6,8 +6,8 @@ 0 0 - 840 - 834 + 834 + 772 @@ -81,7 +81,7 @@ 40 - 20 + 5 @@ -135,16 +135,56 @@ + + + + + + + + + + false + + + + 100 + 16777215 + + + + Broadcast + + + + + + + Broadcast Address + + + + - - + + + + + 100 + 16777215 + + + + 12345678 + + - + -127 @@ -163,17 +203,31 @@ - + + + + -100dB + + + + - - + + + + + 100 + 16777215 + + + - + -127 @@ -192,17 +246,31 @@ - + + + + -100dB + + + + - - + + + + + 100 + 16777215 + + + - + -127 @@ -221,17 +289,31 @@ - + + + + -100dB + + + + - - + + + + + 100 + 16777215 + + + - + -127 @@ -250,28 +332,7 @@ - - - - -100dB - - - - - - - -100dB - - - - - - - -100dB - - - - + -100dB @@ -285,7 +346,7 @@ - 400 + 430 0 @@ -302,15 +363,27 @@ - Firmware Version + Firmware Ver. Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - + + + + 0 + 0 + + + + + 16777215 + 16777215 + + 75 @@ -321,7 +394,7 @@ 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 @@ - + @@ -353,6 +426,12 @@ 0 + + + 0 + 0 + + 16777215 @@ -376,7 +455,7 @@ 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 @@ + + + + Device ID + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 0 + 0 + + + + + 101 + 16777215 + + + + + 75 + true + + + + QLineEdit { + border: none; + border-radius: 1px; + padding: 0 4px; + background: rgba(0, 0, 0, 16); +/* background: transparent; */ +/* selection-background-color: darkgray;*/ +} + + + false + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + 12345678 + + + + + + + Pair ID + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 101 + 16777215 + + + + + 75 + true + + + + QLineEdit { + border: none; + border-radius: 1px; + padding: 0 4px; + background: rgba(0, 0, 0, 16); +/* background: transparent; */ +/* selection-background-color: darkgray;*/ +} + + + false + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + 90ABCDEF + + + @@ -411,9 +592,15 @@ 0 + + + 0 + 0 + + - 16777215 + 101 16777215 @@ -430,7 +617,7 @@ 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 @@ - + Max Frequency @@ -454,7 +641,7 @@ - + @@ -464,7 +651,7 @@ - 16777215 + 101 16777215 @@ -481,7 +668,7 @@ 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 @@ - + Freq. Step Size @@ -505,7 +692,7 @@ - + @@ -513,9 +700,15 @@ 0 + + + 0 + 0 + + - 16777215 + 101 16777215 @@ -532,7 +725,7 @@ 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 @@ + + + + Freq. Band + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 0 + 0 + + + + + 101 + 16777215 + + + + + 75 + true + + + + The current frequency band + + + QLineEdit { + border: none; + border-radius: 1px; + padding: 0 4px; + background: rgba(0, 0, 0, 16); +/* background: transparent; */ +/* selection-background-color: darkgray;*/ +} + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + RSSI + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 0 + 0 + + + + + 101 + 16777215 + + + + + 75 + true + + + + QLineEdit { + border: none; + border-radius: 1px; + padding: 0 4px; + background: rgba(0, 0, 0, 16); +/* background: transparent; */ +/* selection-background-color: darkgray;*/ +} + + + true + + + + + + + Rx AFC + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 101 + 16777215 + + + + + 75 + true + + + + QLineEdit { + border: none; + border-radius: 1px; + padding: 0 4px; + background: rgba(0, 0, 0, 16); +/* background: transparent; */ +/* selection-background-color: darkgray;*/ +} + + + true + + + + + + TX Rate (B/s) + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 0 + 0 + + + + + 101 + 16777215 + + + + + 75 + true + + + + QLineEdit { + border: none; + border-radius: 1px; + padding: 0 4px; + background: rgba(0, 0, 0, 16); +/* background: transparent; */ +/* selection-background-color: darkgray;*/ +} + + + false + + + true + + + + + + + RX Rate (B/s) + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 101 + 16777215 + + + + + 75 + true + + + + QLineEdit { + border: none; + border-radius: 1px; + padding: 0 4px; + background: rgba(0, 0, 0, 16); +/* background: transparent; */ +/* selection-background-color: darkgray;*/ +} + + + false + + + true + + + + Link State @@ -556,17 +974,23 @@ - + - + 0 0 + + + 0 + 0 + + - 16777215 + 101 16777215 @@ -583,7 +1007,7 @@ 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 @@ true + + Disconnected + - - + + - Rx AFC + Errors Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - + + + + + 101 + 16777215 + + 75 @@ -619,12 +1052,15 @@ QLineEdit { border: none; border-radius: 1px; - padding: 0 8px; + padding: 0 4px; background: rgba(0, 0, 0, 16); /* background: transparent; */ /* selection-background-color: darkgray;*/ } + + false + true @@ -642,6 +1078,18 @@ + + + 0 + 0 + + + + + 101 + 16777215 + + 75 @@ -652,7 +1100,7 @@ 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 @@ - - + + + + UAVTalk Errors + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 101 + 16777215 + + 75 @@ -678,7 +1142,7 @@ 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 @@ - - - Errors - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - UAVTalk Errors - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - 75 - true - - - - QLineEdit { - border: none; - border-radius: 1px; - padding: 0 8px; - background: rgba(0, 0, 0, 16); -/* background: transparent; */ -/* selection-background-color: darkgray;*/ -} - - - false - - - true - - - - Resets @@ -748,8 +1166,20 @@ - + + + + 0 + 0 + + + + + 101 + 16777215 + + 75 @@ -760,7 +1190,7 @@ 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 @@ - + Dropped @@ -784,8 +1214,14 @@ - + + + + 101 + 16777215 + + 75 @@ -796,7 +1232,7 @@ 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 @@ - - - - TX Rate (B/s) - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - 75 - true - - - - QLineEdit { - border: none; - border-radius: 1px; - padding: 0 8px; - background: rgba(0, 0, 0, 16); -/* background: transparent; */ -/* selection-background-color: darkgray;*/ -} - - - false - - - true - - - - - - - RX Rate (B/s) - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - 75 - true - - - - QLineEdit { - border: none; - border-radius: 1px; - padding: 0 8px; - background: rgba(0, 0, 0, 16); -/* background: transparent; */ -/* selection-background-color: darkgray;*/ -} - - - false - - - true - - - - - - - - 75 - true - - - - QLineEdit { - border: none; - border-radius: 1px; - padding: 0 8px; - background: rgba(0, 0, 0, 16); -/* background: transparent; */ -/* selection-background-color: darkgray;*/ -} - - - false - - - true - - - - - - - Device ID - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - @@ -926,6 +1254,12 @@ + + + 0 + 0 + + 75 diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/Matlab/GCSControl.m b/ground/openpilotgcs/src/plugins/gcscontrol/Matlab/GCSControl.m new file mode 100644 index 000000000..39e89ffc1 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/gcscontrol/Matlab/GCSControl.m @@ -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 \ No newline at end of file diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrol.pro b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrol.pro index 426579e77..2e4695328 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrol.pro +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrol.pro @@ -2,6 +2,7 @@ TEMPLATE = lib TARGET = GCSControl QT += svg QT += opengl +QT += network include(../../openpilotgcsplugin.pri) include(../../plugins/coreplugin/coreplugin.pri) diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrol.ui b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrol.ui index 024db71bb..0d2889785 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrol.ui +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrol.ui @@ -41,8 +41,21 @@ + + + + false + + + UDP Control + + + + + true + Armed diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp index 3c00987f7..b95082fb9 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.cpp @@ -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(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(); UAVDataObject* obj = dynamic_cast( 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; diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.h b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.h index 4d465b0cd..216e6d075 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.h +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadget.h @@ -34,6 +34,9 @@ #include "sdlgamepad/sdlgamepad.h" #include #include "gcscontrolplugin.h" +#include +#include + namespace Core { class IUAVGadget; @@ -59,6 +62,7 @@ public: private: ManualControlCommand* getManualControlCommand(); + double constrain(double value); QTime joystickTime; QWidget *m_widget; QList 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); diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetconfiguration.cpp b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetconfiguration.cpp index 24a72e62a..c213088c6 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetconfiguration.cpp +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetconfiguration.cpp @@ -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++) { diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetconfiguration.h b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetconfiguration.h index c3ca4c6d3..8f1a07359 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetconfiguration.h +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetconfiguration.h @@ -29,6 +29,7 @@ #define GCSCONTROLGADGETCONFIGURATION_H #include +#include 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 getChannelsMapping(); QList getChannelsReverse(); @@ -72,6 +81,8 @@ class GCSControlGadgetConfiguration : public IUAVGadgetConfiguration int throttleChannel; buttonSettingsStruct buttonSettings[8]; bool channelReverse[8]; + int udp_port; + QHostAddress udp_host; }; diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.cpp b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.cpp index ad0bf9a34..1a68c6be6 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.cpp @@ -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); diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.ui b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.ui index 260aa4e04..5191788a0 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.ui +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetoptionspage.ui @@ -141,7 +141,7 @@ - + 0 @@ -1011,6 +1011,66 @@ + + + UDP Setup + + + + + 20 + 20 + 301 + 71 + + + + UDP Port Configuration + + + + + + Host: + + + + + + + + 120 + 16777215 + + + + 127.0.0.1 + + + + + + + Port: + + + + + + + + 50 + 16777215 + + + + 2323 + + + + + + diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.cpp index 6ee123ff6..09e25132d 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.cpp @@ -34,6 +34,7 @@ #include #include + #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(); +} /** diff --git a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.h b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.h index 58c2ecb0f..5f4b3eba0 100644 --- a/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/gcscontrol/gcscontrolgadgetwidget.h @@ -31,6 +31,8 @@ #include #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; diff --git a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsconstellationwidget.cpp b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsconstellationwidget.cpp index d6bddb291..e70f2ed72 100644 --- a/ground/openpilotgcs/src/plugins/gpsdisplay/gpsconstellationwidget.cpp +++ b/ground/openpilotgcs/src/plugins/gpsdisplay/gpsconstellationwidget.cpp @@ -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()); diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc.cpp b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc.cpp index 51daf2460..f8306e30d 100644 --- a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc.cpp +++ b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc.cpp @@ -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 #include diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc.h b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc.h index 19dae3bce..42b7d4aa5 100644 --- a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc.h +++ b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc.h @@ -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 diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/aerosimrc.pro b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/aerosimrc.pro new file mode 100644 index 000000000..74df87185 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/aerosimrc.pro @@ -0,0 +1,10 @@ +TEMPLATE = subdirs + +win32 { + SUBDIRS += plugin +} + +SUBDIRS += udptest + +plugin.file = src/plugin.pro +udptest.file = src/udptest.pro diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/aerosimrcdatastruct.h b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/aerosimrcdatastruct.h new file mode 100644 index 000000000..4adf7c16e --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/aerosimrcdatastruct.h @@ -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 + +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 diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/enums.h b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/enums.h new file mode 100644 index 000000000..31abb1e0a --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/enums.h @@ -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 diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/plugin.cpp b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/plugin.cpp new file mode 100644 index 000000000..18ba2c15d --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/plugin.cpp @@ -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 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; +} diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/plugin.h b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/plugin.h new file mode 100644 index 000000000..a590c8a24 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/plugin.h @@ -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 +#include +#include +#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 diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/plugin.pro b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/plugin.pro new file mode 100644 index 000000000..b31d6881b --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/plugin.pro @@ -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 + } +} diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/qdebughandler.cpp b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/qdebughandler.cpp new file mode 100644 index 000000000..bb9617511 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/qdebughandler.cpp @@ -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(); +} diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/qdebughandler.h b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/qdebughandler.h new file mode 100644 index 000000000..d41dc6a67 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/qdebughandler.h @@ -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 +#include +#include + +void myQDebugHandler(QtMsgType type, const char *msg); + +#endif // QDEBUGHANDLER_H diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/resources/cc_off.tga b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/resources/cc_off.tga new file mode 100644 index 000000000..d8f4e0297 Binary files /dev/null and b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/resources/cc_off.tga differ diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/resources/cc_off_hover.tga b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/resources/cc_off_hover.tga new file mode 100644 index 000000000..e00d86a48 Binary files /dev/null and b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/resources/cc_off_hover.tga differ diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/resources/cc_on.tga b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/resources/cc_on.tga new file mode 100644 index 000000000..424853384 Binary files /dev/null and b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/resources/cc_on.tga differ diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/resources/cc_on_hover.tga b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/resources/cc_on_hover.tga new file mode 100644 index 000000000..406204e32 Binary files /dev/null and b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/resources/cc_on_hover.tga differ diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/resources/cc_plugin.ini b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/resources/cc_plugin.ini new file mode 100644 index 000000000..8b4174f60 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/resources/cc_plugin.ini @@ -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 diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/resources/plugin.txt b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/resources/plugin.txt new file mode 100644 index 000000000..901e5c50a --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/resources/plugin.txt @@ -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 diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/settings.cpp b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/settings.cpp new file mode 100644 index 000000000..6c19f91dc --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/settings.cpp @@ -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(); + } + } +} diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/settings.h b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/settings.h new file mode 100644 index 000000000..5bc39135d --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/settings.h @@ -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 +#include +#include +#include +#include +#include + +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 getInputMap() { return inputMap; } + QList getOutputMap() { return outputMap; } + bool isToRX() { return sendToRX; } + bool isFromTX() { return takeFromTX; } + QList getVideoModes() { return videoModes; } + +private: + QHash channels; + QSettings *settings; + QString sendToHost; + quint16 sendToPort; + QString listenOnHost; + quint16 listenOnPort; + QList inputMap; + QList outputMap; + bool sendToRX; + bool takeFromTX; + QList videoModes; +}; + +#endif // SETTINGS_H diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udpconnect.cpp b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udpconnect.cpp new file mode 100644 index 000000000..0448a705a --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udpconnect.cpp @@ -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 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 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; +} diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udpconnect.h b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udpconnect.h new file mode 100644 index 000000000..ead07f046 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udpconnect.h @@ -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 +#include +#include +#include +#include +#include +#include "aerosimrcdatastruct.h" + +class UdpSender : public QObject +{ +// Q_OBJECT +public: + explicit UdpSender(const QList 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 channels; + QList channelsMap; + bool takeFromTX; + quint32 packetsSended; +}; + + +class UdpReceiver : public QThread +{ +// Q_OBJECT +public: + explicit UdpReceiver(const QList 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 channels; + QList channelsMap; + bool sendToRX; + quint8 armed; + quint8 mode; + quint32 packetsRecived; + void onReadyRead(); + void processDatagram(QByteArray &datagram); +}; + +#endif // UDPCONNECT_H diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udptest.pro b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udptest.pro new file mode 100644 index 000000000..ac63be5ae --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udptest.pro @@ -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 diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udptestmain.cpp b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udptestmain.cpp new file mode 100644 index 000000000..744e4bf25 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udptestmain.cpp @@ -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 +#include "udptestwidget.h" + +int main(int argc, char *argv[]) +{ + QApplication a(argc, argv); + Widget w; + w.show(); + + return a.exec(); +} diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udptestwidget.cpp b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udptestwidget.cpp new file mode 100644 index 000000000..9ab45e45a --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udptestwidget.cpp @@ -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); +} +*/ diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udptestwidget.h b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udptestwidget.h new file mode 100644 index 000000000..eeca63555 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udptestwidget.h @@ -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 +#include +#include +#if defined(Q_CC_MSVC) +#define _USE_MATH_DEFINES +#endif +#include +#include +#include +#include +#include + +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 diff --git a/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udptestwidget.ui b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udptestwidget.ui new file mode 100644 index 000000000..a060a6042 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/aerosimrc/src/udptestwidget.ui @@ -0,0 +1,940 @@ + + + Widget + + + + 0 + 0 + 440 + 525 + + + + udp_test + + + + + + + + + + + + 60 + 0 + + + + Connect + + + + + + + true + + + + 0 + 0 + + + + + 50 + 0 + + + + 40100 + + + 5 + + + + + + + true + + + + 0 + 0 + + + + 127.0.0.1 + + + + + + + sim host + + + + + + + local host: + + + + + + + local port: + + + + + + + false + + + + 60 + 0 + + + + Disconnect + + + + + + + true + + + + 0 + 0 + + + + 127.0.0.1 + + + + + + + sim port + + + + + + + + 0 + 0 + + + + + 50 + 0 + + + + 40200 + + + 5 + + + + + + + + 60 + 0 + + + + Transmit + + + + + + + false + + + + 60 + 0 + + + + Stop + + + + + + + + + 0 + + + + raw data + + + + + + + Terminal + 10 + + + + + + + + + channels + + + + 0 + + + 9 + + + + + send data + + + + 6 + + + 9 + + + 0 + + + 3 + + + + + true + + + answer on recieve + + + true + + + + + + + true + + + auto send + + + false + + + + + + + + + + Flight mode + + + + 6 + + + 9 + + + 0 + + + 3 + + + + + 6 + + + 1 + + + Qt::Horizontal + + + QSlider::TicksAbove + + + 1 + + + + + + + + + + Armed state + + + + 6 + + + 9 + + + 0 + + + 3 + + + + + Disarmed + + + true + + + + + + + Arming + + + + + + + Armed + + + + + + + + + + Channels + + + + 9 + + + 0 + + + 9 + + + 3 + + + 3 + + + + + CH1 + + + Qt::AlignCenter + + + + + + + -511 + + + 512 + + + 16 + + + 32 + + + Qt::Horizontal + + + QSlider::TicksAbove + + + 128 + + + + + + + + 0 + 0 + + + + + 30 + 0 + + + + 0 + + + + + + + CH2 + + + Qt::AlignCenter + + + + + + + -511 + + + 512 + + + 16 + + + 32 + + + Qt::Horizontal + + + QSlider::TicksAbove + + + 128 + + + + + + + + 0 + 0 + + + + + 30 + 0 + + + + 0 + + + + + + + CH3 + + + Qt::AlignCenter + + + + + + + -511 + + + 512 + + + 16 + + + 32 + + + Qt::Horizontal + + + QSlider::TicksAbove + + + 128 + + + + + + + + 0 + 0 + + + + + 30 + 0 + + + + 0 + + + + + + + CH4 + + + Qt::AlignCenter + + + + + + + -511 + + + 512 + + + 16 + + + 32 + + + Qt::Horizontal + + + QSlider::TicksAbove + + + 128 + + + + + + + + 0 + 0 + + + + + 30 + 0 + + + + 0 + + + + + + + CH5 + + + Qt::AlignCenter + + + + + + + -511 + + + 512 + + + 16 + + + 32 + + + Qt::Horizontal + + + QSlider::TicksAbove + + + 128 + + + + + + + + 0 + 0 + + + + + 30 + 0 + + + + 0 + + + + + + + CH6 + + + Qt::AlignCenter + + + + + + + + 0 + 0 + + + + + 30 + 0 + + + + 0 + + + + + + + Ch7 + + + Qt::AlignCenter + + + + + + + + 0 + 0 + + + + + 30 + 0 + + + + 0 + + + + + + + Ch8 + + + Qt::AlignCenter + + + + + + + + 0 + 0 + + + + + 30 + 0 + + + + 0 + + + + + + + Ch9 + + + Qt::AlignCenter + + + + + + + -511 + + + 512 + + + 16 + + + 32 + + + Qt::Horizontal + + + QSlider::TicksAbove + + + 128 + + + + + + + + 0 + 0 + + + + + 30 + 0 + + + + 0 + + + + + + + Ch10 + + + Qt::AlignCenter + + + + + + + -511 + + + 512 + + + 16 + + + 32 + + + Qt::Horizontal + + + QSlider::TicksAbove + + + 128 + + + + + + + + 0 + 0 + + + + + 30 + 0 + + + + 0 + + + + + + + -511 + + + 512 + + + 16 + + + 32 + + + Qt::Horizontal + + + QSlider::TicksAbove + + + 128 + + + + + + + -511 + + + 512 + + + 16 + + + 32 + + + Qt::Horizontal + + + QSlider::TicksAbove + + + 128 + + + + + + + -511 + + + 512 + + + 16 + + + 32 + + + Qt::Horizontal + + + QSlider::TicksAbove + + + 128 + + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + + + + + + + diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2.pro b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2.pro index 58475b05f..132fbe8ee 100644 --- a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2.pro +++ b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2.pro @@ -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 diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2configuration.cpp b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2configuration.cpp index 890742e41..5977e5220 100644 --- a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2configuration.cpp +++ b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2configuration.cpp @@ -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 diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2configuration.h b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2configuration.h index dfac80ddf..3e6a96df6 100644 --- a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2configuration.h +++ b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2configuration.h @@ -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 diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2factory.cpp b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2factory.cpp index 6e6f1a42e..c9e52ebb4 100644 --- a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2factory.cpp +++ b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2factory.cpp @@ -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" diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2factory.h b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2factory.h index b4142eeed..cfa91750d 100644 --- a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2factory.h +++ b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2factory.h @@ -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 diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2gadget.cpp b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2gadget.cpp index de22d121b..b114cbde5 100644 --- a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2gadget.cpp +++ b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2gadget.cpp @@ -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" diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2gadget.h b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2gadget.h index 0e4fd1a80..45302abb8 100644 --- a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2gadget.h +++ b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2gadget.h @@ -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 diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.cpp b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.cpp index 5b88785ad..280ccf030 100644 --- a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.cpp +++ b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.cpp @@ -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 diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.h b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.h index 5c8d33f59..dbfe028bc 100644 --- a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.h +++ b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2optionspage.h @@ -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 diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2plugin.cpp b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2plugin.cpp index f0ee2d07a..c63ba5c86 100644 --- a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2plugin.cpp +++ b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2plugin.cpp @@ -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 diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2plugin.h b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2plugin.h index e68129bd4..3e83915b3 100644 --- a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2plugin.h +++ b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2plugin.h @@ -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 diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2widget.cpp b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2widget.cpp index 55d14f735..8933d7748 100644 --- a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2widget.cpp +++ b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2widget.cpp @@ -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 diff --git a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2widget.h b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2widget.h index 78f3e9405..6cf1c66c7 100644 --- a/ground/openpilotgcs/src/plugins/hitlv2/hitlv2widget.h +++ b/ground/openpilotgcs/src/plugins/hitlv2/hitlv2widget.h @@ -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 diff --git a/ground/openpilotgcs/src/plugins/hitlv2/plugin.pro b/ground/openpilotgcs/src/plugins/hitlv2/plugin.pro new file mode 100644 index 000000000..2d375e426 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/hitlv2/plugin.pro @@ -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 diff --git a/ground/openpilotgcs/src/plugins/hitlv2/simulatorv2.cpp b/ground/openpilotgcs/src/plugins/hitlv2/simulatorv2.cpp index c4c698689..bc5e0b0b1 100644 --- a/ground/openpilotgcs/src/plugins/hitlv2/simulatorv2.cpp +++ b/ground/openpilotgcs/src/plugins/hitlv2/simulatorv2.cpp @@ -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 diff --git a/ground/openpilotgcs/src/plugins/hitlv2/simulatorv2.h b/ground/openpilotgcs/src/plugins/hitlv2/simulatorv2.h index 6a0a9f008..445358a22 100644 --- a/ground/openpilotgcs/src/plugins/hitlv2/simulatorv2.h +++ b/ground/openpilotgcs/src/plugins/hitlv2/simulatorv2.h @@ -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 diff --git a/ground/openpilotgcs/src/plugins/rawhid/rawhid.cpp b/ground/openpilotgcs/src/plugins/rawhid/rawhid.cpp index ce9c0c125..4649d6162 100644 --- a/ground/openpilotgcs/src/plugins/rawhid/rawhid.cpp +++ b/ground/openpilotgcs/src/plugins/rawhid/rawhid.cpp @@ -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; diff --git a/ground/openpilotgcs/src/plugins/systemhealth/html/AlarmOK.html b/ground/openpilotgcs/src/plugins/systemhealth/html/AlarmOK.html new file mode 100644 index 000000000..fee48402f --- /dev/null +++ b/ground/openpilotgcs/src/plugins/systemhealth/html/AlarmOK.html @@ -0,0 +1,13 @@ + + + + + + + +

Alarm: OK

+

+ There are no problems with this alarm. +

+ + diff --git a/ground/openpilotgcs/src/plugins/systemhealth/systemhealth.qrc b/ground/openpilotgcs/src/plugins/systemhealth/systemhealth.qrc index 30256626f..7ef754ecc 100644 --- a/ground/openpilotgcs/src/plugins/systemhealth/systemhealth.qrc +++ b/ground/openpilotgcs/src/plugins/systemhealth/systemhealth.qrc @@ -3,5 +3,28 @@ html/Actuator-Critical.html html/ManualControl-Critical.html html/ManualControl-Warning.html + html/CPU-Critical.html + html/CPU-Warning.html + html/FlightTime-Error.html + html/Battery-Warning.html + html/BootFault-Critical.html + html/EventSystem-Warning.html + html/FlightTime-Critical.html + html/AlarmOK.html + html/Attitude-Critical.html + html/Attitude-Error.html + html/Battery-Critical.html + html/Battery-Error.html + html/FlightTime-Warning.html + html/GPS-Critical.html + html/GPS-Error.html + html/GPS-Warning.html + html/Guidance-Warning.html + html/Memory-Critical.html + html/Memory-Warning.html + html/Sensors-Critical.html + html/Stabilization-Warning.html + html/Stack-Critical.html + html/Telemetry-Error.html diff --git a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetwidget.cpp index ebc14dcb3..68d19c532 100644 --- a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetwidget.cpp @@ -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(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(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); + } + } +} diff --git a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetwidget.h b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetwidget.h index a7e3e9629..15ab31952 100644 --- a/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/systemhealth/systemhealthgadgetwidget.h @@ -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_ */ diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/treeitem.cpp b/ground/openpilotgcs/src/plugins/uavobjectbrowser/treeitem.cpp index a6924e71d..debe3f81d 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/treeitem.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/treeitem.cpp @@ -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 TopTreeItem::getMetaObjectItems() +{ + return m_metaObjectTreeItemsPerObjectIds.values(); +} diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/treeitem.h b/ground/openpilotgcs/src/plugins/uavobjectbrowser/treeitem.h index 708513d91..f30368e25 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/treeitem.h +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/treeitem.h @@ -33,6 +33,7 @@ #include "uavobjectfield.h" #include #include +#include #include #include #include @@ -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 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 &data, TreeItem *parent = 0) : TreeItem(data, parent) { } TopTreeItem(const QVariant &data, TreeItem *parent = 0) : TreeItem(data, parent) { } - QList 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 getMetaObjectItems(); + private: - QList m_objIds; + QMap m_objectTreeItemsPerObjectIds; + QMap m_metaObjectTreeItemsPerObjectIds; }; class ObjectTreeItem : public TreeItem diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowser.cpp b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowser.cpp index cf343ec5c..46142340c 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowser.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowser.cpp @@ -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()); } diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowser.ui b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowser.ui index 091c0c5eb..0d36859fd 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowser.ui +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowser.ui @@ -202,6 +202,19 @@ + + + + Select to sort objects in to categories. + + + Categorize Objects + + + true + + + diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserconfiguration.cpp b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserconfiguration.cpp index e534542c6..cce3bee89 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserconfiguration.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserconfiguration.cpp @@ -31,17 +31,20 @@ UAVObjectBrowserConfiguration::UAVObjectBrowserConfiguration(QString classId, QS IUAVGadgetConfiguration(classId, parent), m_recentlyUpdatedColor(QColor(255, 230, 230)), m_manuallyChangedColor(QColor(230, 230, 255)), - m_recentlyUpdatedTimeout(500) + m_recentlyUpdatedTimeout(500), + m_onlyHilightChangedValues(false) { //if a saved configuration exists load it if(qSettings != 0) { QColor recent = qSettings->value("recentlyUpdatedColor").value(); QColor manual = qSettings->value("manuallyChangedColor").value(); int timeout = qSettings->value("recentlyUpdatedTimeout").toInt(); + bool hilight = qSettings->value("onlyHilightChangedValues").toBool(); m_recentlyUpdatedColor = recent; m_manuallyChangedColor = manual; m_recentlyUpdatedTimeout = timeout; + m_onlyHilightChangedValues = hilight; } } @@ -51,6 +54,7 @@ IUAVGadgetConfiguration *UAVObjectBrowserConfiguration::clone() m->m_recentlyUpdatedColor = m_recentlyUpdatedColor; m->m_manuallyChangedColor = m_manuallyChangedColor; m->m_recentlyUpdatedTimeout = m_recentlyUpdatedTimeout; + m->m_onlyHilightChangedValues = m_onlyHilightChangedValues; return m; } @@ -62,4 +66,5 @@ void UAVObjectBrowserConfiguration::saveConfig(QSettings* qSettings) const { qSettings->setValue("recentlyUpdatedColor", m_recentlyUpdatedColor); qSettings->setValue("manuallyChangedColor", m_manuallyChangedColor); qSettings->setValue("recentlyUpdatedTimeout", m_recentlyUpdatedTimeout); + qSettings->setValue("onlyHilightChangedValues", m_onlyHilightChangedValues); } diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserconfiguration.h b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserconfiguration.h index 941428f83..8e31c18dc 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserconfiguration.h +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserconfiguration.h @@ -39,6 +39,7 @@ Q_OBJECT Q_PROPERTY(QColor m_recentlyUpdatedColor READ recentlyUpdatedColor WRITE setRecentlyUpdatedColor) Q_PROPERTY(QColor m_manuallyChangedColor READ manuallyChangedColor WRITE setManuallyChangedColor) Q_PROPERTY(int m_recentlyUpdatedTimeout READ recentlyUpdatedTimeout WRITE setRecentlyUpdatedTimeout) +Q_PROPERTY(bool m_onlyHilightChangedValues READ onlyHighlightChangedValues WRITE setOnlyHighlightChangedValues) public: explicit UAVObjectBrowserConfiguration(QString classId, QSettings* qSettings = 0, QObject *parent = 0); @@ -48,6 +49,7 @@ public: QColor recentlyUpdatedColor() const { return m_recentlyUpdatedColor; } QColor manuallyChangedColor() const { return m_manuallyChangedColor; } int recentlyUpdatedTimeout() const { return m_recentlyUpdatedTimeout; } + bool onlyHighlightChangedValues() const {return m_onlyHilightChangedValues;} signals: @@ -55,11 +57,13 @@ public slots: void setRecentlyUpdatedColor(QColor color) { m_recentlyUpdatedColor = color; } void setManuallyChangedColor(QColor color) { m_manuallyChangedColor = color; } void setRecentlyUpdatedTimeout(int timeout) { m_recentlyUpdatedTimeout = timeout; } + void setOnlyHighlightChangedValues(bool hilight) { m_onlyHilightChangedValues = hilight; } private: QColor m_recentlyUpdatedColor; QColor m_manuallyChangedColor; int m_recentlyUpdatedTimeout; + bool m_onlyHilightChangedValues; }; #endif // UAVOBJECTBROWSERCONFIGURATION_H diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowseroptionspage.cpp b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowseroptionspage.cpp index fe0a13290..b9561e471 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowseroptionspage.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowseroptionspage.cpp @@ -52,6 +52,7 @@ QWidget *UAVObjectBrowserOptionsPage::createPage(QWidget *parent) m_page->recentlyUpdatedButton->setColor(m_config->recentlyUpdatedColor()); m_page->manuallyChangedButton->setColor(m_config->manuallyChangedColor()); m_page->recentlyUpdatedTimeoutSpinBox->setValue(m_config->recentlyUpdatedTimeout()); + m_page->hilightBox->setChecked(m_config->onlyHighlightChangedValues()); return w; @@ -62,6 +63,7 @@ void UAVObjectBrowserOptionsPage::apply() m_config->setRecentlyUpdatedColor(m_page->recentlyUpdatedButton->color()); m_config->setManuallyChangedColor(m_page->manuallyChangedButton->color()); m_config->setRecentlyUpdatedTimeout(m_page->recentlyUpdatedTimeoutSpinBox->value()); + m_config->setOnlyHighlightChangedValues(m_page->hilightBox->isChecked()); } void UAVObjectBrowserOptionsPage::finish() diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowseroptionspage.ui b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowseroptionspage.ui index 474e26dc6..350fc440b 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowseroptionspage.ui +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowseroptionspage.ui @@ -48,7 +48,7 @@ - + Qt::Vertical @@ -115,6 +115,13 @@ + + + + Only hilight nodes when value actually changes + + + diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserwidget.cpp b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserwidget.cpp index 31c395ccc..2ce386799 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserwidget.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserwidget.cpp @@ -54,6 +54,7 @@ UAVObjectBrowserWidget::UAVObjectBrowserWidget(QWidget *parent) : QWidget(parent showMetaData(m_browser->metaCheckBox->isChecked()); connect(m_browser->treeView->selectionModel(), SIGNAL(currentChanged(QModelIndex,QModelIndex)), this, SLOT(currentChanged(QModelIndex,QModelIndex))); connect(m_browser->metaCheckBox, SIGNAL(toggled(bool)), this, SLOT(showMetaData(bool))); + connect(m_browser->categorizeCheckbox, SIGNAL(toggled(bool)), this, SLOT(categorize(bool))); connect(m_browser->saveSDButton, SIGNAL(clicked()), this, SLOT(saveObject())); connect(m_browser->readSDButton, SIGNAL(clicked()), this, SLOT(loadObject())); connect(m_browser->eraseSDButton, SIGNAL(clicked()), this, SLOT(eraseObject())); @@ -69,16 +70,32 @@ UAVObjectBrowserWidget::~UAVObjectBrowserWidget() void UAVObjectBrowserWidget::showMetaData(bool show) { - int topRowCount = m_model->rowCount(QModelIndex()); - for (int i = 0; i < topRowCount; ++i) { - QModelIndex index = m_model->index(i, 0, QModelIndex()); - int subRowCount = m_model->rowCount(index); - for (int j = 0; j < subRowCount; ++j) { - m_browser->treeView->setRowHidden(0, index.child(j,0), !show); - } + QList metaIndexes = m_model->getMetaDataIndexes(); + foreach(QModelIndex index , metaIndexes) + { + m_browser->treeView->setRowHidden(index.row(), index.parent(), !show); } } +void UAVObjectBrowserWidget::categorize(bool categorize) +{ + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + Q_ASSERT(pm); + UAVObjectManager *objManager = pm->getObject(); + Q_ASSERT(objManager); + + UAVObjectTreeModel* tmpModel = m_model; + m_model = new UAVObjectTreeModel(0, categorize); + m_model->setRecentlyUpdatedColor(m_recentlyUpdatedColor); + m_model->setManuallyChangedColor(m_manuallyChangedColor); + m_model->setRecentlyUpdatedTimeout(m_recentlyUpdatedTimeout); + m_model->setOnlyHilightChangedValues(m_onlyHilightChangedValues); + m_browser->treeView->setModel(m_model); + showMetaData(m_browser->metaCheckBox->isChecked()); + + delete tmpModel; +} + void UAVObjectBrowserWidget::sendUpdate() { ObjectTreeItem *objItem = findCurrentObjectTreeItem(); diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserwidget.h b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserwidget.h index b047d37f0..d0c3b7700 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserwidget.h +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjectbrowserwidget.h @@ -45,12 +45,15 @@ class UAVObjectBrowserWidget : public QWidget public: UAVObjectBrowserWidget(QWidget *parent = 0); ~UAVObjectBrowserWidget(); - void setRecentlyUpdatedColor(QColor color) { m_model->setRecentlyUpdatedColor(color); } - void setManuallyChangedColor(QColor color) { m_model->setManuallyChangedColor(color); } - void setRecentlyUpdatedTimeout(int timeout) { m_model->setRecentlyUpdatedTimeout(timeout); } + void setRecentlyUpdatedColor(QColor color) { m_recentlyUpdatedColor = color; m_model->setRecentlyUpdatedColor(color); } + void setManuallyChangedColor(QColor color) { m_manuallyChangedColor = color; m_model->setManuallyChangedColor(color); } + void setRecentlyUpdatedTimeout(int timeout) { m_recentlyUpdatedTimeout = timeout; m_model->setRecentlyUpdatedTimeout(timeout); } + void setOnlyHilightChangedValues(bool hilight) { m_onlyHilightChangedValues = hilight; m_model->setOnlyHilightChangedValues(hilight); } + public slots: void showMetaData(bool show); + void categorize(bool categorize); private slots: void sendUpdate(); @@ -66,6 +69,11 @@ private: Ui_UAVObjectBrowser *m_browser; UAVObjectTreeModel *m_model; + int m_recentlyUpdatedTimeout; + QColor m_recentlyUpdatedColor; + QColor m_manuallyChangedColor; + bool m_onlyHilightChangedValues; + void updateObjectPersistance(ObjectPersistence::OperationOptions op, UAVObject *obj); void enableSendRequest(bool enable); ObjectTreeItem *findCurrentObjectTreeItem(); diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjecttreemodel.cpp b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjecttreemodel.cpp index 120d9d0d8..3025e8dc4 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjecttreemodel.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjecttreemodel.cpp @@ -38,7 +38,7 @@ #include #include -UAVObjectTreeModel::UAVObjectTreeModel(QObject *parent) : +UAVObjectTreeModel::UAVObjectTreeModel(QObject *parent, bool categorize) : QAbstractItemModel(parent), m_recentlyUpdatedTimeout(500), // ms m_recentlyUpdatedColor(QColor(255, 230, 230)), @@ -53,7 +53,7 @@ UAVObjectTreeModel::UAVObjectTreeModel(QObject *parent) : connect(objManager, SIGNAL(newInstance(UAVObject*)), this, SLOT(newObject(UAVObject*))); TreeItem::setHighlightTime(m_recentlyUpdatedTimeout); - setupModelData(objManager); + setupModelData(objManager, categorize); } UAVObjectTreeModel::~UAVObjectTreeModel() @@ -62,7 +62,7 @@ UAVObjectTreeModel::~UAVObjectTreeModel() delete m_rootItem; } -void UAVObjectTreeModel::setupModelData(UAVObjectManager *objManager) +void UAVObjectTreeModel::setupModelData(UAVObjectManager *objManager, bool categorize) { // root QList rootData; @@ -82,7 +82,7 @@ void UAVObjectTreeModel::setupModelData(UAVObjectManager *objManager) QList< QList > objList = objManager->getDataObjects(); foreach (QList list, objList) { foreach (UAVDataObject* obj, list) { - addDataObject(obj); + addDataObject(obj, categorize); } } } @@ -90,33 +90,62 @@ void UAVObjectTreeModel::setupModelData(UAVObjectManager *objManager) void UAVObjectTreeModel::newObject(UAVObject *obj) { UAVDataObject *dobj = qobject_cast(obj); - if (dobj) + if (dobj) { addDataObject(dobj); -} - -void UAVObjectTreeModel::addDataObject(UAVDataObject *obj) -{ - TopTreeItem *root = obj->isSettings() ? m_settingsTree : m_nonSettingsTree; - if (root->objIds().contains(obj->getObjID())) { - int index = root->objIds().indexOf(obj->getObjID()); - addInstance(obj, root->child(index)); - } else { - DataObjectTreeItem *data = new DataObjectTreeItem(obj->getName() + " (" + QString::number(obj->getNumBytes()) + " bytes)"); - data->setHighlightManager(m_highlightManager); - connect(data, SIGNAL(updateHighlight(TreeItem*)), this, SLOT(updateHighlight(TreeItem*))); - int index = root->nameIndex(obj->getName()); - root->insert(index, data); - root->insertObjId(index, obj->getObjID()); - UAVMetaObject *meta = obj->getMetaObject(); - addMetaObject(meta, data); - addInstance(obj, data); } } -void UAVObjectTreeModel::addMetaObject(UAVMetaObject *obj, TreeItem *parent) +void UAVObjectTreeModel::addDataObject(UAVDataObject *obj, bool categorize) +{ + TopTreeItem *root = obj->isSettings() ? m_settingsTree : m_nonSettingsTree; + + TreeItem* parent = root; + + if(categorize && obj->getCategory() != 0 && !obj->getCategory().isEmpty()) { + QStringList categoryPath = obj->getCategory().split('/'); + parent = createCategoryItems(categoryPath, root); + } + + ObjectTreeItem* existing = root->findDataObjectTreeItemByObjectId(obj->getObjID()); + if (existing) { + addInstance(obj, existing); + } else { + DataObjectTreeItem *dataTreeItem = new DataObjectTreeItem(obj->getName() + " (" + QString::number(obj->getNumBytes()) + " bytes)"); + dataTreeItem->setHighlightManager(m_highlightManager); + connect(dataTreeItem, SIGNAL(updateHighlight(TreeItem*)), this, SLOT(updateHighlight(TreeItem*))); + parent->insertChild(dataTreeItem); + root->addObjectTreeItem(obj->getObjID(), dataTreeItem); + UAVMetaObject *meta = obj->getMetaObject(); + MetaObjectTreeItem* metaTreeItem = addMetaObject(meta, dataTreeItem); + root->addMetaObjectTreeItem(meta->getObjID(), metaTreeItem); + addInstance(obj, dataTreeItem); + } +} + +TreeItem* UAVObjectTreeModel::createCategoryItems(QStringList categoryPath, TreeItem* root) +{ + TreeItem* parent = root; + foreach(QString category, categoryPath) { + TreeItem* existing = parent->findChildByName(category); + if(!existing) { + TreeItem* categoryItem = new TreeItem(category); + connect(categoryItem, SIGNAL(updateHighlight(TreeItem*)), this, SLOT(updateHighlight(TreeItem*))); + categoryItem->setHighlightManager(m_highlightManager); + parent->insertChild(categoryItem); + parent = categoryItem; + } + else { + parent = existing; + } + } + return parent; +} + +MetaObjectTreeItem* UAVObjectTreeModel::addMetaObject(UAVMetaObject *obj, TreeItem *parent) { connect(obj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(highlightUpdatedObject(UAVObject*))); MetaObjectTreeItem *meta = new MetaObjectTreeItem(obj, tr("Meta Data")); + meta->setHighlightManager(m_highlightManager); connect(meta, SIGNAL(updateHighlight(TreeItem*)), this, SLOT(updateHighlight(TreeItem*))); foreach (UAVObjectField *field, obj->getFields()) { @@ -127,6 +156,7 @@ void UAVObjectTreeModel::addMetaObject(UAVMetaObject *obj, TreeItem *parent) } } parent->appendChild(meta); + return meta; } void UAVObjectTreeModel::addInstance(UAVObject *obj, TreeItem *parent) @@ -153,7 +183,6 @@ void UAVObjectTreeModel::addInstance(UAVObject *obj, TreeItem *parent) } } - void UAVObjectTreeModel::addArrayField(UAVObjectField *field, TreeItem *parent) { TreeItem *item = new ArrayFieldTreeItem(field->getName()); @@ -221,7 +250,7 @@ QModelIndex UAVObjectTreeModel::index(int row, int column, const QModelIndex &pa else parentItem = static_cast(parent.internalPointer()); - TreeItem *childItem = parentItem->child(row); + TreeItem *childItem = parentItem->getChild(row); if (childItem) return createIndex(row, column, childItem); else @@ -281,15 +310,31 @@ int UAVObjectTreeModel::columnCount(const QModelIndex &parent) const return m_rootItem->columnCount(); } +QList UAVObjectTreeModel::getMetaDataIndexes() +{ + QList metaIndexes; + foreach(MetaObjectTreeItem *metaItem , m_settingsTree->getMetaObjectItems()) + { + metaIndexes.append(index(metaItem)); + } + + foreach(MetaObjectTreeItem *metaItem , m_nonSettingsTree->getMetaObjectItems()) + { + metaIndexes.append(index(metaItem)); + } + return metaIndexes; +} + QVariant UAVObjectTreeModel::data(const QModelIndex &index, int role) const { if (!index.isValid()) - return QVariant(); + return QVariant(); if (index.column() == TreeItem::dataColumn && role == Qt::EditRole) { TreeItem *item = static_cast(index.internalPointer()); return item->data(index.column()); } + // if (role == Qt::DecorationRole) // return QIcon(":/core/images/openpilot_logo_128.png"); @@ -301,16 +346,17 @@ QVariant UAVObjectTreeModel::data(const QModelIndex &index, int role) const TreeItem *item = static_cast(index.internalPointer()); if (index.column() == 0 && role == Qt::BackgroundRole) { - ObjectTreeItem *objItem = dynamic_cast(item); - if (objItem && objItem->highlighted()) + if (!dynamic_cast(item) && item->highlighted()) return QVariant(m_recentlyUpdatedColor); } + if (index.column() == TreeItem::dataColumn && role == Qt::BackgroundRole) { FieldTreeItem *fieldItem = dynamic_cast(item); if (fieldItem && fieldItem->highlighted()) return QVariant(m_recentlyUpdatedColor); + if (fieldItem && fieldItem->changed()) - return QVariant(m_manuallyChangedColor); + return QVariant(m_manuallyChangedColor); } if (role != Qt::DisplayRole) @@ -364,58 +410,42 @@ void UAVObjectTreeModel::highlightUpdatedObject(UAVObject *obj) Q_ASSERT(obj); ObjectTreeItem *item = findObjectTreeItem(obj); Q_ASSERT(item); + if(!m_onlyHilightChangedValues){ + item->setHighlight(true); + } item->update(); - QModelIndex itemIndex = index(item); - Q_ASSERT(itemIndex != QModelIndex()); - emit dataChanged(itemIndex, itemIndex); + if(!m_onlyHilightChangedValues){ + QModelIndex itemIndex = index(item); + Q_ASSERT(itemIndex != QModelIndex()); + emit dataChanged(itemIndex, itemIndex); + } } ObjectTreeItem *UAVObjectTreeModel::findObjectTreeItem(UAVObject *object) { - UAVDataObject *dobj = qobject_cast(object); - UAVMetaObject *mobj = qobject_cast(object); - Q_ASSERT(dobj || mobj); - if (dobj) { - return findDataObjectTreeItem(dobj); + UAVDataObject *dataObject = qobject_cast(object); + UAVMetaObject *metaObject = qobject_cast(object); + Q_ASSERT(dataObject || metaObject); + if (dataObject) { + return findDataObjectTreeItem(dataObject); } else { - dobj = qobject_cast(mobj->getParentObject()); - Q_ASSERT(dobj); - ObjectTreeItem *dItem = findDataObjectTreeItem(dobj); - Q_ASSERT(dItem); - Q_ASSERT(dItem->object()); - if (!dItem->object()->isSingleInstance()) - dItem = dynamic_cast(dItem->parent()); - foreach (TreeItem *child, dItem->treeChildren()) { - MetaObjectTreeItem *mItem = dynamic_cast(child); - if (mItem && mItem->object()) { - Q_ASSERT(mItem->object() == mobj); - return mItem; - } - } + return findMetaObjectTreeItem(metaObject); } - return 0; } -DataObjectTreeItem *UAVObjectTreeModel::findDataObjectTreeItem(UAVDataObject *object) +DataObjectTreeItem* UAVObjectTreeModel::findDataObjectTreeItem(UAVDataObject *obj) { - Q_ASSERT(object); - TopTreeItem *root = object->isSettings() ? m_settingsTree : m_nonSettingsTree; - foreach (TreeItem *child, root->treeChildren()) { - DataObjectTreeItem *dItem = dynamic_cast(child); - if (dItem && dItem->object() && dItem->object()->isSingleInstance()) { - if(dItem->object() == object) { - return dItem; - } - } else { - foreach (TreeItem *c, dItem->treeChildren()) { - DataObjectTreeItem *d = dynamic_cast(c); - if (d && d->object() == object) - return d; - } - } - } - return 0; + TopTreeItem *root = obj->isSettings() ? m_settingsTree : m_nonSettingsTree; + return root->findDataObjectTreeItemByObjectId(obj->getObjID()); +} + +MetaObjectTreeItem* UAVObjectTreeModel::findMetaObjectTreeItem(UAVMetaObject *obj) +{ + UAVDataObject *dataObject = qobject_cast(obj->getParentObject()); + Q_ASSERT(dataObject); + TopTreeItem *root = dataObject->isSettings() ? m_settingsTree : m_nonSettingsTree; + return root->findMetaObjectTreeItemByObjectId(obj->getObjID()); } void UAVObjectTreeModel::updateHighlight(TreeItem *item) diff --git a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjecttreemodel.h b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjecttreemodel.h index d9787e3a7..8d1d5a7d7 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjecttreemodel.h +++ b/ground/openpilotgcs/src/plugins/uavobjectbrowser/uavobjecttreemodel.h @@ -31,6 +31,7 @@ #include "treeitem.h" #include #include +#include #include class TopTreeItem; @@ -48,7 +49,7 @@ class UAVObjectTreeModel : public QAbstractItemModel { Q_OBJECT public: - explicit UAVObjectTreeModel(QObject *parent = 0); + explicit UAVObjectTreeModel(QObject *parent = 0, bool categorize=true); ~UAVObjectTreeModel(); QVariant data(const QModelIndex &index, int role) const; @@ -68,6 +69,9 @@ public: m_recentlyUpdatedTimeout = timeout; TreeItem::setHighlightTime(timeout); } + void setOnlyHilightChangedValues(bool hilight) {m_onlyHilightChangedValues = hilight; } + + QList getMetaDataIndexes(); signals: @@ -79,17 +83,20 @@ private slots: void updateHighlight(TreeItem*); private: + void setupModelData(UAVObjectManager *objManager, bool categorize = true); QModelIndex index(TreeItem *item); - void addDataObject(UAVDataObject *obj); - void addMetaObject(UAVMetaObject *obj, TreeItem *parent); + void addDataObject(UAVDataObject *obj, bool categorize = true); + MetaObjectTreeItem *addMetaObject(UAVMetaObject *obj, TreeItem *parent); void addArrayField(UAVObjectField *field, TreeItem *parent); - void addSingleField(int index, UAVObjectField *field, TreeItem *parent); void addInstance(UAVObject *obj, TreeItem *parent); + + TreeItem *createCategoryItems(QStringList categoryPath, TreeItem *root); + QString updateMode(quint8 updateMode); - void setupModelData(UAVObjectManager *objManager); ObjectTreeItem *findObjectTreeItem(UAVObject *obj); DataObjectTreeItem *findDataObjectTreeItem(UAVDataObject *obj); + MetaObjectTreeItem *findMetaObjectTreeItem(UAVMetaObject *obj); TreeItem *m_rootItem; TopTreeItem *m_settingsTree; @@ -97,6 +104,7 @@ private: int m_recentlyUpdatedTimeout; QColor m_recentlyUpdatedColor; QColor m_manuallyChangedColor; + bool m_onlyHilightChangedValues; // Highlight manager to handle highlighting of tree items. HighLightManager *m_highlightManager; diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavmetaobject.cpp b/ground/openpilotgcs/src/plugins/uavobjects/uavmetaobject.cpp index e190b4f27..2805c9a1f 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavmetaobject.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavmetaobject.cpp @@ -31,7 +31,7 @@ /** * Constructor */ -UAVMetaObject::UAVMetaObject(quint32 objID, const QString& name, UAVObject* parent): +UAVMetaObject::UAVMetaObject(quint32 objID, const QString& name, UAVObject *parent): UAVObject(objID, true, name) { this->parent = parent; diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobject.cpp b/ground/openpilotgcs/src/plugins/uavobjects/uavobject.cpp index 47960b060..d01caba21 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobject.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobject.cpp @@ -145,6 +145,22 @@ void UAVObject::setDescription(const QString& description) this->description = description; } +/** + * Get the category of the object + */ +QString UAVObject::getCategory() +{ + return category; +} + +/** + * Set the category of the object + */ +void UAVObject::setCategory(const QString& category) +{ + this->category = category; +} + /** * Get the total number of bytes of the object's data diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobject.h b/ground/openpilotgcs/src/plugins/uavobjects/uavobject.h index 0962ea73d..d092c6731 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobject.h +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobject.h @@ -103,6 +103,7 @@ public: quint32 getInstID(); bool isSingleInstance(); QString getName(); + QString getCategory(); QString getDescription(); quint32 getNumBytes(); qint32 pack(quint8* dataOut); @@ -163,6 +164,7 @@ protected: bool isSingleInst; QString name; QString description; + QString category; quint32 numBytes; QMutex* mutex; quint8* data; @@ -170,6 +172,7 @@ protected: void initializeFields(QList& fields, quint8* data, quint32 numBytes); void setDescription(const QString& description); + void setCategory(const QString& category); }; #endif // UAVOBJECT_H diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index 943422207..db01e3153 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -56,6 +56,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/gpsposition.h \ $$UAVOBJECT_SYNTHETICS/gpstime.h \ $$UAVOBJECT_SYNTHETICS/gpssatellites.h \ + $$UAVOBJECT_SYNTHETICS/gpssettings.h \ $$UAVOBJECT_SYNTHETICS/pathaction.h \ $$UAVOBJECT_SYNTHETICS/pathdesired.h \ $$UAVOBJECT_SYNTHETICS/pathplannersettings.h \ @@ -130,6 +131,7 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/gpsposition.cpp \ $$UAVOBJECT_SYNTHETICS/gpstime.cpp \ $$UAVOBJECT_SYNTHETICS/gpssatellites.cpp \ + $$UAVOBJECT_SYNTHETICS/gpssettings.cpp \ $$UAVOBJECT_SYNTHETICS/pathaction.cpp \ $$UAVOBJECT_SYNTHETICS/pathdesired.cpp \ $$UAVOBJECT_SYNTHETICS/pathplannersettings.cpp \ diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjecttemplate.cpp b/ground/openpilotgcs/src/plugins/uavobjects/uavobjecttemplate.cpp index 58aa84ead..f08f5e434 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjecttemplate.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjecttemplate.cpp @@ -8,26 +8,26 @@ * @{ * @addtogroup UAVObjectsPlugin UAVObjects Plugin * @{ - * - * @note Object definition file: $(XMLFILE). + * + * @note Object definition file: $(XMLFILE). * This is an automatically generated file. * DO NOT modify manually. * * @brief The UAVUObjects GCS 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 +/* + * 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 + * + * 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., + * + * 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 "$(NAMELC).h" @@ -35,6 +35,7 @@ const QString $(NAME)::NAME = QString("$(NAME)"); const QString $(NAME)::DESCRIPTION = QString("$(DESCRIPTION)"); +const QString $(NAME)::CATEGORY = QString("$(CATEGORY)"); /** * Constructor @@ -51,6 +52,9 @@ $(FIELDSINIT) // Set the object description setDescription(DESCRIPTION); + // Set the Category of this object type + setCategory(CATEGORY); + connect(this, SIGNAL(objectUpdated(UAVObject*)), SLOT(emitNotifications())); } diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjecttemplate.h b/ground/openpilotgcs/src/plugins/uavobjects/uavobjecttemplate.h index 9b5a766ee..f68eebe49 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjecttemplate.h +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjecttemplate.h @@ -54,6 +54,7 @@ $(DATAFIELDINFO) static const quint32 OBJID = $(OBJIDHEX); static const QString NAME; static const QString DESCRIPTION; + static const QString CATEGORY; static const bool ISSINGLEINST = $(ISSINGLEINST); static const bool ISSETTINGS = $(ISSETTINGS); static const quint32 NUMBYTES = sizeof(DataFields); diff --git a/ground/openpilotgcs/src/plugins/uploader/op_dfu.cpp b/ground/openpilotgcs/src/plugins/uploader/op_dfu.cpp index 3320565c3..3e45a6733 100644 --- a/ground/openpilotgcs/src/plugins/uploader/op_dfu.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/op_dfu.cpp @@ -90,6 +90,8 @@ DFUObject::DFUObject(bool _debug,bool _use_serial,QString portname): if (devices.length()==1 && hidHandle.open(1,devices.first().vendorID,devices.first().productID,0,0)==1) { qDebug()<<"OP_DFU detected first time"; mready=true; + QTimer::singleShot(200,&m_eventloop, SLOT(quit())); + m_eventloop.exec(); } else { // Wait for the board to appear on the USB bus: USBSignalFilter filter(0x20a0,-1,-1,USBMonitor::Bootloader); @@ -107,6 +109,8 @@ DFUObject::DFUObject(bool _debug,bool _use_serial,QString portname): if (devices.length()==1) { if(hidHandle.open(1,devices.first().vendorID,devices.first().productID,0,0)==1) { + QTimer::singleShot(200,&m_eventloop, SLOT(quit())); + m_eventloop.exec(); qDebug()<<"OP_DFU detected after delay"; mready=true; break; diff --git a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp index c554a1709..4827ce4a7 100755 --- a/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/uploadergadgetwidget.cpp @@ -213,7 +213,8 @@ void UploaderGadgetWidget::goToBootloader(UAVObject* callerObj, bool success) m_config->haltButton->setEnabled(true); break; } - delay::msleep(600); + QTimer::singleShot(600, &m_eventloop, SLOT(quit())); + m_eventloop.exec(); fwIAP->getField("Command")->setValue("2233"); currentStep = IAP_STATE_STEP_2; log(QString("IAP Step 2")); @@ -228,7 +229,8 @@ void UploaderGadgetWidget::goToBootloader(UAVObject* callerObj, bool success) m_config->haltButton->setEnabled(true); break; } - delay::msleep(600); + QTimer::singleShot(600, &m_eventloop, SLOT(quit())); + m_eventloop.exec(); fwIAP->getField("Command")->setValue("3344"); currentStep = IAP_STEP_RESET; log(QString("IAP Step 3")); @@ -250,8 +252,12 @@ void UploaderGadgetWidget::goToBootloader(UAVObject* callerObj, bool success) QString dli = cm->getCurrentDevice().Name; QString dlj = cm->getCurrentDevice().devName; cm->disconnectDevice(); + QTimer::singleShot(200, &m_eventloop, SLOT(quit())); + m_eventloop.exec(); // Tell connections to stop their polling threads: otherwise it will mess up DFU cm->suspendPolling(); + QTimer::singleShot(200, &m_eventloop, SLOT(quit())); + m_eventloop.exec(); log("Board Halt"); m_config->boardStatus->setText("Bootloader"); if (dlj.startsWith("USB")) diff --git a/ground/uavobjgenerator/generators/generator_common.cpp b/ground/uavobjgenerator/generators/generator_common.cpp index 7814a9c05..12fae596d 100644 --- a/ground/uavobjgenerator/generators/generator_common.cpp +++ b/ground/uavobjgenerator/generators/generator_common.cpp @@ -56,6 +56,8 @@ void replaceCommonTags(QString& out, ObjectInfo* info) out.replace(QString("$(NAMELC)"), info->namelc); // Replace $(DESCRIPTION) tag out.replace(QString("$(DESCRIPTION)"), info->description); + // Replace $(CATEGORY) tag + out.replace(QString("$(CATEGORY)"), info->category); // Replace $(NAMEUC) tag out.replace(QString("$(NAMEUC)"), info->name.toUpper()); // Replace $(OBJID) tag diff --git a/ground/uavobjgenerator/uavobjectparser.cpp b/ground/uavobjgenerator/uavobjectparser.cpp index 94fce6043..73c4833b0 100644 --- a/ground/uavobjgenerator/uavobjectparser.cpp +++ b/ground/uavobjgenerator/uavobjectparser.cpp @@ -499,6 +499,13 @@ QString UAVObjectParser::processObjectAttributes(QDomNode& node, ObjectInfo* inf info->name = attr.nodeValue(); info->namelc = attr.nodeValue().toLower(); + // Get category attribute if present + attr = attributes.namedItem("category"); + if ( !attr.isNull() ) + { + info->category = attr.nodeValue(); + } + // Get singleinstance attribute attr = attributes.namedItem("singleinstance"); if ( attr.isNull() ) diff --git a/ground/uavobjgenerator/uavobjectparser.h b/ground/uavobjgenerator/uavobjectparser.h index 66b52ff88..cad891d3c 100644 --- a/ground/uavobjgenerator/uavobjectparser.h +++ b/ground/uavobjgenerator/uavobjectparser.h @@ -96,6 +96,7 @@ typedef struct { int loggingUpdatePeriod; /** Update period used by the logging module (only if logging mode is PERIODIC) */ QList fields; /** The data fields for the object **/ QString description; /** Description used for Doxygen **/ + QString category; /** Description used for Doxygen **/ } ObjectInfo; class UAVObjectParser @@ -127,6 +128,7 @@ private: QString processObjectFields(QDomNode& childNode, ObjectInfo* info); QString processObjectAccess(QDomNode& childNode, ObjectInfo* info); QString processObjectDescription(QDomNode& childNode, QString * description); + QString processObjectCategory(QDomNode& childNode, QString * category); QString processObjectMetadata(QDomNode& childNode, UpdateMode* mode, int* period, bool* acked); void calculateID(ObjectInfo* info); quint32 updateHash(quint32 value, quint32 hash); diff --git a/package/winx86/openpilotgcs.nsi b/package/winx86/openpilotgcs.nsi index c36a4bb39..2a032870d 100644 --- a/package/winx86/openpilotgcs.nsi +++ b/package/winx86/openpilotgcs.nsi @@ -1,7 +1,7 @@ # # Project: OpenPilot # NSIS configuration file for OpenPilot GCS -# The OpenPilot Team, http://www.openpilot.org, Copyright (C) 2010-2011. +# The OpenPilot Team, http://www.openpilot.org, Copyright (C) 2010-2012. # # 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 @@ -21,14 +21,15 @@ # This script requires Unicode NSIS 2.46 or higher: # http://www.scratchpaper.com/ -# Features: -# - Installs to the user local appdata path, no admin rights required. -# # TODO: -# - optionally install for all users (to Program Files with admin rights on Vista/7). # - install only built/used modules, not a whole directory. # - remove only installed files, not a whole directory. +;-------------------------------- +; Includes + +!include "x64.nsh" + ;-------------------------------- ; Paths @@ -36,12 +37,14 @@ !define PROJECT_ROOT "..\.." !define NSIS_DATA_TREE "." !define GCS_BUILD_TREE "..\..\build\ground\openpilotgcs" + !define UAVO_SYNTH_TREE "..\..\build\uavobject-synthetics" + !define AEROSIMRC_TREE "..\..\build\ground\AeroSIM-RC" ; Default installation folder - InstallDir "$LOCALAPPDATA\OpenPilot" + InstallDir "$PROGRAMFILES\OpenPilot" ; Get installation folder from registry if available - InstallDirRegKey HKCU "Software\OpenPilot" "Install Location" + InstallDirRegKey HKLM "Software\OpenPilot" "Install Location" ;-------------------------------- ; Version information @@ -82,7 +85,7 @@ XPStyle on ; Request application privileges for Windows Vista/7 - RequestExecutionLevel user + RequestExecutionLevel admin ; Compression level SetCompressor /solid lzma @@ -90,7 +93,7 @@ ;-------------------------------- ; Branding - BrandingText "© 2010-2011 The OpenPilot Team, http://www.openpilot.org" + BrandingText "© 2010-2012 The OpenPilot Team, http://www.openpilot.org" !define MUI_ICON "${NSIS_DATA_TREE}\resources\openpilot.ico" !define MUI_HEADERIMAGE @@ -113,6 +116,7 @@ ;-------------------------------- ; Settings for MUI_PAGE_FINISH !define MUI_FINISHPAGE_RUN + !define MUI_FINISHPAGE_SHOWREADME "$INSTDIR\HISTORY.txt" !define MUI_FINISHPAGE_RUN_FUNCTION "RunApplication" ;-------------------------------- @@ -148,6 +152,7 @@ ;-------------------------------- ; Installer sections +; Copy GCS core files Section "Core files" InSecCore SectionIn RO SetOutPath "$INSTDIR\bin" @@ -156,14 +161,16 @@ Section "Core files" InSecCore File "${PROJECT_ROOT}\HISTORY.txt" SectionEnd -Section "Plugins" InSecPlugins +; Copy GCS plugins +Section "-Plugins" InSecPlugins SectionIn RO SetOutPath "$INSTDIR\lib\openpilotgcs\plugins" File /r "${GCS_BUILD_TREE}\lib\openpilotgcs\plugins\*.dll" File /r "${GCS_BUILD_TREE}\lib\openpilotgcs\plugins\*.pluginspec" SectionEnd -Section "Resources" InSecResources +; Copy GCS resources +Section "-Resources" InSecResources SetOutPath "$INSTDIR\share\openpilotgcs\diagrams" File /r "${GCS_BUILD_TREE}\share\openpilotgcs\diagrams\*" SetOutPath "$INSTDIR\share\openpilotgcs\dials" @@ -176,22 +183,57 @@ Section "Resources" InSecResources File /r "${GCS_BUILD_TREE}\share\openpilotgcs\pfd\*" SectionEnd -Section "Sound files" InSecSounds +; Copy Notify plugin sound files +Section "-Sound files" InSecSounds SetOutPath "$INSTDIR\share\openpilotgcs\sounds" File /r "${GCS_BUILD_TREE}\share\openpilotgcs\sounds\*" SectionEnd -Section "Localization" InSecLocalization +; Copy localization files +; Disabled until GCS source is stable and properly localized +Section "-Localization" InSecLocalization SetOutPath "$INSTDIR\share\openpilotgcs\translations" ; File /r "${GCS_BUILD_TREE}\share\openpilotgcs\translations\openpilotgcs_*.qm" File /r "${GCS_BUILD_TREE}\share\openpilotgcs\translations\qt_*.qm" SectionEnd +; Copy firmware files Section "Firmware" InSecFirmware ; SetOutPath "$INSTDIR\firmware\${FIRMWARE_DIR}" ; File /r "${PACKAGE_DIR}\${FIRMWARE_DIR}\*" SetOutPath "$INSTDIR\firmware" - File /r "${PACKAGE_DIR}\${FIRMWARE_DIR}\fw_coptercontrol-${PACKAGE_LBL}.opfw" + File "${PACKAGE_DIR}\${FIRMWARE_DIR}\fw_coptercontrol-${PACKAGE_LBL}.opfw" + File "${PACKAGE_DIR}\${FIRMWARE_DIR}\fw_pipxtreme-${PACKAGE_LBL}.opfw" +SectionEnd + +; Copy utility files +Section "-Utilities" InSecUtilities + SetOutPath "$INSTDIR\utilities" + File "/oname=OPLogConvert-${PACKAGE_LBL}.m" "${UAVO_SYNTH_TREE}\matlab\OPLogConvert.m" +SectionEnd + +; Copy driver files +Section "-Drivers" InSecDrivers + SetOutPath "$INSTDIR\drivers" + File "${PROJECT_ROOT}\flight\Project\Windows USB\OpenPilot-CDC.inf" +SectionEnd + +; Preinstall OpenPilot CDC driver +Section "CDC driver" InSecInstallDrivers + InitPluginsDir + SetOutPath "$PLUGINSDIR" + ${If} ${RunningX64} + File "/oname=dpinst.exe" "${NSIS_DATA_TREE}\redist\dpinst_x64.exe" + ${Else} + File "/oname=dpinst.exe" "${NSIS_DATA_TREE}\redist\dpinst_x86.exe" + ${EndIf} + ExecWait '"$PLUGINSDIR\dpinst.exe" /lm /path "$INSTDIR\drivers"' +SectionEnd + +; AeroSimRC plugin files +Section "AeroSimRC plugin" InSecAeroSimRC + SetOutPath "$INSTDIR\misc\AeroSIM-RC" + File /r "${AEROSIMRC_TREE}\*" SectionEnd Section "Shortcuts" InSecShortcuts @@ -239,6 +281,10 @@ SectionEnd !insertmacro MUI_DESCRIPTION_TEXT ${InSecSounds} $(DESC_InSecSounds) !insertmacro MUI_DESCRIPTION_TEXT ${InSecLocalization} $(DESC_InSecLocalization) !insertmacro MUI_DESCRIPTION_TEXT ${InSecFirmware} $(DESC_InSecFirmware) + !insertmacro MUI_DESCRIPTION_TEXT ${InSecUtilities} $(DESC_InSecUtilities) + !insertmacro MUI_DESCRIPTION_TEXT ${InSecDrivers} $(DESC_InSecDrivers) + !insertmacro MUI_DESCRIPTION_TEXT ${InSecInstallDrivers} $(DESC_InSecInstallDrivers) + !insertmacro MUI_DESCRIPTION_TEXT ${InSecAeroSimRC} $(DESC_InSecAeroSimRC) !insertmacro MUI_DESCRIPTION_TEXT ${InSecShortcuts} $(DESC_InSecShortcuts) !insertmacro MUI_FUNCTION_DESCRIPTION_END @@ -247,6 +293,7 @@ SectionEnd Function .onInit + SetShellVarContext all !insertmacro MUI_LANGDLL_DISPLAY FunctionEnd @@ -260,6 +307,9 @@ Section "un.OpenPilot GCS" UnSecProgram RMDir /r /rebootok "$INSTDIR\lib" RMDir /r /rebootok "$INSTDIR\share" RMDir /r /rebootok "$INSTDIR\firmware" + RMDir /r /rebootok "$INSTDIR\utilities" + RMDir /r /rebootok "$INSTDIR\drivers" + RMDir /r /rebootok "$INSTDIR\misc" Delete /rebootok "$INSTDIR\HISTORY.txt" Delete /rebootok "$INSTDIR\Uninstall.exe" @@ -306,6 +356,7 @@ SectionEnd Function un.onInit + SetShellVarContext all !insertmacro MUI_UNGETLANGUAGE FunctionEnd diff --git a/package/winx86/redist/dpinst_x64.exe b/package/winx86/redist/dpinst_x64.exe new file mode 100644 index 000000000..3182f9868 Binary files /dev/null and b/package/winx86/redist/dpinst_x64.exe differ diff --git a/package/winx86/redist/dpinst_x86.exe b/package/winx86/redist/dpinst_x86.exe new file mode 100644 index 000000000..a8d609956 Binary files /dev/null and b/package/winx86/redist/dpinst_x86.exe differ diff --git a/package/winx86/translations/strings_de.nsh b/package/winx86/translations/strings_de.nsh index 8a0f3d6fa..71ee8c489 100644 --- a/package/winx86/translations/strings_de.nsh +++ b/package/winx86/translations/strings_de.nsh @@ -31,6 +31,10 @@ LangString DESC_InSecSounds ${LANG_GERMAN} "GCS Sounddateien (benötigt für akustische Ereignisbenachrichtigungen)." LangString DESC_InSecLocalization ${LANG_GERMAN} "GCS Lokalisierung (für unterstützte Sprachen)." LangString DESC_InSecFirmware ${LANG_GERMAN} "OpenPilot firmware binaries." + LangString DESC_InSecUtilities ${LANG_GERMAN} "OpenPilot Dienstprogramme (Matlab Log Parser)." + LangString DESC_InSecDrivers ${LANG_GERMAN} "OpenPilot Hardware Treiberdateien (optionaler OpenPilot CDC Treiber)." + LangString DESC_InSecInstallDrivers ${LANG_GERMAN} "OpenPilot CDC Treiber (optional)." + LangString DESC_InSecAeroSimRC ${LANG_GERMAN} "AeroSimRC plugin files with sample configuration." LangString DESC_InSecShortcuts ${LANG_GERMAN} "Installiere Verknüpfungen unter Startmenü->Anwendungen." ;-------------------------------- diff --git a/package/winx86/translations/strings_en.nsh b/package/winx86/translations/strings_en.nsh index 3e0fbb401..e8aa794b0 100644 --- a/package/winx86/translations/strings_en.nsh +++ b/package/winx86/translations/strings_en.nsh @@ -31,6 +31,10 @@ LangString DESC_InSecSounds ${LANG_ENGLISH} "GCS sound files (used for audible event notifications)." LangString DESC_InSecLocalization ${LANG_ENGLISH} "GCS localization (for supported languages)." LangString DESC_InSecFirmware ${LANG_ENGLISH} "OpenPilot firmware binaries." + LangString DESC_InSecUtilities ${LANG_ENGLISH} "OpenPilot utilities (Matlab log parser)." + LangString DESC_InSecDrivers ${LANG_ENGLISH} "OpenPilot hardware driver files (optional OpenPilot CDC driver)." + LangString DESC_InSecInstallDrivers ${LANG_ENGLISH} "Optional OpenPilot CDC driver (virtual USB COM port)." + LangString DESC_InSecAeroSimRC ${LANG_ENGLISH} "AeroSimRC plugin files with sample configuration." LangString DESC_InSecShortcuts ${LANG_ENGLISH} "Install application start menu shortcuts." ;-------------------------------- diff --git a/package/winx86/translations/strings_es.nsh b/package/winx86/translations/strings_es.nsh index aa43b06bd..6ac59f0d8 100644 --- a/package/winx86/translations/strings_es.nsh +++ b/package/winx86/translations/strings_es.nsh @@ -31,6 +31,10 @@ LangString DESC_InSecSounds ${LANG_SPANISH} "Archivos de sonido del GCS (usados para los eventos y notificaciones audibles)." LangString DESC_InSecLocalization ${LANG_SPANISH} "Localización GCS (idiomas soportados)." LangString DESC_InSecFirmware ${LANG_SPANISH} "OpenPilot firmware binaries." + LangString DESC_InSecUtilities ${LANG_SPANISH} "OpenPilot utilities (Matlab log parser)." + LangString DESC_InSecDrivers ${LANG_SPANISH} "OpenPilot hardware driver files (optional OpenPilot CDC driver)." + LangString DESC_InSecInstallDrivers ${LANG_SPANISH} "Optional OpenPilot CDC driver (virtual USB COM port)." + LangString DESC_InSecAeroSimRC ${LANG_SPANISH} "AeroSimRC plugin files with sample configuration." LangString DESC_InSecShortcuts ${LANG_SPANISH} "Instalar accesos directos de la aplicación (menú inicio y escritorio)." ;-------------------------------- diff --git a/package/winx86/translations/strings_fr.nsh b/package/winx86/translations/strings_fr.nsh index 7c831590c..c0919405e 100644 --- a/package/winx86/translations/strings_fr.nsh +++ b/package/winx86/translations/strings_fr.nsh @@ -31,6 +31,10 @@ LangString DESC_InSecSounds ${LANG_FRENCH} "Fichiers son GCS (pour les notifications sonores)." LangString DESC_InSecLocalization ${LANG_FRENCH} "Fichiers de localisation (langues supportées)." LangString DESC_InSecFirmware ${LANG_FRENCH} "OpenPilot firmware binaries." + LangString DESC_InSecUtilities ${LANG_FRENCH} "OpenPilot utilities (Matlab log parser)." + LangString DESC_InSecDrivers ${LANG_FRENCH} "OpenPilot hardware driver files (optional OpenPilot CDC driver)." + LangString DESC_InSecInstallDrivers ${LANG_FRENCH} "Optional OpenPilot CDC driver (virtual USB COM port)." + LangString DESC_InSecAeroSimRC ${LANG_FRENCH} "AeroSimRC plugin files with sample configuration." LangString DESC_InSecShortcuts ${LANG_FRENCH} "Installer les raccourcis dans le menu démarrer." ;-------------------------------- diff --git a/package/winx86/translations/strings_ru.nsh b/package/winx86/translations/strings_ru.nsh index 4b5a380dd..0aa90b31a 100644 --- a/package/winx86/translations/strings_ru.nsh +++ b/package/winx86/translations/strings_ru.nsh @@ -31,6 +31,10 @@ LangString DESC_InSecSounds ${LANG_RUSSIAN} "Звуковые файлы (используются для звуковых уведомлений о событиях)." LangString DESC_InSecLocalization ${LANG_RUSSIAN} "Файлы языковой поддержки (для поддерживаемых языков)." LangString DESC_InSecFirmware ${LANG_RUSSIAN} "Файлы прошивок OpenPilot." + LangString DESC_InSecUtilities ${LANG_RUSSIAN} "Утилиты (конвертор логов для Matlab)." + LangString DESC_InSecDrivers ${LANG_RUSSIAN} "Файлы драйверов (опциональный драйвер CDC порта)." + LangString DESC_InSecInstallDrivers ${LANG_RUSSIAN} "Опциональный OpenPilot CDC драйвер (виртуальный USB COM порт)." + LangString DESC_InSecAeroSimRC ${LANG_RUSSIAN} "Файлы плагина для симулятора AeroSimRC с примером конфигурации." LangString DESC_InSecShortcuts ${LANG_RUSSIAN} "Установка ярлыков для приложения." ;-------------------------------- diff --git a/package/winx86/translations/strings_zh_CN.nsh b/package/winx86/translations/strings_zh_CN.nsh index 5dad290a8..50d867223 100644 --- a/package/winx86/translations/strings_zh_CN.nsh +++ b/package/winx86/translations/strings_zh_CN.nsh @@ -31,6 +31,10 @@ LangString DESC_InSecSounds ${LANG_TRADCHINESE} "地面站音频文件（用于对于特定事件的提醒）." LangString DESC_InSecLocalization ${LANG_TRADCHINESE} "地面站本土化（适用于它所支持的语言）." LangString DESC_InSecFirmware ${LANG_TRADCHINESE} "OpenPilot firmware binaries." + LangString DESC_InSecUtilities ${LANG_TRADCHINESE} "OpenPilot utilities (Matlab log parser)." + LangString DESC_InSecDrivers ${LANG_TRADCHINESE} "OpenPilot hardware driver files (optional OpenPilot CDC driver)." + LangString DESC_InSecInstallDrivers ${LANG_TRADCHINESE} "Optional OpenPilot CDC driver (virtual USB COM port)." + LangString DESC_InSecAeroSimRC ${LANG_TRADCHINESE} "AeroSimRC plugin files with sample configuration." LangString DESC_InSecShortcuts ${LANG_TRADCHINESE} "安装开始菜单的快捷方式." ;-------------------------------- diff --git a/shared/uavobjectdefinition/gcsreceiver.xml b/shared/uavobjectdefinition/gcsreceiver.xml index cbc00c965..ee4fa696f 100644 --- a/shared/uavobjectdefinition/gcsreceiver.xml +++ b/shared/uavobjectdefinition/gcsreceiver.xml @@ -2,7 +2,7 @@ A receiver channel group carried over the telemetry link. - + diff --git a/shared/uavobjectdefinition/gpssettings.xml b/shared/uavobjectdefinition/gpssettings.xml new file mode 100644 index 000000000..5c38f43fe --- /dev/null +++ b/shared/uavobjectdefinition/gpssettings.xml @@ -0,0 +1,10 @@ + + + Settings for the GPS + + + + + + + diff --git a/shared/uavobjectdefinition/gpsvelocity.xml b/shared/uavobjectdefinition/gpsvelocity.xml index 7d57ccacc..8673463a8 100644 --- a/shared/uavobjectdefinition/gpsvelocity.xml +++ b/shared/uavobjectdefinition/gpsvelocity.xml @@ -1,6 +1,6 @@ - Raw GPS data from @ref GPSModule. Should only be used by @ref AHRSCommsModule. + Raw GPS data from @ref GPSModule. diff --git a/shared/uavobjectdefinition/pipxsettings.xml b/shared/uavobjectdefinition/pipxsettings.xml index a8899ff47..52524aa22 100644 --- a/shared/uavobjectdefinition/pipxsettings.xml +++ b/shared/uavobjectdefinition/pipxsettings.xml @@ -2,9 +2,9 @@ PipXtreme configurations options. - + - +