From 7d7d03cfd0e7dad8bd9ea4b30c3b539d0d8c754b Mon Sep 17 00:00:00 2001 From: James Cotton Date: Wed, 15 Jun 2011 11:18:29 -0500 Subject: [PATCH 001/145] OP-193 OP-511: Basic code for camera stabilization. Currently tune range with the servo endpoints and neutral. --- flight/Modules/CameraStab/camerastab.c | 115 ++++++++++++++++++ flight/Modules/CameraStab/inc/camerastab.h | 42 +++++++ .../OpenPilotOSX.xcodeproj/project.pbxproj | 20 +++ 3 files changed, 177 insertions(+) create mode 100644 flight/Modules/CameraStab/camerastab.c create mode 100644 flight/Modules/CameraStab/inc/camerastab.h diff --git a/flight/Modules/CameraStab/camerastab.c b/flight/Modules/CameraStab/camerastab.c new file mode 100644 index 000000000..c82a14759 --- /dev/null +++ b/flight/Modules/CameraStab/camerastab.c @@ -0,0 +1,115 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup CameraStab Camera Stabilization Module + * @brief Camera stabilization module + * Updates accessory outputs with values appropriate for camera stabilization + * @{ + * + * @file camerastab.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Stabilize camera against the roll pitch and yaw of aircraft + * + * @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 + */ + +/** + * Output object: Accessory + * + * This module will periodically calculate the output values for stabilizing the camera + * + * UAVObjects are automatically generated by the UAVObjectGenerator from + * the object definition XML file. + * + * Modules have no API, all communication to other modules is done through UAVObjects. + * However modules may use the API exposed by shared libraries. + * See the OpenPilot wiki for more details. + * http://www.openpilot.org/OpenPilot_Application_Architecture + * + */ + +#include "openpilot.h" + +#include "accessorydesired.h" +#include "attitudeactual.h" + +// +// Configuration +// +#define SAMPLE_PERIOD_MS 50 + +// Private types + +// Private variables + +// Private functions +static void attitudeUpdated(UAVObjEvent* ev); + +/** + * Initialise the module, called on startup + * \returns 0 on success or -1 if initialisation failed + */ +int32_t CameraStabInitialize(void) +{ + static UAVObjEvent ev; + ev.obj = AttitudeActualHandle(); + ev.instId = 0; + ev.event = 0; + + EventPeriodicCallbackCreate(&ev, attitudeUpdated, SAMPLE_PERIOD_MS / portTICK_RATE_MS); + + return 0; +} + +static void attitudeUpdated(UAVObjEvent* ev) +{ + if (ev->obj != AttitudeActualHandle()) + return; + + float attitude; + AccessoryDesiredData accessory; + float rollConst = 1.0/35.0; + float pitchConst = 1.0/35.0; + float yawConst = 1.0/35.0; + + + AttitudeActualRollGet(&attitude); + accessory.AccessoryVal = attitude * rollConst; + if(AccessoryDesiredInstSet(0, &accessory) != 0) + AccessoryDesiredCreateInstance(); + + AttitudeActualPitchGet(&attitude); + accessory.AccessoryVal = attitude * pitchConst; + if(AccessoryDesiredInstSet(1, &accessory) != 0) + AccessoryDesiredCreateInstance(); + + AttitudeActualYawGet(&attitude); + accessory.AccessoryVal = attitude * yawConst; + if(AccessoryDesiredInstSet(2, &accessory) != 0) + AccessoryDesiredCreateInstance(); +} + +/** + * @} + */ + +/** + * @} + */ diff --git a/flight/Modules/CameraStab/inc/camerastab.h b/flight/Modules/CameraStab/inc/camerastab.h new file mode 100644 index 000000000..bc2515696 --- /dev/null +++ b/flight/Modules/CameraStab/inc/camerastab.h @@ -0,0 +1,42 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup BatteryModule Battery Module + * @{ + * + * @file battery.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Module to read the battery Voltage and Current periodically and set alarms appropriately. + * + * @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 BATTERY_H +#define BATTERY_H + +#include "openpilot.h" + +int32_t BatteryInitialize(void); + +#endif // BATTERY_H + +/** + * @} + * @} + */ diff --git a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj index 4f2252237..452547f2e 100644 --- a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj +++ b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj @@ -2687,6 +2687,8 @@ 65C35EA812F0A834004811C2 /* eventdispatcher.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = eventdispatcher.c; sourceTree = ""; }; 65C35F6612F0DC2D004811C2 /* attitude.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = attitude.c; sourceTree = ""; }; 65C35F6812F0DC2D004811C2 /* attitude.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = attitude.h; sourceTree = ""; }; + 65C9903C13A871B90082BD60 /* camerastab.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = camerastab.c; sourceTree = ""; }; + 65C9903E13A871B90082BD60 /* camerastab.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = camerastab.h; sourceTree = ""; }; 65D2CA841248F9A400B1E7D6 /* mixersettings.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = mixersettings.xml; sourceTree = ""; }; 65D2CA851248F9A400B1E7D6 /* mixerstatus.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = mixerstatus.xml; sourceTree = ""; }; 65E410AE12F65AEA00725888 /* attitudesettings.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = attitudesettings.xml; sourceTree = ""; }; @@ -3205,6 +3207,7 @@ 650D8E2812DFE16400D05CC9 /* Altitude */, 65C35F6512F0DC2D004811C2 /* Attitude */, 650D8E2E12DFE16400D05CC9 /* Battery */, + 65C9903B13A871B90082BD60 /* CameraStab */, 650D8E3212DFE16400D05CC9 /* Example */, 650D8E3B12DFE16400D05CC9 /* FirmwareIAP */, 650D8E3F12DFE16400D05CC9 /* FlightPlan */, @@ -7498,6 +7501,23 @@ path = inc; sourceTree = ""; }; + 65C9903B13A871B90082BD60 /* CameraStab */ = { + isa = PBXGroup; + children = ( + 65C9903C13A871B90082BD60 /* camerastab.c */, + 65C9903D13A871B90082BD60 /* inc */, + ); + path = CameraStab; + sourceTree = ""; + }; + 65C9903D13A871B90082BD60 /* inc */ = { + isa = PBXGroup; + children = ( + 65C9903E13A871B90082BD60 /* camerastab.h */, + ); + path = inc; + sourceTree = ""; + }; 65E6DF7012E02E8E00058553 /* CopterControl */ = { isa = PBXGroup; children = ( From 926246b40208252d9b10050e0c1843627576ef72 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Thu, 16 Jun 2011 16:35:37 +0200 Subject: [PATCH 002/145] UAVTalk: Make UAVTalk object oriented, allowing multiple UAVTalk instances --- flight/Modules/Telemetry/telemetry.c | 13 +- flight/UAVTalk/inc/uavtalk.h | 52 +++- flight/UAVTalk/uavtalk.c | 436 ++++++++++++++------------- 3 files changed, 277 insertions(+), 224 deletions(-) diff --git a/flight/Modules/Telemetry/telemetry.c b/flight/Modules/Telemetry/telemetry.c index 07c7dc4dc..481641f76 100644 --- a/flight/Modules/Telemetry/telemetry.c +++ b/flight/Modules/Telemetry/telemetry.c @@ -66,6 +66,7 @@ static uint32_t txErrors; static uint32_t txRetries; static TelemetrySettingsData settings; static uint32_t timeOfLastObjectUpdate; +static UAVTalkConnection uavTalkCon; // Private functions static void telemetryTxTask(void *parameters); @@ -102,7 +103,7 @@ int32_t TelemetryInitialize(void) updateSettings(); // Initialise UAVTalk - UAVTalkInitialize(&transmitData); + UAVTalkInitialize(&uavTalkCon, &transmitData); // Process all registered objects and connect queue for updates UAVObjIterate(®isterObject); @@ -221,7 +222,7 @@ static void processObjEvent(UAVObjEvent * ev) if (ev->event == EV_UPDATED || ev->event == EV_UPDATED_MANUAL) { // Send update to GCS (with retries) while (retries < MAX_RETRIES && success == -1) { - success = UAVTalkSendObject(ev->obj, ev->instId, metadata.telemetryAcked, REQ_TIMEOUT_MS); // call blocks until ack is received or timeout + success = UAVTalkSendObject(&uavTalkCon, ev->obj, ev->instId, metadata.telemetryAcked, REQ_TIMEOUT_MS); // call blocks until ack is received or timeout ++retries; } // Update stats @@ -232,7 +233,7 @@ static void processObjEvent(UAVObjEvent * ev) } else if (ev->event == EV_UPDATE_REQ) { // Request object update from GCS (with retries) while (retries < MAX_RETRIES && success == -1) { - success = UAVTalkSendObjectRequest(ev->obj, ev->instId, REQ_TIMEOUT_MS); // call blocks until update is received or timeout + success = UAVTalkSendObjectRequest(&uavTalkCon, ev->obj, ev->instId, REQ_TIMEOUT_MS); // call blocks until update is received or timeout ++retries; } // Update stats @@ -309,7 +310,7 @@ static void telemetryRxTask(void *parameters) // TODO: Currently we periodically check the buffer for data, update once the PIOS_COM is made blocking len = PIOS_COM_ReceiveBufferUsed(inputPort); for (int32_t n = 0; n < len; ++n) { - UAVTalkProcessInputStream(PIOS_COM_ReceiveBuffer(inputPort)); + UAVTalkProcessInputStream(&uavTalkCon, PIOS_COM_ReceiveBuffer(inputPort)); } vTaskDelay(5); // <- remove when blocking calls are implemented @@ -403,8 +404,8 @@ static void updateTelemetryStats() uint32_t timeNow; // Get stats - UAVTalkGetStats(&utalkStats); - UAVTalkResetStats(); + UAVTalkGetStats(&uavTalkCon, &utalkStats); + UAVTalkResetStats(&uavTalkCon); // Get object data FlightTelemetryStatsGet(&flightStats); diff --git a/flight/UAVTalk/inc/uavtalk.h b/flight/UAVTalk/inc/uavtalk.h index 7a257620d..6d53f04f6 100644 --- a/flight/UAVTalk/inc/uavtalk.h +++ b/flight/UAVTalk/inc/uavtalk.h @@ -32,10 +32,21 @@ // Public constants #define UAVTALK_WAITFOREVER -1 #define UAVTALK_NOWAIT 0 +#define UAVTALK_MIN_HEADER_LENGTH 8 // sync(1), type (1), size (2), object ID (4) +#define UAVTALK_MAX_HEADER_LENGTH 10 // sync(1), type (1), size (2), object ID (4), instance ID (2, not used in single objects) + +#define UAVTALK_CHECKSUM_LENGTH 1 + +#define UAVTALK_MAX_PAYLOAD_LENGTH 256 + +#define UAVTALK_MAX_PACKET_LENGTH (UAVTALK_MAX_HEADER_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH + UAVTALK_CHECKSUM_LENGTH) + // Public types typedef int32_t (*UAVTalkOutputStream)(uint8_t* data, int32_t length); +typedef enum {UAVTALK_STATE_SYNC, UAVTALK_STATE_TYPE, UAVTALK_STATE_SIZE, UAVTALK_STATE_OBJID, UAVTALK_STATE_INSTID, UAVTALK_STATE_DATA, UAVTALK_STATE_CS} UAVTalkRxState; + typedef struct { uint32_t txBytes; uint32_t rxBytes; @@ -47,14 +58,41 @@ typedef struct { uint32_t rxErrors; } UAVTalkStats; +typedef struct { + UAVObjHandle obj; + uint8_t type; + uint16_t packet_size; + uint32_t objId; + uint16_t instId; + uint32_t length; + uint8_t cs; + int32_t rxCount; + UAVTalkRxState state; + uint16_t rxPacketLength; +} UAVTalkInputProcessor; + +typedef struct { + UAVTalkOutputStream outStream; + xSemaphoreHandle lock; + xSemaphoreHandle transLock; + xSemaphoreHandle respSema; + UAVObjHandle respObj; + uint16_t respInstId; + uint8_t rxBuffer[UAVTALK_MAX_PACKET_LENGTH]; + uint8_t txBuffer[UAVTALK_MAX_PACKET_LENGTH]; + UAVTalkStats stats; + UAVTalkInputProcessor iproc; +} UAVTalkConnection; + // Public functions -int32_t UAVTalkInitialize(UAVTalkOutputStream outputStream); -int32_t UAVTalkSetOutputStream(UAVTalkOutputStream outputStream); -int32_t UAVTalkSendObject(UAVObjHandle obj, uint16_t instId, uint8_t acked, int32_t timeoutMs); -int32_t UAVTalkSendObjectRequest(UAVObjHandle obj, uint16_t instId, int32_t timeoutMs); -int32_t UAVTalkProcessInputStream(uint8_t rxbyte); -void UAVTalkGetStats(UAVTalkStats* stats); -void UAVTalkResetStats(); +int32_t UAVTalkInitialize(UAVTalkConnection *connection, UAVTalkOutputStream outputStream); +int32_t UAVTalkSetOutputStream(UAVTalkConnection *connection, UAVTalkOutputStream outputStream); +UAVTalkOutputStream UAVTalkGetOutputStream(UAVTalkConnection *connection); +int32_t UAVTalkSendObject(UAVTalkConnection *connection, UAVObjHandle obj, uint16_t instId, uint8_t acked, int32_t timeoutMs); +int32_t UAVTalkSendObjectRequest(UAVTalkConnection *connection, UAVObjHandle obj, uint16_t instId, int32_t timeoutMs); +int32_t UAVTalkProcessInputStream(UAVTalkConnection *connection, uint8_t rxbyte); +void UAVTalkGetStats(UAVTalkConnection *connection, UAVTalkStats* stats); +void UAVTalkResetStats(UAVTalkConnection *connection); #endif // UAVTALK_H /** diff --git a/flight/UAVTalk/uavtalk.c b/flight/UAVTalk/uavtalk.c index 48ad03f53..36fdd9c12 100644 --- a/flight/UAVTalk/uavtalk.c +++ b/flight/UAVTalk/uavtalk.c @@ -41,102 +41,119 @@ #define TYPE_ACK (TYPE_VER | 0x03) #define TYPE_NACK (TYPE_VER | 0x04) -#define MIN_HEADER_LENGTH 8 // sync(1), type (1), size (2), object ID (4) -#define MAX_HEADER_LENGTH 10 // sync(1), type (1), size (2), object ID (4), instance ID (2, not used in single objects) - -#define CHECKSUM_LENGTH 1 - -#define MAX_PAYLOAD_LENGTH 256 - -#define MAX_PACKET_LENGTH (MAX_HEADER_LENGTH + MAX_PAYLOAD_LENGTH + CHECKSUM_LENGTH) - - // Private types -typedef enum {STATE_SYNC, STATE_TYPE, STATE_SIZE, STATE_OBJID, STATE_INSTID, STATE_DATA, STATE_CS} RxState; - -// Private variables -static UAVTalkOutputStream outStream; -static xSemaphoreHandle lock; -static xSemaphoreHandle transLock; -static xSemaphoreHandle respSema; -static UAVObjHandle respObj; -static uint16_t respInstId; -static uint8_t rxBuffer[MAX_PACKET_LENGTH]; -static uint8_t txBuffer[MAX_PACKET_LENGTH]; -static UAVTalkStats stats; // Private functions -static int32_t objectTransaction(UAVObjHandle objectId, uint16_t instId, uint8_t type, int32_t timeout); -static int32_t sendObject(UAVObjHandle obj, uint16_t instId, uint8_t type); -static int32_t sendSingleObject(UAVObjHandle obj, uint16_t instId, uint8_t type); -static int32_t sendNack(uint32_t objId); -static int32_t receiveObject(uint8_t type, uint32_t objId, uint16_t instId, uint8_t* data, int32_t length); -static void updateAck(UAVObjHandle obj, uint16_t instId); +static int32_t objectTransaction(UAVTalkConnection *connection, UAVObjHandle objectId, uint16_t instId, uint8_t type, int32_t timeout); +static int32_t sendObject(UAVTalkConnection *connection, UAVObjHandle obj, uint16_t instId, uint8_t type); +static int32_t sendSingleObject(UAVTalkConnection *connection, UAVObjHandle obj, uint16_t instId, uint8_t type); +static int32_t sendNack(UAVTalkConnection *connection, uint32_t objId); +static int32_t receiveObject(UAVTalkConnection *connection, uint8_t type, uint32_t objId, uint16_t instId, uint8_t* data, int32_t length); +static void updateAck(UAVTalkConnection *connection, UAVObjHandle obj, uint16_t instId); /** * Initialize the UAVTalk library + * \param[in] connection UAVTalkConnection to be used * \param[in] outputStream Function pointer that is called to send a data buffer * \return 0 Success * \return -1 Failure */ -int32_t UAVTalkInitialize(UAVTalkOutputStream outputStream) +int32_t UAVTalkInitialize(UAVTalkConnection *connection, UAVTalkOutputStream outputStream) { - outStream = outputStream; - lock = xSemaphoreCreateRecursiveMutex(); - transLock = xSemaphoreCreateRecursiveMutex(); - vSemaphoreCreateBinary(respSema); - xSemaphoreTake(respSema, 0); // reset to zero - UAVTalkResetStats(); + connection->iproc.rxPacketLength = 0; + connection->iproc.state = UAVTALK_STATE_SYNC; + connection->outStream = outputStream; + connection->lock = xSemaphoreCreateRecursiveMutex(); + connection->transLock = xSemaphoreCreateRecursiveMutex(); + vSemaphoreCreateBinary(connection->respSema); + xSemaphoreTake(connection->respSema, 0); // reset to zero + UAVTalkResetStats(connection); return 0; } /** - * Get communication statistics counters - * @param[out] statsOut Statistics counters + * Set the communication output stream + * \param[in] connection UAVTalkConnection to be used + * \param[in] outputStream Function pointer that is called to send a data buffer + * \return 0 Success + * \return -1 Failure */ -void UAVTalkGetStats(UAVTalkStats* statsOut) +int32_t UAVTalkSetOutputStream(UAVTalkConnection *connection, UAVTalkOutputStream outputStream) { // Lock - xSemaphoreTakeRecursive(lock, portMAX_DELAY); + xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); - // Copy stats - memcpy(statsOut, &stats, sizeof(UAVTalkStats)); + // set output stream + connection->outStream = outputStream; // Release lock - xSemaphoreGiveRecursive(lock); + xSemaphoreGiveRecursive(connection->lock); + + return 0; + +} + +/** + * Get current output stream + * \param[in] connection UAVTalkConnection to be used + * @return UAVTarlkOutputStream the output stream used + */ +UAVTalkOutputStream UAVTalkGetOutputStream(UAVTalkConnection *connection) +{ + return connection->outStream; +} + +/** + * Get communication statistics counters + * \param[in] connection UAVTalkConnection to be used + * @param[out] statsOut Statistics counters + */ +void UAVTalkGetStats(UAVTalkConnection *connection, UAVTalkStats* statsOut) +{ + // Lock + xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); + + // Copy stats + memcpy(statsOut, &connection->stats, sizeof(UAVTalkStats)); + + // Release lock + xSemaphoreGiveRecursive(connection->lock); } /** * Reset the statistics counters. + * \param[in] connection UAVTalkConnection to be used */ -void UAVTalkResetStats() +void UAVTalkResetStats(UAVTalkConnection *connection) { // Lock - xSemaphoreTakeRecursive(lock, portMAX_DELAY); + xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); // Clear stats - memset(&stats, 0, sizeof(UAVTalkStats)); + memset(&connection->stats, 0, sizeof(UAVTalkStats)); // Release lock - xSemaphoreGiveRecursive(lock); + xSemaphoreGiveRecursive(connection->lock); } /** * Request an update for the specified object, on success the object data would have been * updated by the GCS. + * \param[in] connection UAVTalkConnection to be used * \param[in] obj Object to update * \param[in] instId The instance ID or UAVOBJ_ALL_INSTANCES for all instances. * \param[in] timeout Time to wait for the response, when zero it will return immediately * \return 0 Success * \return -1 Failure */ -int32_t UAVTalkSendObjectRequest(UAVObjHandle obj, uint16_t instId, int32_t timeout) +int32_t UAVTalkSendObjectRequest(UAVTalkConnection *connection, UAVObjHandle obj, uint16_t instId, int32_t timeout) { - return objectTransaction(obj, instId, TYPE_OBJ_REQ, timeout); + return objectTransaction(connection, obj, instId, TYPE_OBJ_REQ, timeout); } /** * Send the specified object through the telemetry link. + * \param[in] connection UAVTalkConnection to be used * \param[in] obj Object to send * \param[in] instId The instance ID or UAVOBJ_ALL_INSTANCES for all instances. * \param[in] acked Selects if an ack is required (1:ack required, 0: ack not required) @@ -144,21 +161,22 @@ int32_t UAVTalkSendObjectRequest(UAVObjHandle obj, uint16_t instId, int32_t time * \return 0 Success * \return -1 Failure */ -int32_t UAVTalkSendObject(UAVObjHandle obj, uint16_t instId, uint8_t acked, int32_t timeoutMs) +int32_t UAVTalkSendObject(UAVTalkConnection *connection, UAVObjHandle obj, uint16_t instId, uint8_t acked, int32_t timeoutMs) { // Send object if (acked == 1) { - return objectTransaction(obj, instId, TYPE_OBJ_ACK, timeoutMs); + return objectTransaction(connection, obj, instId, TYPE_OBJ_ACK, timeoutMs); } else { - return objectTransaction(obj, instId, TYPE_OBJ, timeoutMs); + return objectTransaction(connection, obj, instId, TYPE_OBJ, timeoutMs); } } /** * Execute the requested transaction on an object. + * \param[in] connection UAVTalkConnection to be used * \param[in] obj Object * \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances. * \param[in] type Transaction type @@ -168,7 +186,7 @@ int32_t UAVTalkSendObject(UAVObjHandle obj, uint16_t instId, uint8_t acked, int3 * \return 0 Success * \return -1 Failure */ -static int32_t objectTransaction(UAVObjHandle obj, uint16_t instId, uint8_t type, int32_t timeoutMs) +static int32_t objectTransaction(UAVTalkConnection *connection, UAVObjHandle obj, uint16_t instId, uint8_t type, int32_t timeoutMs) { int32_t respReceived; @@ -176,37 +194,37 @@ static int32_t objectTransaction(UAVObjHandle obj, uint16_t instId, uint8_t type if (type == TYPE_OBJ_ACK || type == TYPE_OBJ_REQ) { // Get transaction lock (will block if a transaction is pending) - xSemaphoreTakeRecursive(transLock, portMAX_DELAY); + xSemaphoreTakeRecursive(connection->transLock, portMAX_DELAY); // Send object - xSemaphoreTakeRecursive(lock, portMAX_DELAY); - respObj = obj; - respInstId = instId; - sendObject(obj, instId, type); - xSemaphoreGiveRecursive(lock); + xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); + connection->respObj = obj; + connection->respInstId = instId; + sendObject(connection, obj, instId, type); + xSemaphoreGiveRecursive(connection->lock); // Wait for response (or timeout) - respReceived = xSemaphoreTake(respSema, timeoutMs/portTICK_RATE_MS); + respReceived = xSemaphoreTake(connection->respSema, timeoutMs/portTICK_RATE_MS); // Check if a response was received if (respReceived == pdFALSE) { // Cancel transaction - xSemaphoreTakeRecursive(lock, portMAX_DELAY); - xSemaphoreTake(respSema, 0); // non blocking call to make sure the value is reset to zero (binary sema) - respObj = 0; - xSemaphoreGiveRecursive(lock); - xSemaphoreGiveRecursive(transLock); + xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); + xSemaphoreTake(connection->respSema, 0); // non blocking call to make sure the value is reset to zero (binary sema) + connection->respObj = 0; + xSemaphoreGiveRecursive(connection->lock); + xSemaphoreGiveRecursive(connection->transLock); return -1; } else { - xSemaphoreGiveRecursive(transLock); + xSemaphoreGiveRecursive(connection->transLock); return 0; } } else if (type == TYPE_OBJ) { - xSemaphoreTakeRecursive(lock, portMAX_DELAY); - sendObject(obj, instId, TYPE_OBJ); - xSemaphoreGiveRecursive(lock); + xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); + sendObject(connection, obj, instId, TYPE_OBJ); + xSemaphoreGiveRecursive(connection->lock); return 0; } else @@ -217,222 +235,211 @@ static int32_t objectTransaction(UAVObjHandle obj, uint16_t instId, uint8_t type /** * Process an byte from the telemetry stream. + * \param[in] connection UAVTalkConnection to be used * \param[in] rxbyte Received byte * \return 0 Success * \return -1 Failure */ -int32_t UAVTalkProcessInputStream(uint8_t rxbyte) +int32_t UAVTalkProcessInputStream(UAVTalkConnection *connection, uint8_t rxbyte) { - static UAVObjHandle obj; - static uint8_t type; - static uint16_t packet_size; - static uint32_t objId; - static uint16_t instId; - static uint32_t length; - static uint8_t cs, csRx; - static int32_t rxCount; - static RxState state = STATE_SYNC; - static uint16_t rxPacketLength = 0; + UAVTalkInputProcessor *iproc = &connection->iproc; + ++connection->stats.rxBytes; - ++stats.rxBytes; - - if (rxPacketLength < 0xffff) - rxPacketLength++; // update packet byte count + if (iproc->rxPacketLength < 0xffff) + iproc->rxPacketLength++; // update packet byte count // Receive state machine - switch (state) + switch (iproc->state) { - case STATE_SYNC: + case UAVTALK_STATE_SYNC: if (rxbyte != SYNC_VAL) break; // Initialize and update the CRC - cs = PIOS_CRC_updateByte(0, rxbyte); + iproc->cs = PIOS_CRC_updateByte(0, rxbyte); - rxPacketLength = 1; + iproc->rxPacketLength = 1; - state = STATE_TYPE; + iproc->state = UAVTALK_STATE_TYPE; break; - case STATE_TYPE: + case UAVTALK_STATE_TYPE: // update the CRC - cs = PIOS_CRC_updateByte(cs, rxbyte); + iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte); if ((rxbyte & TYPE_MASK) != TYPE_VER) { - state = STATE_SYNC; + iproc->state = UAVTALK_STATE_SYNC; break; } - type = rxbyte; + iproc->type = rxbyte; - packet_size = 0; + iproc->packet_size = 0; - state = STATE_SIZE; - rxCount = 0; + iproc->state = UAVTALK_STATE_SIZE; + iproc->rxCount = 0; break; - case STATE_SIZE: + case UAVTALK_STATE_SIZE: // update the CRC - cs = PIOS_CRC_updateByte(cs, rxbyte); + iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte); - if (rxCount == 0) + if (iproc->rxCount == 0) { - packet_size += rxbyte; - rxCount++; + iproc->packet_size += rxbyte; + iproc->rxCount++; break; } - packet_size += rxbyte << 8; + iproc->packet_size += rxbyte << 8; - if (packet_size < MIN_HEADER_LENGTH || packet_size > MAX_HEADER_LENGTH + MAX_PAYLOAD_LENGTH) + if (iproc->packet_size < UAVTALK_MIN_HEADER_LENGTH || iproc->packet_size > UAVTALK_MAX_HEADER_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH) { // incorrect packet size - state = STATE_SYNC; + iproc->state = UAVTALK_STATE_SYNC; break; } - rxCount = 0; - objId = 0; - state = STATE_OBJID; + iproc->rxCount = 0; + iproc->objId = 0; + iproc->state = UAVTALK_STATE_OBJID; break; - case STATE_OBJID: + case UAVTALK_STATE_OBJID: // update the CRC - cs = PIOS_CRC_updateByte(cs, rxbyte); + iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte); - objId += rxbyte << (8*(rxCount++)); + iproc->objId += rxbyte << (8*(iproc->rxCount++)); - if (rxCount < 4) + if (iproc->rxCount < 4) break; // Search for object, if not found reset state machine // except if we got a OBJ_REQ for an object which does not // exist, in which case we'll send a NACK - obj = UAVObjGetByID(objId); - if (obj == 0 && type != TYPE_OBJ_REQ) + iproc->obj = UAVObjGetByID(iproc->objId); + if (iproc->obj == 0 && iproc->type != TYPE_OBJ_REQ) { - stats.rxErrors++; - state = STATE_SYNC; + connection->stats.rxErrors++; + iproc->state = UAVTALK_STATE_SYNC; break; } // Determine data length - if (type == TYPE_OBJ_REQ || type == TYPE_ACK || type == TYPE_NACK) - length = 0; + if (iproc->type == TYPE_OBJ_REQ || iproc->type == TYPE_ACK || iproc->type == TYPE_NACK) + iproc->length = 0; else - length = UAVObjGetNumBytes(obj); + iproc->length = UAVObjGetNumBytes(iproc->obj); // Check length and determine next state - if (length >= MAX_PAYLOAD_LENGTH) + if (iproc->length >= UAVTALK_MAX_PAYLOAD_LENGTH) { - stats.rxErrors++; - state = STATE_SYNC; + connection->stats.rxErrors++; + iproc->state = UAVTALK_STATE_SYNC; break; } // Check the lengths match - if ((rxPacketLength + length) != packet_size) + if ((iproc->rxPacketLength + iproc->length) != iproc->packet_size) { // packet error - mismatched packet size - stats.rxErrors++; - state = STATE_SYNC; + connection->stats.rxErrors++; + iproc->state = UAVTALK_STATE_SYNC; break; } - instId = 0; - if (obj == 0) + iproc->instId = 0; + if (iproc->obj == 0) { // If this is a NACK, we skip to Checksum - state = STATE_CS; - rxCount = 0; + iproc->state = UAVTALK_STATE_CS; + iproc->rxCount = 0; } // Check if this is a single instance object (i.e. if the instance ID field is coming next) - else if (UAVObjIsSingleInstance(obj)) + else if (UAVObjIsSingleInstance(iproc->obj)) { // If there is a payload get it, otherwise receive checksum - if (length > 0) - state = STATE_DATA; + if (iproc->length > 0) + iproc->state = UAVTALK_STATE_DATA; else - state = STATE_CS; + iproc->state = UAVTALK_STATE_CS; - rxCount = 0; + iproc->rxCount = 0; } else { - state = STATE_INSTID; - rxCount = 0; + iproc->state = UAVTALK_STATE_INSTID; + iproc->rxCount = 0; } break; - case STATE_INSTID: + case UAVTALK_STATE_INSTID: // update the CRC - cs = PIOS_CRC_updateByte(cs, rxbyte); + iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte); - instId += rxbyte << (8*(rxCount++)); + iproc->instId += rxbyte << (8*(iproc->rxCount++)); - if (rxCount < 2) + if (iproc->rxCount < 2) break; - rxCount = 0; + iproc->rxCount = 0; // If there is a payload get it, otherwise receive checksum - if (length > 0) - state = STATE_DATA; + if (iproc->length > 0) + iproc->state = UAVTALK_STATE_DATA; else - state = STATE_CS; + iproc->state = UAVTALK_STATE_CS; break; - case STATE_DATA: + case UAVTALK_STATE_DATA: // update the CRC - cs = PIOS_CRC_updateByte(cs, rxbyte); + iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte); - rxBuffer[rxCount++] = rxbyte; - if (rxCount < length) + connection->rxBuffer[iproc->rxCount++] = rxbyte; + if (iproc->rxCount < iproc->length) break; - state = STATE_CS; - rxCount = 0; + iproc->state = UAVTALK_STATE_CS; + iproc->rxCount = 0; break; - case STATE_CS: + case UAVTALK_STATE_CS: // the CRC byte - csRx = rxbyte; - - if (csRx != cs) + if (rxbyte != iproc->cs) { // packet error - faulty CRC - stats.rxErrors++; - state = STATE_SYNC; + connection->stats.rxErrors++; + iproc->state = UAVTALK_STATE_SYNC; break; } - if (rxPacketLength != (packet_size + 1)) + if (iproc->rxPacketLength != (iproc->packet_size + 1)) { // packet error - mismatched packet size - stats.rxErrors++; - state = STATE_SYNC; + connection->stats.rxErrors++; + iproc->state = UAVTALK_STATE_SYNC; break; } - xSemaphoreTakeRecursive(lock, portMAX_DELAY); - receiveObject(type, objId, instId, rxBuffer, length); - stats.rxObjectBytes += length; - stats.rxObjects++; - xSemaphoreGiveRecursive(lock); + xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); + receiveObject(connection, iproc->type, iproc->objId, iproc->instId, connection->rxBuffer, iproc->length); + connection->stats.rxObjectBytes += iproc->length; + connection->stats.rxObjects++; + xSemaphoreGiveRecursive(connection->lock); - state = STATE_SYNC; + iproc->state = UAVTALK_STATE_SYNC; break; default: - stats.rxErrors++; - state = STATE_SYNC; + connection->stats.rxErrors++; + iproc->state = UAVTALK_STATE_SYNC; } // Done @@ -441,6 +448,7 @@ int32_t UAVTalkProcessInputStream(uint8_t rxbyte) /** * Receive an object. This function process objects received through the telemetry stream. + * \param[in] connection UAVTalkConnection to be used * \param[in] type Type of received message (TYPE_OBJ, TYPE_OBJ_REQ, TYPE_OBJ_ACK, TYPE_ACK, TYPE_NACK) * \param[in] objId ID of the object to work on * \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances. @@ -449,9 +457,9 @@ int32_t UAVTalkProcessInputStream(uint8_t rxbyte) * \return 0 Success * \return -1 Failure */ -static int32_t receiveObject(uint8_t type, uint32_t objId, uint16_t instId, uint8_t* data, int32_t length) +static int32_t receiveObject(UAVTalkConnection *connection, uint8_t type, uint32_t objId, uint16_t instId, uint8_t* data, int32_t length) { - static UAVObjHandle obj; + UAVObjHandle obj; int32_t ret = 0; // Get the handle to the Object. Will be zero @@ -467,7 +475,7 @@ static int32_t receiveObject(uint8_t type, uint32_t objId, uint16_t instId, uint // Unpack object, if the instance does not exist it will be created! UAVObjUnpack(obj, instId, data); // Check if an ack is pending - updateAck(obj, instId); + updateAck(connection, obj, instId); } else { @@ -482,7 +490,7 @@ static int32_t receiveObject(uint8_t type, uint32_t objId, uint16_t instId, uint if ( UAVObjUnpack(obj, instId, data) == 0 ) { // Transmit ACK - sendObject(obj, instId, TYPE_ACK); + sendObject(connection, obj, instId, TYPE_ACK); } else { @@ -497,9 +505,9 @@ static int32_t receiveObject(uint8_t type, uint32_t objId, uint16_t instId, uint case TYPE_OBJ_REQ: // Send requested object if message is of type OBJ_REQ if (obj == 0) - sendNack(objId); + sendNack(connection, objId); else - sendObject(obj, instId, TYPE_OBJ); + sendObject(connection, obj, instId, TYPE_OBJ); break; case TYPE_NACK: // Do nothing on flight side, let it time out. @@ -509,7 +517,7 @@ static int32_t receiveObject(uint8_t type, uint32_t objId, uint16_t instId, uint if (instId != UAVOBJ_ALL_INSTANCES) { // Check if an ack is pending - updateAck(obj, instId); + updateAck(connection, obj, instId); } else { @@ -525,25 +533,29 @@ static int32_t receiveObject(uint8_t type, uint32_t objId, uint16_t instId, uint /** * Check if an ack is pending on an object and give response semaphore + * \param[in] connection UAVTalkConnection to be used + * \param[in] obj Object + * \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances. */ -static void updateAck(UAVObjHandle obj, uint16_t instId) +static void updateAck(UAVTalkConnection *connection, UAVObjHandle obj, uint16_t instId) { - if (respObj == obj && (respInstId == instId || respInstId == UAVOBJ_ALL_INSTANCES)) + if (connection->respObj == obj && (connection->respInstId == instId || connection->respInstId == UAVOBJ_ALL_INSTANCES)) { - xSemaphoreGive(respSema); - respObj = 0; + xSemaphoreGive(connection->respSema); + connection->respObj = 0; } } /** * Send an object through the telemetry link. + * \param[in] connection UAVTalkConnection to be used * \param[in] obj Object handle to send * \param[in] instId The instance ID or UAVOBJ_ALL_INSTANCES for all instances * \param[in] type Transaction type * \return 0 Success * \return -1 Failure */ -static int32_t sendObject(UAVObjHandle obj, uint16_t instId, uint8_t type) +static int32_t sendObject(UAVTalkConnection *connection, UAVObjHandle obj, uint16_t instId, uint8_t type) { uint32_t numInst; uint32_t n; @@ -564,24 +576,24 @@ static int32_t sendObject(UAVObjHandle obj, uint16_t instId, uint8_t type) // Send all instances for (n = 0; n < numInst; ++n) { - sendSingleObject(obj, n, type); + sendSingleObject(connection, obj, n, type); } return 0; } else { - return sendSingleObject(obj, instId, type); + return sendSingleObject(connection, obj, instId, type); } } else if (type == TYPE_OBJ_REQ) { - return sendSingleObject(obj, instId, TYPE_OBJ_REQ); + return sendSingleObject(connection, obj, instId, TYPE_OBJ_REQ); } else if (type == TYPE_ACK) { if ( instId != UAVOBJ_ALL_INSTANCES ) { - return sendSingleObject(obj, instId, TYPE_ACK); + return sendSingleObject(connection, obj, instId, TYPE_ACK); } else { @@ -596,13 +608,14 @@ static int32_t sendObject(UAVObjHandle obj, uint16_t instId, uint8_t type) /** * Send an object through the telemetry link. + * \param[in] connection UAVTalkConnection to be used * \param[in] obj Object handle to send * \param[in] instId The instance ID (can NOT be UAVOBJ_ALL_INSTANCES, use sendObject() instead) * \param[in] type Transaction type * \return 0 Success * \return -1 Failure */ -static int32_t sendSingleObject(UAVObjHandle obj, uint16_t instId, uint8_t type) +static int32_t sendSingleObject(UAVTalkConnection *connection, UAVObjHandle obj, uint16_t instId, uint8_t type) { int32_t length; int32_t dataOffset; @@ -610,13 +623,13 @@ static int32_t sendSingleObject(UAVObjHandle obj, uint16_t instId, uint8_t type) // Setup type and object id fields objId = UAVObjGetID(obj); - txBuffer[0] = SYNC_VAL; // sync byte - txBuffer[1] = type; + connection->txBuffer[0] = SYNC_VAL; // sync byte + connection->txBuffer[1] = type; // data length inserted here below - txBuffer[4] = (uint8_t)(objId & 0xFF); - txBuffer[5] = (uint8_t)((objId >> 8) & 0xFF); - txBuffer[6] = (uint8_t)((objId >> 16) & 0xFF); - txBuffer[7] = (uint8_t)((objId >> 24) & 0xFF); + connection->txBuffer[4] = (uint8_t)(objId & 0xFF); + connection->txBuffer[5] = (uint8_t)((objId >> 8) & 0xFF); + connection->txBuffer[6] = (uint8_t)((objId >> 16) & 0xFF); + connection->txBuffer[7] = (uint8_t)((objId >> 24) & 0xFF); // Setup instance ID if one is required if (UAVObjIsSingleInstance(obj)) @@ -625,8 +638,8 @@ static int32_t sendSingleObject(UAVObjHandle obj, uint16_t instId, uint8_t type) } else { - txBuffer[8] = (uint8_t)(instId & 0xFF); - txBuffer[9] = (uint8_t)((instId >> 8) & 0xFF); + connection->txBuffer[8] = (uint8_t)(instId & 0xFF); + connection->txBuffer[9] = (uint8_t)((instId >> 8) & 0xFF); dataOffset = 10; } @@ -641,7 +654,7 @@ static int32_t sendSingleObject(UAVObjHandle obj, uint16_t instId, uint8_t type) } // Check length - if (length >= MAX_PAYLOAD_LENGTH) + if (length >= UAVTALK_MAX_PAYLOAD_LENGTH) { return -1; } @@ -649,26 +662,26 @@ static int32_t sendSingleObject(UAVObjHandle obj, uint16_t instId, uint8_t type) // Copy data (if any) if (length > 0) { - if ( UAVObjPack(obj, instId, &txBuffer[dataOffset]) < 0 ) + if ( UAVObjPack(obj, instId, &connection->txBuffer[dataOffset]) < 0 ) { return -1; } } // Store the packet length - txBuffer[2] = (uint8_t)((dataOffset+length) & 0xFF); - txBuffer[3] = (uint8_t)(((dataOffset+length) >> 8) & 0xFF); + connection->txBuffer[2] = (uint8_t)((dataOffset+length) & 0xFF); + connection->txBuffer[3] = (uint8_t)(((dataOffset+length) >> 8) & 0xFF); // Calculate checksum - txBuffer[dataOffset+length] = PIOS_CRC_updateCRC(0, txBuffer, dataOffset+length); + connection->txBuffer[dataOffset+length] = PIOS_CRC_updateCRC(0, connection->txBuffer, dataOffset+length); // Send buffer - if (outStream!=NULL) (*outStream)(txBuffer, dataOffset+length+CHECKSUM_LENGTH); + if (connection->outStream!=NULL) (*connection->outStream)(connection->txBuffer, dataOffset+length+UAVTALK_CHECKSUM_LENGTH); // Update stats - ++stats.txObjects; - stats.txBytes += dataOffset+length+CHECKSUM_LENGTH; - stats.txObjectBytes += length; + ++connection->stats.txObjects; + connection->stats.txBytes += dataOffset+length+UAVTALK_CHECKSUM_LENGTH; + connection->stats.txObjectBytes += length; // Done return 0; @@ -676,36 +689,37 @@ static int32_t sendSingleObject(UAVObjHandle obj, uint16_t instId, uint8_t type) /** * Send a NACK through the telemetry link. + * \param[in] connection UAVTalkConnection to be used * \param[in] objId Object ID to send a NACK for * \return 0 Success * \return -1 Failure */ -static int32_t sendNack(uint32_t objId) +static int32_t sendNack(UAVTalkConnection *connection, uint32_t objId) { int32_t dataOffset; - txBuffer[0] = SYNC_VAL; // sync byte - txBuffer[1] = TYPE_NACK; + connection->txBuffer[0] = SYNC_VAL; // sync byte + connection->txBuffer[1] = TYPE_NACK; // data length inserted here below - txBuffer[4] = (uint8_t)(objId & 0xFF); - txBuffer[5] = (uint8_t)((objId >> 8) & 0xFF); - txBuffer[6] = (uint8_t)((objId >> 16) & 0xFF); - txBuffer[7] = (uint8_t)((objId >> 24) & 0xFF); + connection->txBuffer[4] = (uint8_t)(objId & 0xFF); + connection->txBuffer[5] = (uint8_t)((objId >> 8) & 0xFF); + connection->txBuffer[6] = (uint8_t)((objId >> 16) & 0xFF); + connection->txBuffer[7] = (uint8_t)((objId >> 24) & 0xFF); dataOffset = 8; // Store the packet length - txBuffer[2] = (uint8_t)((dataOffset) & 0xFF); - txBuffer[3] = (uint8_t)(((dataOffset) >> 8) & 0xFF); + connection->txBuffer[2] = (uint8_t)((dataOffset) & 0xFF); + connection->txBuffer[3] = (uint8_t)(((dataOffset) >> 8) & 0xFF); // Calculate checksum - txBuffer[dataOffset] = PIOS_CRC_updateCRC(0, txBuffer, dataOffset); + connection->txBuffer[dataOffset] = PIOS_CRC_updateCRC(0, connection->txBuffer, dataOffset); // Send buffer - if (outStream!=NULL) (*outStream)(txBuffer, dataOffset+CHECKSUM_LENGTH); + if (connection->outStream!=NULL) (*connection->outStream)(connection->txBuffer, dataOffset+UAVTALK_CHECKSUM_LENGTH); // Update stats - stats.txBytes += dataOffset+CHECKSUM_LENGTH; + connection->stats.txBytes += dataOffset+UAVTALK_CHECKSUM_LENGTH; // Done return 0; From 9720a8444f3d1625337861342bd826813c996bc1 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Fri, 17 Jun 2011 09:50:10 -0500 Subject: [PATCH 003/145] Make a few things optional if they are mostly for diagnostics to keep memory down --- flight/CopterControl/Makefile | 17 ++++++++++++++--- .../CopterControl/System/inc/FreeRTOSConfig.h | 9 ++++++--- flight/CopterControl/System/taskmonitor.c | 5 +++++ flight/Modules/Actuator/actuator.c | 6 +++++- flight/Modules/Stabilization/stabilization.c | 7 ++++++- flight/OpenPilot/System/inc/FreeRTOSConfig.h | 6 +++++- flight/OpenPilot/System/taskmonitor.c | 5 +++++ 7 files changed, 46 insertions(+), 9 deletions(-) diff --git a/flight/CopterControl/Makefile b/flight/CopterControl/Makefile index 349edd2d1..91d0cd12b 100644 --- a/flight/CopterControl/Makefile +++ b/flight/CopterControl/Makefile @@ -37,6 +37,9 @@ OUTDIR := $(TOP)/build/$(TARGET) # Set to YES to compile for debugging DEBUG ?= NO +# Include objects that are just nice information to show +DIAGNOSTICS ?= NO + # Set to YES to build a FW version that will erase all flash memory ERASE_FLASH ?= NO # Set to YES to use the Servo output pins for debugging via scope or logic analyser @@ -156,16 +159,20 @@ SRC += $(OPUAVSYNTHDIR)/actuatorsettings.c SRC += $(OPUAVSYNTHDIR)/attituderaw.c SRC += $(OPUAVSYNTHDIR)/attitudeactual.c SRC += $(OPUAVSYNTHDIR)/manualcontrolcommand.c -SRC += $(OPUAVSYNTHDIR)/taskinfo.c SRC += $(OPUAVSYNTHDIR)/i2cstats.c SRC += $(OPUAVSYNTHDIR)/watchdogstatus.c SRC += $(OPUAVSYNTHDIR)/telemetrysettings.c -SRC += $(OPUAVSYNTHDIR)/ratedesired.c SRC += $(OPUAVSYNTHDIR)/manualcontrolsettings.c SRC += $(OPUAVSYNTHDIR)/mixersettings.c -SRC += $(OPUAVSYNTHDIR)/mixerstatus.c SRC += $(OPUAVSYNTHDIR)/firmwareiapobj.c SRC += $(OPUAVSYNTHDIR)/attitudesettings.c + +ifeq ($(DIAGNOISTCS),YES) +SRC += $(OPUAVSYNTHDIR)/taskinfo.c +SRC += $(OPUAVSYNTHDIR)/mixerstatus.c +SRC += $(OPUAVSYNTHDIR)/ratedesired.c +endif + #${wildcard ${OBJ}/$(shell echo $(VAR) | tr A-Z a-z)/*.c} #SRC += ${foreach OBJ, ${UAVOBJECTS}, $(UAVOBJECTS)/$(OBJ).c} # Cant use until i can automatically generate list of UAVObjects @@ -417,6 +424,10 @@ ifeq ($(DEBUG),YES) CFLAGS = -DDEBUG endif +ifeq ($(DIAGNOSTICS),YES) +CFLAGS = -DDIAGNOSTICS +endif + CFLAGS += -g$(DEBUGF) CFLAGS += -O$(OPT) CFLAGS += -mcpu=$(MCU) diff --git a/flight/CopterControl/System/inc/FreeRTOSConfig.h b/flight/CopterControl/System/inc/FreeRTOSConfig.h index c05f71ea5..f0ead760f 100644 --- a/flight/CopterControl/System/inc/FreeRTOSConfig.h +++ b/flight/CopterControl/System/inc/FreeRTOSConfig.h @@ -39,7 +39,6 @@ #define configUSE_RECURSIVE_MUTEXES 1 #define configUSE_COUNTING_SEMAPHORES 0 #define configUSE_ALTERNATIVE_API 0 -#define configCHECK_FOR_STACK_OVERFLOW 2 #define configQUEUE_REGISTRY_SIZE 10 /* Co-routine definitions. */ @@ -72,7 +71,9 @@ NVIC value of 255. */ #define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15 /* Enable run time stats collection */ -//#if defined(DEBUG) +#if defined(DIAGNOSTICS) +#define configCHECK_FOR_STACK_OVERFLOW 2 + #define configGENERATE_RUN_TIME_STATS 1 #define INCLUDE_uxTaskGetRunTime 1 #define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS()\ @@ -81,7 +82,9 @@ do {\ (*(unsigned long *)0xe0001000) |= 1; /* DWT_CTRL |= DWT_CYCCNT_ENA */\ } while(0) #define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004)/* DWT_CYCCNT */ -//#endif +#else +#define configCHECK_FOR_STACK_OVERFLOW 1 +#endif /** * @} diff --git a/flight/CopterControl/System/taskmonitor.c b/flight/CopterControl/System/taskmonitor.c index 8941a616b..6ff6299d0 100644 --- a/flight/CopterControl/System/taskmonitor.c +++ b/flight/CopterControl/System/taskmonitor.c @@ -46,7 +46,10 @@ int32_t TaskMonitorInitialize(void) { lock = xSemaphoreCreateRecursiveMutex(); memset(handles, 0, sizeof(xTaskHandle)*TASKINFO_RUNNING_NUMELEM); + lastMonitorTime = 0; +#if defined(DIAGNOSTICS) lastMonitorTime = portGET_RUN_TIME_COUNTER_VALUE(); +#endif return 0; } @@ -73,6 +76,7 @@ int32_t TaskMonitorAdd(TaskInfoRunningElem task, xTaskHandle handle) */ void TaskMonitorUpdateAll(void) { +#if defined(DIAGNOSTICS) TaskInfoData data; int n; @@ -123,4 +127,5 @@ void TaskMonitorUpdateAll(void) // Done xSemaphoreGiveRecursive(lock); +#endif } diff --git a/flight/Modules/Actuator/actuator.c b/flight/Modules/Actuator/actuator.c index af8ee06ea..c610b842c 100644 --- a/flight/Modules/Actuator/actuator.c +++ b/flight/Modules/Actuator/actuator.c @@ -168,11 +168,13 @@ static void actuatorTask(void* parameters) lastSysTime = thisSysTime; FlightStatusGet(&flightStatus); - MixerStatusGet(&mixerStatus); MixerSettingsGet (&mixerSettings); ActuatorDesiredGet(&desired); ActuatorCommandGet(&command); +#if defined(DIAGNOSTICS) + MixerStatusGet(&mixerStatus); +#endif ActuatorSettingsMotorsSpinWhileArmedGet(&MotorsSpinWhileArmed); ActuatorSettingsChannelMaxGet(ChannelMax); ActuatorSettingsChannelMinGet(ChannelMin); @@ -277,7 +279,9 @@ static void actuatorTask(void* parameters) ChannelMin[ct], ChannelNeutral[ct]); } +#if defined(DIAGNOSTICS) MixerStatusSet(&mixerStatus); +#endif // Store update time command.UpdateTime = 1000*dT; diff --git a/flight/Modules/Stabilization/stabilization.c b/flight/Modules/Stabilization/stabilization.c index 702020966..e57b8eb18 100644 --- a/flight/Modules/Stabilization/stabilization.c +++ b/flight/Modules/Stabilization/stabilization.c @@ -153,9 +153,12 @@ static void stabilizationTask(void* parameters) StabilizationDesiredGet(&stabDesired); AttitudeActualGet(&attitudeActual); AttitudeRawGet(&attitudeRaw); - RateDesiredGet(&rateDesired); SystemSettingsGet(&systemSettings); +#if defined(DIAGNOSTICS) + RateDesiredGet(&rateDesired); +#endif + #if defined(PIOS_QUATERNION_STABILIZATION) // Quaternion calculation of error in each axis. Uses more memory. float rpy_desired[3]; @@ -213,7 +216,9 @@ static void stabilizationTask(void* parameters) } uint8_t shouldUpdate = 1; +#if defined(DIAGNOSTICS) RateDesiredSet(&rateDesired); +#endif ActuatorDesiredGet(&actuatorDesired); //Calculate desired command for(int8_t ct=0; ct< MAX_AXES; ct++) diff --git a/flight/OpenPilot/System/inc/FreeRTOSConfig.h b/flight/OpenPilot/System/inc/FreeRTOSConfig.h index 3e770e421..044b7a9e1 100644 --- a/flight/OpenPilot/System/inc/FreeRTOSConfig.h +++ b/flight/OpenPilot/System/inc/FreeRTOSConfig.h @@ -72,7 +72,9 @@ NVIC value of 255. */ #define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15 /* Enable run time stats collection */ -#if defined(DEBUG) +#if defined(DIAGNOSTICS) +#define configCHECK_FOR_STACK_OVERFLOW 2 + #define configGENERATE_RUN_TIME_STATS 1 #define INCLUDE_uxTaskGetRunTime 1 #define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS()\ @@ -81,6 +83,8 @@ do {\ (*(unsigned long *)0xe0001000) |= 1; /* DWT_CTRL |= DWT_CYCCNT_ENA */\ } while(0) #define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004)/* DWT_CYCCNT */ +#else +#define configCHECK_FOR_STACK_OVERFLOW 1 #endif diff --git a/flight/OpenPilot/System/taskmonitor.c b/flight/OpenPilot/System/taskmonitor.c index d7fa3fde4..dcd08945c 100644 --- a/flight/OpenPilot/System/taskmonitor.c +++ b/flight/OpenPilot/System/taskmonitor.c @@ -46,7 +46,10 @@ int32_t TaskMonitorInitialize(void) { lock = xSemaphoreCreateRecursiveMutex(); memset(handles, 0, sizeof(xTaskHandle)*TASKINFO_RUNNING_NUMELEM); + lastMonitorTime = 0; +#if defined(DIAGNOSTICS) lastMonitorTime = portGET_RUN_TIME_COUNTER_VALUE(); +#endif return 0; } @@ -91,6 +94,7 @@ int32_t TaskMonitorRemove(TaskInfoRunningElem task) */ void TaskMonitorUpdateAll(void) { +#if defined(DIAGNOSTICS) TaskInfoData data; int n; @@ -142,4 +146,5 @@ void TaskMonitorUpdateAll(void) // Done xSemaphoreGiveRecursive(lock); +#endif } From 5044ea36def865ca927fdd4b881017a585a7730f Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sat, 18 Jun 2011 11:59:02 -0500 Subject: [PATCH 004/145] Start initializing objects in the modules that consume them. Shouldn't affect OP yet but not tested. --- flight/CopterControl/Makefile | 4 ++- flight/CopterControl/System/pios_board.c | 1 - flight/Modules/AHRSComms/ahrs_comms.c | 4 +++ flight/Modules/Actuator/actuator.c | 7 +++++ flight/Modules/Altitude/altitude.c | 6 ++++ flight/Modules/Attitude/attitude.c | 4 +++ flight/Modules/Battery/battery.c | 3 ++ flight/Modules/FirmwareIAP/firmwareiap.c | 3 ++ flight/Modules/GPS/GPS.c | 4 +++ flight/Modules/ManualControl/manualcontrol.c | 7 +++++ flight/Modules/Stabilization/stabilization.c | 11 +++---- flight/Modules/System/systemmod.c | 29 ++++++++++++++----- flight/Modules/Telemetry/telemetry.c | 4 +++ flight/OpenPilot/System/alarms.c | 12 ++++---- .../OpenPilotOSX.xcodeproj/project.pbxproj | 4 +-- 15 files changed, 79 insertions(+), 24 deletions(-) diff --git a/flight/CopterControl/Makefile b/flight/CopterControl/Makefile index 91d0cd12b..e05420f90 100644 --- a/flight/CopterControl/Makefile +++ b/flight/CopterControl/Makefile @@ -65,7 +65,9 @@ endif FLASH_TOOL = OPENOCD # List of modules to include -MODULES = Telemetry Attitude Stabilization Actuator ManualControl FirmwareIAP +MODULES = Attitude Stabilization Actuator ManualControl FirmwareIAP +# Telemetry must be last to grab the optional modules +MODULES += Telemetry # Paths OPSYSTEM = ./System diff --git a/flight/CopterControl/System/pios_board.c b/flight/CopterControl/System/pios_board.c index 53dd5b505..dcf4c7ddc 100644 --- a/flight/CopterControl/System/pios_board.c +++ b/flight/CopterControl/System/pios_board.c @@ -694,7 +694,6 @@ void PIOS_Board_Init(void) { /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); - UAVObjectsInitializeAll(); /* Initialize the alarms library */ AlarmsInitialize(); diff --git a/flight/Modules/AHRSComms/ahrs_comms.c b/flight/Modules/AHRSComms/ahrs_comms.c index 76abb1a40..7c1d6a16f 100644 --- a/flight/Modules/AHRSComms/ahrs_comms.c +++ b/flight/Modules/AHRSComms/ahrs_comms.c @@ -72,6 +72,10 @@ static void ahrscommsTask(void *parameters); int32_t AHRSCommsInitialize(void) { // Start main task + AHRSStatusInitialize(); + AHRSCalibrationInitialize(); + AttitudeRawInitialize(); + xTaskCreate(ahrscommsTask, (signed char *)"AHRSComms", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle); TaskMonitorAdd(TASKINFO_RUNNING_AHRSCOMMS, taskHandle); PIOS_WDG_RegisterFlag(PIOS_WDG_AHRS); diff --git a/flight/Modules/Actuator/actuator.c b/flight/Modules/Actuator/actuator.c index c610b842c..d1f1794b7 100644 --- a/flight/Modules/Actuator/actuator.c +++ b/flight/Modules/Actuator/actuator.c @@ -95,6 +95,13 @@ int32_t ActuatorInitialize() // Create object queue queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); + ActuatorSettingsInitialize(); + ActuatorDesiredInitialize(); + MixerSettingsInitialize(); +#if defined(DIAGNOSTICS) + MixerStatusInitialize(); +#endif + // Listen for ExampleObject1 updates ActuatorDesiredConnectQueue(queue); diff --git a/flight/Modules/Altitude/altitude.c b/flight/Modules/Altitude/altitude.c index 705eb061d..de4e90b84 100644 --- a/flight/Modules/Altitude/altitude.c +++ b/flight/Modules/Altitude/altitude.c @@ -68,6 +68,12 @@ static void altitudeTask(void *parameters); */ int32_t AltitudeInitialize() { + + BaroAltitudeInitialize(); +#if defined(PIOS_INCLUDE_HCSR04) + SonarAltitudeInitialze(); +#endif + // Start main task xTaskCreate(altitudeTask, (signed char *)"Altitude", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle); TaskMonitorAdd(TASKINFO_RUNNING_ALTITUDE, taskHandle); diff --git a/flight/Modules/Attitude/attitude.c b/flight/Modules/Attitude/attitude.c index d05ae1152..29c6dc2de 100644 --- a/flight/Modules/Attitude/attitude.c +++ b/flight/Modules/Attitude/attitude.c @@ -96,6 +96,10 @@ static bool zero_during_arming = false; */ int32_t AttitudeInitialize(void) { + AttitudeActualInitialize(); + AttitudeRawInitialize(); + AttitudeSettingsInitialize(); + // Initialize quaternion AttitudeActualData attitude; AttitudeActualGet(&attitude); diff --git a/flight/Modules/Battery/battery.c b/flight/Modules/Battery/battery.c index eabb5f91c..5f9c988e1 100644 --- a/flight/Modules/Battery/battery.c +++ b/flight/Modules/Battery/battery.c @@ -77,6 +77,9 @@ static void onTimer(UAVObjEvent* ev); */ int32_t BatteryInitialize(void) { + BatteryStateInitialze(); + BatterySettingsInitialize(); + static UAVObjEvent ev; memset(&ev,0,sizeof(UAVObjEvent)); diff --git a/flight/Modules/FirmwareIAP/firmwareiap.c b/flight/Modules/FirmwareIAP/firmwareiap.c index 395ed960f..e44025590 100644 --- a/flight/Modules/FirmwareIAP/firmwareiap.c +++ b/flight/Modules/FirmwareIAP/firmwareiap.c @@ -91,6 +91,9 @@ static void resetTask(UAVObjEvent *); int32_t FirmwareIAPInitialize() { + + FirmwareIAPObjInitialize(); + const struct pios_board_info * bdinfo = &pios_board_info_blob; data.BoardType= bdinfo->board_type; diff --git a/flight/Modules/GPS/GPS.c b/flight/Modules/GPS/GPS.c index 7d5ce8e5b..b4e77b5f3 100644 --- a/flight/Modules/GPS/GPS.c +++ b/flight/Modules/GPS/GPS.c @@ -111,6 +111,10 @@ static uint32_t numParsingErrors; int32_t GPSInitialize(void) { + GPSPositionInitialize(); + GPSTimeInitialize(); + HomeLocationInitialize(); + signed portBASE_TYPE xReturn; // TODO: Get gps settings object diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c index 8b972bc6e..214241874 100644 --- a/flight/Modules/ManualControl/manualcontrol.c +++ b/flight/Modules/ManualControl/manualcontrol.c @@ -149,6 +149,13 @@ int32_t ManualControlInitialize() /* Check the assumptions about uavobject enum's are correct */ if(!assumptions) return -1; + + AccessoryDesiredInitialize(); + ManualControlSettingsInitialize(); + ManualControlCommandInitialize(); + FlightStatusInitialize(); + StabilizationDesiredInitialize(); + // Start main task xTaskCreate(manualControlTask, (signed char *)"ManualControl", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle); TaskMonitorAdd(TASKINFO_RUNNING_MANUALCONTROL, taskHandle); diff --git a/flight/Modules/Stabilization/stabilization.c b/flight/Modules/Stabilization/stabilization.c index e57b8eb18..52cf23609 100644 --- a/flight/Modules/Stabilization/stabilization.c +++ b/flight/Modules/Stabilization/stabilization.c @@ -40,8 +40,6 @@ #include "attitudeactual.h" #include "attituderaw.h" #include "flightstatus.h" -#include "systemsettings.h" -#include "ahrssettings.h" #include "manualcontrol.h" // Just to get a macro #include "CoordinateConversions.h" @@ -92,7 +90,12 @@ static void SettingsUpdatedCb(UAVObjEvent * ev); int32_t StabilizationInitialize() { // Initialize variables - + StabilizationSettingsInitialize(); + ActuatorDesiredInitialize(); +#if defined(DIAGNOSTICS) + RateDesiredInitialize(); +#endif + // Create object queue queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); @@ -125,7 +128,6 @@ static void stabilizationTask(void* parameters) RateDesiredData rateDesired; AttitudeActualData attitudeActual; AttitudeRawData attitudeRaw; - SystemSettingsData systemSettings; FlightStatusData flightStatus; SettingsUpdatedCb((UAVObjEvent *) NULL); @@ -153,7 +155,6 @@ static void stabilizationTask(void* parameters) StabilizationDesiredGet(&stabDesired); AttitudeActualGet(&attitudeActual); AttitudeRawGet(&attitudeRaw); - SystemSettingsGet(&systemSettings); #if defined(DIAGNOSTICS) RateDesiredGet(&rateDesired); diff --git a/flight/Modules/System/systemmod.c b/flight/Modules/System/systemmod.c index c36160a1e..8fc89bbe0 100644 --- a/flight/Modules/System/systemmod.c +++ b/flight/Modules/System/systemmod.c @@ -43,7 +43,9 @@ #include "objectpersistence.h" #include "flightstatus.h" #include "systemstats.h" +#include "systemsettings.h" #include "i2cstats.h" +#include "taskinfo.h" #include "watchdogstatus.h" #include "taskmonitor.h" #include "pios_config.h" @@ -78,11 +80,12 @@ static int32_t stackOverflow; // Private functions static void objectUpdatedCb(UAVObjEvent * ev); static void updateStats(); -static void updateI2Cstats(); -static void updateWDGstats(); static void updateSystemAlarms(); static void systemTask(void *parameters); - +#if defined(DIAGNOSTICS) +static void updateI2Cstats(); +static void updateWDGstats(); +#endif /** * Initialise the module, called on startup. * \returns 0 on success or -1 if initialisation failed @@ -106,6 +109,16 @@ static void systemTask(void *parameters) // System initialization OpenPilotInit(); + // Must registers objects here for system thread because ObjectManager started in OpenPilotInit + SystemSettingsInitialize(); + SystemStatsInitialize(); + ObjectPersistenceInitialize(); +#if defined(DIAGNOSTICS) + TaskInfoInitialize(); + I2CStatsInitialize(); + WatchdogStatusInitialize(); +#endif + // Register task TaskMonitorAdd(TASKINFO_RUNNING_SYSTEM, systemTaskHandle); @@ -124,9 +137,10 @@ static void systemTask(void *parameters) // Update the system alarms updateSystemAlarms(); +#if defined(DIAGNOSTICS) updateI2Cstats(); updateWDGstats(); - +#endif // Update the task status object TaskMonitorUpdateAll(); @@ -228,9 +242,7 @@ static void objectUpdatedCb(UAVObjEvent * ev) /** * Called periodically to update the I2C statistics */ -#if defined(ARCH_POSIX) || defined(ARCH_WIN32) -static void updateI2Cstats() {} //Posix and win32 don't have I2C -#else +#if defined(DIAGNOSTICS) static void updateI2Cstats() { #if defined(PIOS_INCLUDE_I2C) @@ -250,7 +262,6 @@ static void updateI2Cstats() I2CStatsSet(&i2cStats); #endif } -#endif static void updateWDGstats() { @@ -259,6 +270,8 @@ static void updateWDGstats() watchdogStatus.ActiveFlags = PIOS_WDG_GetActiveFlags(); WatchdogStatusSet(&watchdogStatus); } +#endif + /** * Called periodically to update the system stats diff --git a/flight/Modules/Telemetry/telemetry.c b/flight/Modules/Telemetry/telemetry.c index 07c7dc4dc..9e965e460 100644 --- a/flight/Modules/Telemetry/telemetry.c +++ b/flight/Modules/Telemetry/telemetry.c @@ -89,6 +89,10 @@ int32_t TelemetryInitialize(void) { UAVObjEvent ev; + FlightTelemetryStatsInitialize(); + GCSTelemetryStatsInitialize(); + TelemetrySettingsInitialize(); + // Initialize vars timeOfLastObjectUpdate = 0; diff --git a/flight/OpenPilot/System/alarms.c b/flight/OpenPilot/System/alarms.c index e899be779..e61c7c1ea 100644 --- a/flight/OpenPilot/System/alarms.c +++ b/flight/OpenPilot/System/alarms.c @@ -41,14 +41,12 @@ static xSemaphoreHandle lock; static int32_t hasSeverity(SystemAlarmsAlarmOptions severity); /** - * Initialize the alarms library + * Initialize the alarms library */ int32_t AlarmsInitialize(void) { + SystemAlarmsInitialize(); lock = xSemaphoreCreateRecursiveMutex(); - //do not change the default states of the alarms, let the init code generated by the uavobjectgenerator handle that - //AlarmsClearAll(); - //AlarmsDefaultAll(); return 0; } @@ -56,7 +54,7 @@ int32_t AlarmsInitialize(void) * Set an alarm * @param alarm The system alarm to be modified * @param severity The alarm severity - * @return 0 if success, -1 if an error + * @return 0 if success, -1 if an error */ int32_t AlarmsSet(SystemAlarmsAlarmElem alarm, SystemAlarmsAlarmOptions severity) { @@ -151,7 +149,7 @@ void AlarmsClearAll() /** * Check if there are any alarms with the given or higher severity - * @return 0 if no alarms are found, 1 if at least one alarm is found + * @return 0 if no alarms are found, 1 if at least one alarm is found */ int32_t AlarmsHasWarnings() { @@ -208,5 +206,5 @@ static int32_t hasSeverity(SystemAlarmsAlarmOptions severity) /** * @} * @} - */ + */ diff --git a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj index 452547f2e..009d9cfb9 100644 --- a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj +++ b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj @@ -2677,7 +2677,6 @@ 65C35E7912EFB2F3004811C2 /* watchdogstatus.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = watchdogstatus.xml; sourceTree = ""; }; 65C35E9E12F0A834004811C2 /* uavobjecttemplate.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = uavobjecttemplate.c; sourceTree = ""; }; 65C35E9F12F0A834004811C2 /* uavobjectsinittemplate.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = uavobjectsinittemplate.c; sourceTree = ""; }; - 65C35EA012F0A834004811C2 /* uavobjectsinit_cc.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = uavobjectsinit_cc.c; sourceTree = ""; }; 65C35EA112F0A834004811C2 /* uavobjectmanager.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = uavobjectmanager.c; sourceTree = ""; }; 65C35EA312F0A834004811C2 /* eventdispatcher.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = eventdispatcher.h; sourceTree = ""; }; 65C35EA412F0A834004811C2 /* uavobjectmanager.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = uavobjectmanager.h; sourceTree = ""; }; @@ -2689,6 +2688,7 @@ 65C35F6812F0DC2D004811C2 /* attitude.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = attitude.h; sourceTree = ""; }; 65C9903C13A871B90082BD60 /* camerastab.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = camerastab.c; sourceTree = ""; }; 65C9903E13A871B90082BD60 /* camerastab.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = camerastab.h; sourceTree = ""; }; + 65C9908E13AC5D8D0082BD60 /* uavobjectsinit_linker.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = uavobjectsinit_linker.c; sourceTree = ""; }; 65D2CA841248F9A400B1E7D6 /* mixersettings.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = mixersettings.xml; sourceTree = ""; }; 65D2CA851248F9A400B1E7D6 /* mixerstatus.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = mixerstatus.xml; sourceTree = ""; }; 65E410AE12F65AEA00725888 /* attitudesettings.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = attitudesettings.xml; sourceTree = ""; }; @@ -3515,9 +3515,9 @@ 650D8E6A12DFE17500D05CC9 /* UAVObjects */ = { isa = PBXGroup; children = ( + 65C9908E13AC5D8D0082BD60 /* uavobjectsinit_linker.c */, 65C35E9E12F0A834004811C2 /* uavobjecttemplate.c */, 65C35E9F12F0A834004811C2 /* uavobjectsinittemplate.c */, - 65C35EA012F0A834004811C2 /* uavobjectsinit_cc.c */, 65C35EA112F0A834004811C2 /* uavobjectmanager.c */, 65C35EA212F0A834004811C2 /* inc */, 65C35EA812F0A834004811C2 /* eventdispatcher.c */, From 2218ff6fe9997253b94500cb2e53b3d8d35c656f Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sat, 18 Jun 2011 14:21:14 -0500 Subject: [PATCH 005/145] Change to object template so when they are already registered it doesn't initialize them a second time (which causes the handle to get lost) --- flight/UAVObjects/uavobjecttemplate.c | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/flight/UAVObjects/uavobjecttemplate.c b/flight/UAVObjects/uavobjecttemplate.c index 3fd1517eb..f5375c9e6 100644 --- a/flight/UAVObjects/uavobjecttemplate.c +++ b/flight/UAVObjects/uavobjecttemplate.c @@ -45,10 +45,14 @@ static UAVObjHandle handle; /** * Initialize object. * \return 0 Success - * \return -1 Failure + * \return -1 Failure to initialize or -2 for already initialized */ int32_t $(NAME)Initialize(void) { + // Don't set the handle to null if already registered + if(UAVObjGetByID($(NAMEUC)_OBJID) != NULL) + return -2; + // Register object with the object manager handle = UAVObjRegister($(NAMEUC)_OBJID, $(NAMEUC)_NAME, $(NAMEUC)_METANAME, 0, $(NAMEUC)_ISSINGLEINST, $(NAMEUC)_ISSETTINGS, $(NAMEUC)_NUMBYTES, &$(NAME)SetDefaults); From 724016ffb049c0c5f2379566d645e452082f568a Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sat, 18 Jun 2011 14:32:09 -0500 Subject: [PATCH 006/145] When erasing flash at the end start flashing LED to indicate it is done --- flight/CopterControl/System/coptercontrol.c | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/flight/CopterControl/System/coptercontrol.c b/flight/CopterControl/System/coptercontrol.c index 060fb3646..5fdf7bae6 100644 --- a/flight/CopterControl/System/coptercontrol.c +++ b/flight/CopterControl/System/coptercontrol.c @@ -92,7 +92,10 @@ void OpenPilotInit() #ifdef ERASE_FLASH PIOS_Flash_W25X_EraseChip(); - while(TRUE){}; + while(TRUE){ + PIOS_LED_Toggle(LED1); + PIOS_DELAY_WaitmS(50); + }; #endif /* Initialize modules */ From 17fa227d1f8c832fe1482da6d91a71ae6248b7da Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sun, 19 Jun 2011 16:51:54 -0500 Subject: [PATCH 007/145] Fixed typo, thanks Os --- flight/Modules/CameraStab/inc/camerastab.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/Modules/CameraStab/inc/camerastab.h b/flight/Modules/CameraStab/inc/camerastab.h index bc2515696..5d7bf9d06 100644 --- a/flight/Modules/CameraStab/inc/camerastab.h +++ b/flight/Modules/CameraStab/inc/camerastab.h @@ -32,7 +32,7 @@ #include "openpilot.h" -int32_t BatteryInitialize(void); +int32_t CameraStabInitialize(void); #endif // BATTERY_H From 493fcaef2a8a41af2380963b276a72e69eb6737e Mon Sep 17 00:00:00 2001 From: James Cotton Date: Thu, 23 Jun 2011 00:46:41 -0500 Subject: [PATCH 008/145] OP-193 OP-511: Add settings to scale the camera input output range --- flight/CopterControl/Makefile | 5 +- flight/Modules/CameraStab/camerastab.c | 60 +++++++++++++------ .../OpenPilotOSX.xcodeproj/project.pbxproj | 2 + .../src/plugins/uavobjects/uavobjects.pro | 2 + 4 files changed, 49 insertions(+), 20 deletions(-) diff --git a/flight/CopterControl/Makefile b/flight/CopterControl/Makefile index e05420f90..8a612aab9 100644 --- a/flight/CopterControl/Makefile +++ b/flight/CopterControl/Makefile @@ -168,6 +168,7 @@ SRC += $(OPUAVSYNTHDIR)/manualcontrolsettings.c SRC += $(OPUAVSYNTHDIR)/mixersettings.c SRC += $(OPUAVSYNTHDIR)/firmwareiapobj.c SRC += $(OPUAVSYNTHDIR)/attitudesettings.c +SRC += $(OPUAVSYNTHDIR)/camerastabsettings.c ifeq ($(DIAGNOISTCS),YES) SRC += $(OPUAVSYNTHDIR)/taskinfo.c @@ -438,7 +439,7 @@ CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) -I. #CFLAGS += -fno-cprop-registers -fno-defer-pop -fno-guess-branch-probability -fno-section-anchors #CFLAGS += -fno-if-conversion -fno-if-conversion2 -fno-ipa-pure-const -fno-ipa-reference -fno-merge-constants -#CFLAGS += -fno-split-wide-types -fno-tree-ccp -fno-tree-ch -fno-tree-copy-prop -fno-tree-copyrename +#CFLAGS += -fno-split-wide-types -fno-tree-ccp -fno-tree-ch -fno-tree-copy-prop -fno-tree-copyrename #CFLAGS += -fno-tree-dce -fno-tree-dominator-opts -fno-tree-dse -fno-tree-fre -fno-tree-sink -fno-tree-sra #CFLAGS += -fno-tree-ter #CFLAGS += -g$(DEBUGF) -DDEBUG @@ -529,7 +530,7 @@ ${OUTDIR}/InitMods.c: Makefile @echo ${quote}}${quote} >> ${OUTDIR}/InitMods.c # Generate code for PyMite -#$(OUTDIR)/pmlib_img.c $(OUTDIR)/pmlib_nat.c $(OUTDIR)/pmlibusr_img.c $(OUTDIR)/pmlibusr_nat.c $(OUTDIR)/pmfeatures.h: $(wildcard $(PYMITELIB)/*.py) $(wildcard $(PYMITEPLAT)/*.py) $(wildcard $(FLIGHTPLANLIB)/*.py) $(wildcard $(FLIGHTPLANS)/*.py) +#$(OUTDIR)/pmlib_img.c $(OUTDIR)/pmlib_nat.c $(OUTDIR)/pmlibusr_img.c $(OUTDIR)/pmlibusr_nat.c $(OUTDIR)/pmfeatures.h: $(wildcard $(PYMITELIB)/*.py) $(wildcard $(PYMITEPLAT)/*.py) $(wildcard $(FLIGHTPLANLIB)/*.py) $(wildcard $(FLIGHTPLANS)/*.py) # @echo $(MSG_PYMITEINIT) $(call toprel, $@) # @$(PYTHON) $(PYMITETOOLS)/pmImgCreator.py -f $(PYMITEPLAT)/pmfeatures.py -c -s --memspace=flash -o $(OUTDIR)/pmlib_img.c --native-file=$(OUTDIR)/pmlib_nat.c $(PYMITELIB)/list.py $(PYMITELIB)/dict.py $(PYMITELIB)/__bi.py $(PYMITELIB)/sys.py $(PYMITELIB)/string.py $(wildcard $(FLIGHTPLANLIB)/*.py) # @$(PYTHON) $(PYMITETOOLS)/pmGenPmFeatures.py $(PYMITEPLAT)/pmfeatures.py > $(OUTDIR)/pmfeatures.h diff --git a/flight/Modules/CameraStab/camerastab.c b/flight/Modules/CameraStab/camerastab.c index c82a14759..4682a7ed0 100644 --- a/flight/Modules/CameraStab/camerastab.c +++ b/flight/Modules/CameraStab/camerastab.c @@ -49,11 +49,12 @@ #include "accessorydesired.h" #include "attitudeactual.h" +#include "camerastabsettings.h" // // Configuration // -#define SAMPLE_PERIOD_MS 50 +#define SAMPLE_PERIOD_MS 10 // Private types @@ -73,6 +74,8 @@ int32_t CameraStabInitialize(void) ev.instId = 0; ev.event = 0; + CameraStabSettingsInitialize(); + EventPeriodicCallbackCreate(&ev, attitudeUpdated, SAMPLE_PERIOD_MS / portTICK_RATE_MS); return 0; @@ -85,25 +88,46 @@ static void attitudeUpdated(UAVObjEvent* ev) float attitude; AccessoryDesiredData accessory; - float rollConst = 1.0/35.0; - float pitchConst = 1.0/35.0; - float yawConst = 1.0/35.0; - - - AttitudeActualRollGet(&attitude); - accessory.AccessoryVal = attitude * rollConst; - if(AccessoryDesiredInstSet(0, &accessory) != 0) - AccessoryDesiredCreateInstance(); - AttitudeActualPitchGet(&attitude); - accessory.AccessoryVal = attitude * pitchConst; - if(AccessoryDesiredInstSet(1, &accessory) != 0) - AccessoryDesiredCreateInstance(); + CameraStabSettingsData cameraStab; + CameraStabSettingsGet(&cameraStab); - AttitudeActualYawGet(&attitude); - accessory.AccessoryVal = attitude * yawConst; - if(AccessoryDesiredInstSet(2, &accessory) != 0) - AccessoryDesiredCreateInstance(); + // Read any input channels + float inputs[3] = {0,0,0}; + if(cameraStab.Inputs[CAMERASTABSETTINGS_INPUTS_ROLL] != CAMERASTABSETTINGS_INPUTS_NONE) { + if(AccessoryDesiredInstGet(cameraStab.Inputs[CAMERASTABSETTINGS_INPUTS_ROLL] - CAMERASTABSETTINGS_INPUTS_ACCESSORY0, &accessory) != 0) + inputs[0] = accessory.AccessoryVal * cameraStab.InputRange[CAMERASTABSETTINGS_INPUTRANGE_ROLL]; + } + if(cameraStab.Inputs[CAMERASTABSETTINGS_INPUTS_PITCH] != CAMERASTABSETTINGS_INPUTS_NONE) { + if(AccessoryDesiredInstGet(cameraStab.Inputs[CAMERASTABSETTINGS_INPUTS_PITCH] - CAMERASTABSETTINGS_INPUTS_ACCESSORY0, &accessory) != 0) + inputs[1] = accessory.AccessoryVal * cameraStab.InputRange[CAMERASTABSETTINGS_INPUTRANGE_PITCH]; + } + if(cameraStab.Inputs[CAMERASTABSETTINGS_INPUTS_YAW] != CAMERASTABSETTINGS_INPUTS_NONE) { + if(AccessoryDesiredInstGet(cameraStab.Inputs[CAMERASTABSETTINGS_INPUTS_YAW] - CAMERASTABSETTINGS_INPUTS_ACCESSORY0, &accessory) != 0) + inputs[2] = accessory.AccessoryVal * cameraStab.InputRange[CAMERASTABSETTINGS_INPUTRANGE_YAW]; + } + + // Set output channels + if(cameraStab.Outputs[CAMERASTABSETTINGS_OUTPUTS_ROLL] != CAMERASTABSETTINGS_OUTPUTS_NONE) { + AttitudeActualRollGet(&attitude); + accessory.AccessoryVal = (attitude + inputs[0]) / cameraStab.OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_ROLL]; + if(AccessoryDesiredInstSet(cameraStab.Outputs[CAMERASTABSETTINGS_OUTPUTS_ROLL] - CAMERASTABSETTINGS_OUTPUTS_ACCESSORY0, &accessory) != 0) + AccessoryDesiredCreateInstance(); + } + + if(cameraStab.Outputs[CAMERASTABSETTINGS_OUTPUTS_PITCH] != CAMERASTABSETTINGS_OUTPUTS_NONE) { + AttitudeActualPitchGet(&attitude); + accessory.AccessoryVal = (attitude + inputs[1]) / cameraStab.OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_PITCH]; + if(AccessoryDesiredInstSet(cameraStab.Outputs[CAMERASTABSETTINGS_OUTPUTS_PITCH] - CAMERASTABSETTINGS_OUTPUTS_ACCESSORY0, &accessory) != 0) + AccessoryDesiredCreateInstance(); + } + + if(cameraStab.Outputs[CAMERASTABSETTINGS_OUTPUTS_YAW] != CAMERASTABSETTINGS_OUTPUTS_NONE) { + AttitudeActualYawGet(&attitude); + accessory.AccessoryVal = (attitude + inputs[2]) / cameraStab.OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_YAW]; + if(AccessoryDesiredInstSet(cameraStab.Outputs[CAMERASTABSETTINGS_OUTPUTS_YAW] - CAMERASTABSETTINGS_OUTPUTS_ACCESSORY0, &accessory) != 0) + AccessoryDesiredCreateInstance(); + } } /** diff --git a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj index 009d9cfb9..a5503a27f 100644 --- a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj +++ b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj @@ -85,6 +85,7 @@ 6549E0D21279B3C800C5476F /* fifo_buffer.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = fifo_buffer.c; sourceTree = ""; }; 6549E0D31279B3CF00C5476F /* fifo_buffer.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = fifo_buffer.h; sourceTree = ""; }; 655268BC121FBD2900410C6E /* ahrscalibration.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = ahrscalibration.xml; sourceTree = ""; }; + 655B1A8E13B2FC0900B0E48D /* camerastabsettings.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = camerastabsettings.xml; sourceTree = ""; }; 656268C612DC1923007B0A0F /* nedaccel.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = nedaccel.xml; sourceTree = ""; }; 65632DF51251650300469B77 /* pios_board.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_board.h; sourceTree = ""; }; 65632DF61251650300469B77 /* STM32103CB_AHRS.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = STM32103CB_AHRS.h; sourceTree = ""; }; @@ -7434,6 +7435,7 @@ 65E410AE12F65AEA00725888 /* attitudesettings.xml */, 65C35E5912EFB2F3004811C2 /* baroaltitude.xml */, 65C35E5A12EFB2F3004811C2 /* batterysettings.xml */, + 655B1A8E13B2FC0900B0E48D /* camerastabsettings.xml */, 652C8568132B632A00BFCC70 /* firmwareiapobj.xml */, 65C35E5C12EFB2F3004811C2 /* flightbatterystate.xml */, 65C35E5D12EFB2F3004811C2 /* flightplancontrol.xml */, diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index 8bd7420be..31cd6b78a 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -31,6 +31,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/ahrssettings.h \ $$UAVOBJECT_SYNTHETICS/gcstelemetrystats.h \ $$UAVOBJECT_SYNTHETICS/attituderaw.h \ + $$UAVOBJECT_SYNTHETICS/camerastabsettings.h \ $$UAVOBJECT_SYNTHETICS/flighttelemetrystats.h \ $$UAVOBJECT_SYNTHETICS/systemstats.h \ $$UAVOBJECT_SYNTHETICS/systemalarms.h \ @@ -78,6 +79,7 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/ahrssettings.cpp \ $$UAVOBJECT_SYNTHETICS/gcstelemetrystats.cpp \ $$UAVOBJECT_SYNTHETICS/attituderaw.cpp \ + $$UAVOBJECT_SYNTHETICS/camerastabsettings.cpp \ $$UAVOBJECT_SYNTHETICS/flighttelemetrystats.cpp \ $$UAVOBJECT_SYNTHETICS/systemstats.cpp \ $$UAVOBJECT_SYNTHETICS/systemalarms.cpp \ From 368d5b649abe3526bd0a11af832e05fd2314497c Mon Sep 17 00:00:00 2001 From: James Cotton Date: Thu, 23 Jun 2011 01:47:55 -0500 Subject: [PATCH 009/145] OP-193: Output mixer should read from up to 6 accessory channels (may add more in future). --- flight/Modules/Actuator/actuator.c | 69 ++++++++++++++++-------------- 1 file changed, 37 insertions(+), 32 deletions(-) diff --git a/flight/Modules/Actuator/actuator.c b/flight/Modules/Actuator/actuator.c index d1f1794b7..ffea2eafa 100644 --- a/flight/Modules/Actuator/actuator.c +++ b/flight/Modules/Actuator/actuator.c @@ -104,15 +104,15 @@ int32_t ActuatorInitialize() // Listen for ExampleObject1 updates ActuatorDesiredConnectQueue(queue); - + // If settings change, update the output rate ActuatorSettingsConnectCallback(actuator_update_rate); - + // Start main task xTaskCreate(actuatorTask, (signed char*)"Actuator", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle); TaskMonitorAdd(TASKINFO_RUNNING_ACTUATOR, taskHandle); PIOS_WDG_RegisterFlag(PIOS_WDG_ACTUATOR); - + return 0; } @@ -158,7 +158,7 @@ static void actuatorTask(void* parameters) // Main task loop lastSysTime = xTaskGetTickCount(); while (1) - { + { PIOS_WDG_UpdateFlag(PIOS_WDG_ACTUATOR); // Wait until the ActuatorDesired object is updated, if a timeout then go to failsafe @@ -196,7 +196,7 @@ static void actuatorTask(void* parameters) nMixers ++; } } - if((nMixers < 2) && !ActuatorCommandReadOnly(dummy)) //Nothing can fly with less than two mixers. + if((nMixers < 2) && !ActuatorCommandReadOnly(dummy)) //Nothing can fly with less than two mixers. { setFailsafe(); // So that channels like PWM buzzer keep working continue; @@ -207,7 +207,7 @@ static void actuatorTask(void* parameters) bool armed = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED; bool positiveThrottle = desired.Throttle >= 0.00; bool spinWhileArmed = MotorsSpinWhileArmed == ACTUATORSETTINGS_MOTORSSPINWHILEARMED_TRUE; - + float curve1 = MixerCurve(desired.Throttle,mixerSettings.ThrottleCurve1); //The source for the secondary curve is selectable float curve2 = 0; @@ -233,11 +233,11 @@ static void actuatorTask(void* parameters) case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY5: if(AccessoryDesiredInstGet(mixerSettings.Curve2Source - MIXERSETTINGS_CURVE2SOURCE_ACCESSORY0,&accessory) == 0) curve2 = MixerCurve(accessory.AccessoryVal,mixerSettings.ThrottleCurve2); - else - curve2 = 0; + else + curve2 = 0; break; } - + for(int ct=0; ct < MAX_MIX_ACTUATORS; ct++) { if(mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_DISABLED) { @@ -246,41 +246,46 @@ static void actuatorTask(void* parameters) command.Channel[ct] = 0; continue; } - - status[ct] = ProcessMixer(ct, curve1, curve2, &mixerSettings, &desired, dT); - + + if((mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) || (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_SERVO)) + status[ct] = ProcessMixer(ct, curve1, curve2, &mixerSettings, &desired, dT); + else + status[ct] = -1; + + + // Motors have additional protection for when to be on - if(mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) { + if(mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) { // If not armed or motors aren't meant to spin all the time if( !armed || (!spinWhileArmed && !positiveThrottle)) { filterAccumulator[ct] = 0; - lastResult[ct] = 0; + lastResult[ct] = 0; status[ct] = -1; //force min throttle - } - // If armed meant to keep spinning, + } + // If armed meant to keep spinning, else if ((spinWhileArmed && !positiveThrottle) || (status[ct] < 0) ) - status[ct] = 0; + status[ct] = 0; } - + // If an accessory channel is selected for direct bypass mode // In this configuration the accessory channel is scaled and mapped // directly to output. Note: THERE IS NO SAFETY CHECK HERE FOR ARMING - // these also will not be updated in failsafe mode. I'm not sure what + // these also will not be updated in failsafe mode. I'm not sure what // the correct behavior is since it seems domain specific. I don't love // this code - if( (mixers[ct].type >= MIXERSETTINGS_MIXER1TYPE_ACCESSORY0) && - (mixers[ct].type <= MIXERSETTINGS_MIXER1TYPE_ACCESSORY2)) + if( (mixers[ct].type >= MIXERSETTINGS_MIXER1TYPE_ACCESSORY0) && + (mixers[ct].type <= MIXERSETTINGS_MIXER1TYPE_ACCESSORY5)) { if(AccessoryDesiredInstGet(mixers[ct].type - MIXERSETTINGS_MIXER1TYPE_ACCESSORY0,&accessory) == 0) status[ct] = accessory.AccessoryVal; else status[ct] = -1; } - + command.Channel[ct] = scaleChannel(status[ct], ChannelMax[ct], ChannelMin[ct], @@ -294,7 +299,7 @@ static void actuatorTask(void* parameters) command.UpdateTime = 1000*dT; if(1000*dT > command.MaxUpdateTime) command.MaxUpdateTime = 1000*dT; - + // Update output object ActuatorCommandSet(&command); // Update in case read only (eg. during servo configuration) @@ -302,7 +307,7 @@ static void actuatorTask(void* parameters) // Update servo outputs bool success = true; - + for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) { success &= set_channel(n, command.Channel[n]); @@ -311,7 +316,7 @@ static void actuatorTask(void* parameters) if(!success) { command.NumFailedUpdates++; ActuatorCommandSet(&command); - AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL); + AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL); } } @@ -465,7 +470,7 @@ static void setFailsafe() // Reset ActuatorCommand to safe values for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) { - + if(mixers[n].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) { Channel[n] = ChannelMin[n]; @@ -512,10 +517,10 @@ static bool set_channel(uint8_t mixer_channel, uint16_t value) { } #else static bool set_channel(uint8_t mixer_channel, uint16_t value) { - + ActuatorSettingsData settings; ActuatorSettingsGet(&settings); - + switch(settings.ChannelType[mixer_channel]) { case ACTUATORSETTINGS_CHANNELTYPE_PWMALARMBUZZER: { // This is for buzzers that take a PWM input @@ -567,7 +572,7 @@ static bool set_channel(uint8_t mixer_channel, uint16_t value) { PIOS_Servo_Set(settings.ChannelAddr[mixer_channel], value); return true; #if defined(PIOS_INCLUDE_I2C_ESC) - case ACTUATORSETTINGS_CHANNELTYPE_MK: + case ACTUATORSETTINGS_CHANNELTYPE_MK: return PIOS_SetMKSpeed(settings.ChannelAddr[mixer_channel],value); case ACTUATORSETTINGS_CHANNELTYPE_ASTEC4: return PIOS_SetAstec4Speed(settings.ChannelAddr[mixer_channel],value); @@ -575,10 +580,10 @@ static bool set_channel(uint8_t mixer_channel, uint16_t value) { #endif default: return false; - } - + } + return false; - + } #endif From 9aba787b021bb027c5c60a7fd483f7b23883af16 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Thu, 23 Jun 2011 03:01:37 -0500 Subject: [PATCH 010/145] OP-193: Check for instance read success was backwards --- flight/Modules/CameraStab/camerastab.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/flight/Modules/CameraStab/camerastab.c b/flight/Modules/CameraStab/camerastab.c index 4682a7ed0..710bc17c7 100644 --- a/flight/Modules/CameraStab/camerastab.c +++ b/flight/Modules/CameraStab/camerastab.c @@ -95,15 +95,15 @@ static void attitudeUpdated(UAVObjEvent* ev) // Read any input channels float inputs[3] = {0,0,0}; if(cameraStab.Inputs[CAMERASTABSETTINGS_INPUTS_ROLL] != CAMERASTABSETTINGS_INPUTS_NONE) { - if(AccessoryDesiredInstGet(cameraStab.Inputs[CAMERASTABSETTINGS_INPUTS_ROLL] - CAMERASTABSETTINGS_INPUTS_ACCESSORY0, &accessory) != 0) + if(AccessoryDesiredInstGet(cameraStab.Inputs[CAMERASTABSETTINGS_INPUTS_ROLL] - CAMERASTABSETTINGS_INPUTS_ACCESSORY0, &accessory) == 0) inputs[0] = accessory.AccessoryVal * cameraStab.InputRange[CAMERASTABSETTINGS_INPUTRANGE_ROLL]; } if(cameraStab.Inputs[CAMERASTABSETTINGS_INPUTS_PITCH] != CAMERASTABSETTINGS_INPUTS_NONE) { - if(AccessoryDesiredInstGet(cameraStab.Inputs[CAMERASTABSETTINGS_INPUTS_PITCH] - CAMERASTABSETTINGS_INPUTS_ACCESSORY0, &accessory) != 0) + if(AccessoryDesiredInstGet(cameraStab.Inputs[CAMERASTABSETTINGS_INPUTS_PITCH] - CAMERASTABSETTINGS_INPUTS_ACCESSORY0, &accessory) == 0) inputs[1] = accessory.AccessoryVal * cameraStab.InputRange[CAMERASTABSETTINGS_INPUTRANGE_PITCH]; } if(cameraStab.Inputs[CAMERASTABSETTINGS_INPUTS_YAW] != CAMERASTABSETTINGS_INPUTS_NONE) { - if(AccessoryDesiredInstGet(cameraStab.Inputs[CAMERASTABSETTINGS_INPUTS_YAW] - CAMERASTABSETTINGS_INPUTS_ACCESSORY0, &accessory) != 0) + if(AccessoryDesiredInstGet(cameraStab.Inputs[CAMERASTABSETTINGS_INPUTS_YAW] - CAMERASTABSETTINGS_INPUTS_ACCESSORY0, &accessory) == 0) inputs[2] = accessory.AccessoryVal * cameraStab.InputRange[CAMERASTABSETTINGS_INPUTRANGE_YAW]; } From a684592546759d943dba08521b13eb57d1be3a38 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Thu, 23 Jun 2011 11:20:37 -0500 Subject: [PATCH 011/145] Stopped hoarding all the UAVObject definition files. MUAHAHA --- shared/uavobjectdefinition/camerastabsettings.xml | 13 +++++++++++++ 1 file changed, 13 insertions(+) create mode 100644 shared/uavobjectdefinition/camerastabsettings.xml diff --git a/shared/uavobjectdefinition/camerastabsettings.xml b/shared/uavobjectdefinition/camerastabsettings.xml new file mode 100644 index 000000000..8d7b68cd2 --- /dev/null +++ b/shared/uavobjectdefinition/camerastabsettings.xml @@ -0,0 +1,13 @@ + + + Settings for the @ref CameraStab mmodule + + + + + + + + + + From f6312c7798f4bd095a7ba90739c6512e01a45559 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Thu, 23 Jun 2011 14:51:18 -0500 Subject: [PATCH 012/145] OP-410 OP-333: Created an Axis-lock setting for stabilization --- flight/Modules/ManualControl/manualcontrol.c | 98 ++++++++++--------- .../manualcontrolsettings.xml | 12 +-- .../stabilizationdesired.xml | 2 +- 3 files changed, 57 insertions(+), 55 deletions(-) diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c index 214241874..37a5fa063 100644 --- a/flight/Modules/ManualControl/manualcontrol.c +++ b/flight/Modules/ManualControl/manualcontrol.c @@ -147,15 +147,15 @@ static bool validInputRange(int16_t min, int16_t max, uint16_t value); int32_t ManualControlInitialize() { /* Check the assumptions about uavobject enum's are correct */ - if(!assumptions) + if(!assumptions) return -1; - + AccessoryDesiredInitialize(); ManualControlSettingsInitialize(); ManualControlCommandInitialize(); FlightStatusInitialize(); StabilizationDesiredInitialize(); - + // Start main task xTaskCreate(manualControlTask, (signed char *)"ManualControl", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle); TaskMonitorAdd(TASKINFO_RUNNING_MANUALCONTROL, taskHandle); @@ -307,14 +307,14 @@ static void manualControlTask(void *parameters) ManualControlCommandSet(&cmd); } - + } else { ManualControlCommandGet(&cmd); /* Under GCS control */ } FlightStatusGet(&flightStatus); - + // Depending on the mode update the Stabilization or Actuator objects switch(PARSE_FLIGHT_MODE(flightStatus.FlightMode)) { case FLIGHTMODE_UNDEFINED: @@ -330,11 +330,11 @@ static void manualControlTask(void *parameters) case FLIGHTMODE_GUIDANCE: // TODO: Implement break; - } + } } } -static void updateActuatorDesired(ManualControlCommandData * cmd) +static void updateActuatorDesired(ManualControlCommandData * cmd) { ActuatorDesiredData actuator; ActuatorDesiredGet(&actuator); @@ -342,17 +342,17 @@ static void updateActuatorDesired(ManualControlCommandData * cmd) actuator.Pitch = cmd->Pitch; actuator.Yaw = cmd->Yaw; actuator.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle; - ActuatorDesiredSet(&actuator); + ActuatorDesiredSet(&actuator); } static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings) { StabilizationDesiredData stabilization; StabilizationDesiredGet(&stabilization); - + StabilizationSettingsData stabSettings; StabilizationSettingsGet(&stabSettings); - + uint8_t * stab_settings; FlightStatusData flightStatus; FlightStatusGet(&flightStatus); @@ -369,30 +369,32 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon default: // Major error, this should not occur because only enter this block when one of these is true AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); - return; + return; } - + // TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = stab_settings[0]; stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = stab_settings[1]; stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = stab_settings[2]; - + stabilization.Roll = (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.RollMax : 0; // this is an invalid mode ; stabilization.Pitch = (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : - 0; // this is an invalid mode + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Pitch * stabSettings.PitchMax : + 0; // this is an invalid mode stabilization.Yaw = (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? fmod(cmd->Yaw * 180.0, 360) : - 0; // this is an invalid mode - - stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle; + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? fmod(cmd->Yaw * 180.0, 360) : + 0; // this is an invalid mode + + stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle; StabilizationDesiredSet(&stabilization); } @@ -478,11 +480,11 @@ static void setArmedIfChanged(uint8_t val) { * @param[out] cmd The structure to set the armed in * @param[in] settings Settings indicating the necessary position */ -static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings) -{ +static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings) +{ bool lowThrottle = cmd->Throttle <= 0; - + if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSDISARMED) { // In this configuration we always disarm setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); @@ -490,7 +492,7 @@ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData // Not really needed since this function not called when disconnected if (cmd->Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) return; - + // The throttle is not low, in case we where arming or disarming, abort if (!lowThrottle) { switch(armState) { @@ -507,20 +509,20 @@ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData } return; } - + // The rest of these cases throttle is low if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSARMED) { // In this configuration, we go into armed state as soon as the throttle is low, never disarm setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED); return; } - - + + // When the configuration is not "Always armed" and no "Always disarmed", // the state will not be changed when the throttle is not low static portTickType armedDisarmStart; float armingInputLevel = 0; - + // Calc channel see assumptions7 int8_t sign = ((settings->Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2) ? -1 : 1; switch ( (settings->Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 ) { @@ -528,26 +530,26 @@ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData case ARMING_CHANNEL_PITCH: armingInputLevel = sign * cmd->Pitch; break; case ARMING_CHANNEL_YAW: armingInputLevel = sign * cmd->Yaw; break; } - + bool manualArm = false; bool manualDisarm = false; - + if (armingInputLevel <= -ARMED_THRESHOLD) manualArm = true; else if (armingInputLevel >= +ARMED_THRESHOLD) manualDisarm = true; - + switch(armState) { case ARM_STATE_DISARMED: setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); - + // only allow arming if it's OK too if (manualArm && okToArm()) { armedDisarmStart = lastSysTime; armState = ARM_STATE_ARMING_MANUAL; } break; - + case ARM_STATE_ARMING_MANUAL: setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMING); @@ -556,7 +558,7 @@ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData else if (!manualArm) armState = ARM_STATE_DISARMED; break; - + case ARM_STATE_ARMED: // When we get here, the throttle is low, // we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled @@ -564,19 +566,19 @@ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData armState = ARM_STATE_DISARMING_TIMEOUT; setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED); break; - + case ARM_STATE_DISARMING_TIMEOUT: // We get here when armed while throttle low, even when the arming timeout is not enabled if ((settings->ArmedTimeout != 0) && (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings->ArmedTimeout)) armState = ARM_STATE_DISARMED; - + // Switch to disarming due to manual control when needed if (manualDisarm) { armedDisarmStart = lastSysTime; armState = ARM_STATE_DISARMING_MANUAL; } break; - + case ARM_STATE_DISARMING_MANUAL: if (manualDisarm &&(timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS)) armState = ARM_STATE_DISARMED; @@ -593,25 +595,25 @@ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * @param[in] settings The settings which indicate which position is which mode * @param[in] flightMode the value of the switch position */ -static void processFlightMode(ManualControlSettingsData * settings, float flightMode) +static void processFlightMode(ManualControlSettingsData * settings, float flightMode) { FlightStatusData flightStatus; FlightStatusGet(&flightStatus); - + uint8_t newMode; // Note here the code is ass - if (flightMode < -FLIGHT_MODE_LIMIT) + if (flightMode < -FLIGHT_MODE_LIMIT) newMode = settings->FlightModePosition[0]; - else if (flightMode > FLIGHT_MODE_LIMIT) + else if (flightMode > FLIGHT_MODE_LIMIT) newMode = settings->FlightModePosition[2]; - else - newMode = settings->FlightModePosition[1]; - + else + newMode = settings->FlightModePosition[1]; + if(flightStatus.FlightMode != newMode) { flightStatus.FlightMode = newMode; FlightStatusSet(&flightStatus); } - + } /** diff --git a/shared/uavobjectdefinition/manualcontrolsettings.xml b/shared/uavobjectdefinition/manualcontrolsettings.xml index 95423a3f5..375de146b 100644 --- a/shared/uavobjectdefinition/manualcontrolsettings.xml +++ b/shared/uavobjectdefinition/manualcontrolsettings.xml @@ -11,15 +11,15 @@ - + - - - - + + + + - + diff --git a/shared/uavobjectdefinition/stabilizationdesired.xml b/shared/uavobjectdefinition/stabilizationdesired.xml index c3bda9a2b..f8e6e173d 100644 --- a/shared/uavobjectdefinition/stabilizationdesired.xml +++ b/shared/uavobjectdefinition/stabilizationdesired.xml @@ -6,7 +6,7 @@ - + From f52f6d30c6ccef7b074c1028d9c8bc52e38f7dc8 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Fri, 24 Jun 2011 02:08:20 +0200 Subject: [PATCH 013/145] VTalk (flight): Buffers allocated dynamically with size calculated by UAVObjGenerator. Support for tx packet sizes < object size.i --- flight/Modules/Telemetry/telemetry.c | 12 +- ...objectsinit.h => uavobjectsinittemplate.h} | 2 + flight/UAVTalk/inc/uavtalk.h | 57 +------ flight/UAVTalk/inc/uavtalk_priv.h | 111 ++++++++++++ flight/UAVTalk/uavtalk.c | 160 ++++++++++-------- .../flight/uavobjectgeneratorflight.cpp | 17 +- .../flight/uavobjectgeneratorflight.h | 2 +- 7 files changed, 239 insertions(+), 122 deletions(-) rename flight/UAVObjects/inc/{uavobjectsinit.h => uavobjectsinittemplate.h} (93%) create mode 100644 flight/UAVTalk/inc/uavtalk_priv.h diff --git a/flight/Modules/Telemetry/telemetry.c b/flight/Modules/Telemetry/telemetry.c index 481641f76..3099120c7 100644 --- a/flight/Modules/Telemetry/telemetry.c +++ b/flight/Modules/Telemetry/telemetry.c @@ -103,7 +103,7 @@ int32_t TelemetryInitialize(void) updateSettings(); // Initialise UAVTalk - UAVTalkInitialize(&uavTalkCon, &transmitData); + uavTalkCon = UAVTalkInitialize(&transmitData,256); // Process all registered objects and connect queue for updates UAVObjIterate(®isterObject); @@ -222,7 +222,7 @@ static void processObjEvent(UAVObjEvent * ev) if (ev->event == EV_UPDATED || ev->event == EV_UPDATED_MANUAL) { // Send update to GCS (with retries) while (retries < MAX_RETRIES && success == -1) { - success = UAVTalkSendObject(&uavTalkCon, ev->obj, ev->instId, metadata.telemetryAcked, REQ_TIMEOUT_MS); // call blocks until ack is received or timeout + success = UAVTalkSendObject(uavTalkCon, ev->obj, ev->instId, metadata.telemetryAcked, REQ_TIMEOUT_MS); // call blocks until ack is received or timeout ++retries; } // Update stats @@ -233,7 +233,7 @@ static void processObjEvent(UAVObjEvent * ev) } else if (ev->event == EV_UPDATE_REQ) { // Request object update from GCS (with retries) while (retries < MAX_RETRIES && success == -1) { - success = UAVTalkSendObjectRequest(&uavTalkCon, ev->obj, ev->instId, REQ_TIMEOUT_MS); // call blocks until update is received or timeout + success = UAVTalkSendObjectRequest(uavTalkCon, ev->obj, ev->instId, REQ_TIMEOUT_MS); // call blocks until update is received or timeout ++retries; } // Update stats @@ -310,7 +310,7 @@ static void telemetryRxTask(void *parameters) // TODO: Currently we periodically check the buffer for data, update once the PIOS_COM is made blocking len = PIOS_COM_ReceiveBufferUsed(inputPort); for (int32_t n = 0; n < len; ++n) { - UAVTalkProcessInputStream(&uavTalkCon, PIOS_COM_ReceiveBuffer(inputPort)); + UAVTalkProcessInputStream(uavTalkCon, PIOS_COM_ReceiveBuffer(inputPort)); } vTaskDelay(5); // <- remove when blocking calls are implemented @@ -404,8 +404,8 @@ static void updateTelemetryStats() uint32_t timeNow; // Get stats - UAVTalkGetStats(&uavTalkCon, &utalkStats); - UAVTalkResetStats(&uavTalkCon); + UAVTalkGetStats(uavTalkCon, &utalkStats); + UAVTalkResetStats(uavTalkCon); // Get object data FlightTelemetryStatsGet(&flightStats); diff --git a/flight/UAVObjects/inc/uavobjectsinit.h b/flight/UAVObjects/inc/uavobjectsinittemplate.h similarity index 93% rename from flight/UAVObjects/inc/uavobjectsinit.h rename to flight/UAVObjects/inc/uavobjectsinittemplate.h index ec946ba92..ec11c47ad 100644 --- a/flight/UAVObjects/inc/uavobjectsinit.h +++ b/flight/UAVObjects/inc/uavobjectsinittemplate.h @@ -29,4 +29,6 @@ void UAVObjectsInitializeAll(); +#define UAVOBJECTS_LARGEST $(SIZECALCULATION) + #endif // UAVOBJECTSINIT_H diff --git a/flight/UAVTalk/inc/uavtalk.h b/flight/UAVTalk/inc/uavtalk.h index 6d53f04f6..71114ce74 100644 --- a/flight/UAVTalk/inc/uavtalk.h +++ b/flight/UAVTalk/inc/uavtalk.h @@ -29,24 +29,9 @@ #ifndef UAVTALK_H #define UAVTALK_H -// Public constants -#define UAVTALK_WAITFOREVER -1 -#define UAVTALK_NOWAIT 0 -#define UAVTALK_MIN_HEADER_LENGTH 8 // sync(1), type (1), size (2), object ID (4) -#define UAVTALK_MAX_HEADER_LENGTH 10 // sync(1), type (1), size (2), object ID (4), instance ID (2, not used in single objects) - -#define UAVTALK_CHECKSUM_LENGTH 1 - -#define UAVTALK_MAX_PAYLOAD_LENGTH 256 - -#define UAVTALK_MAX_PACKET_LENGTH (UAVTALK_MAX_HEADER_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH + UAVTALK_CHECKSUM_LENGTH) - - // Public types typedef int32_t (*UAVTalkOutputStream)(uint8_t* data, int32_t length); -typedef enum {UAVTALK_STATE_SYNC, UAVTALK_STATE_TYPE, UAVTALK_STATE_SIZE, UAVTALK_STATE_OBJID, UAVTALK_STATE_INSTID, UAVTALK_STATE_DATA, UAVTALK_STATE_CS} UAVTalkRxState; - typedef struct { uint32_t txBytes; uint32_t rxBytes; @@ -58,41 +43,17 @@ typedef struct { uint32_t rxErrors; } UAVTalkStats; -typedef struct { - UAVObjHandle obj; - uint8_t type; - uint16_t packet_size; - uint32_t objId; - uint16_t instId; - uint32_t length; - uint8_t cs; - int32_t rxCount; - UAVTalkRxState state; - uint16_t rxPacketLength; -} UAVTalkInputProcessor; - -typedef struct { - UAVTalkOutputStream outStream; - xSemaphoreHandle lock; - xSemaphoreHandle transLock; - xSemaphoreHandle respSema; - UAVObjHandle respObj; - uint16_t respInstId; - uint8_t rxBuffer[UAVTALK_MAX_PACKET_LENGTH]; - uint8_t txBuffer[UAVTALK_MAX_PACKET_LENGTH]; - UAVTalkStats stats; - UAVTalkInputProcessor iproc; -} UAVTalkConnection; +typedef void* UAVTalkConnection; // Public functions -int32_t UAVTalkInitialize(UAVTalkConnection *connection, UAVTalkOutputStream outputStream); -int32_t UAVTalkSetOutputStream(UAVTalkConnection *connection, UAVTalkOutputStream outputStream); -UAVTalkOutputStream UAVTalkGetOutputStream(UAVTalkConnection *connection); -int32_t UAVTalkSendObject(UAVTalkConnection *connection, UAVObjHandle obj, uint16_t instId, uint8_t acked, int32_t timeoutMs); -int32_t UAVTalkSendObjectRequest(UAVTalkConnection *connection, UAVObjHandle obj, uint16_t instId, int32_t timeoutMs); -int32_t UAVTalkProcessInputStream(UAVTalkConnection *connection, uint8_t rxbyte); -void UAVTalkGetStats(UAVTalkConnection *connection, UAVTalkStats* stats); -void UAVTalkResetStats(UAVTalkConnection *connection); +UAVTalkConnection UAVTalkInitialize(UAVTalkOutputStream outputStream, uint32_t maxPacketSize); +int32_t UAVTalkSetOutputStream(UAVTalkConnection connection, UAVTalkOutputStream outputStream); +UAVTalkOutputStream UAVTalkGetOutputStream(UAVTalkConnection connection); +int32_t UAVTalkSendObject(UAVTalkConnection connection, UAVObjHandle obj, uint16_t instId, uint8_t acked, int32_t timeoutMs); +int32_t UAVTalkSendObjectRequest(UAVTalkConnection connection, UAVObjHandle obj, uint16_t instId, int32_t timeoutMs); +int32_t UAVTalkProcessInputStream(UAVTalkConnection connection, uint8_t rxbyte); +void UAVTalkGetStats(UAVTalkConnection connection, UAVTalkStats *stats); +void UAVTalkResetStats(UAVTalkConnection connection); #endif // UAVTALK_H /** diff --git a/flight/UAVTalk/inc/uavtalk_priv.h b/flight/UAVTalk/inc/uavtalk_priv.h new file mode 100644 index 000000000..1bcf01fd9 --- /dev/null +++ b/flight/UAVTalk/inc/uavtalk_priv.h @@ -0,0 +1,111 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotSystem OpenPilot System + * @{ + * @addtogroup OpenPilotLibraries OpenPilot System Libraries + * @{ + * @file uavtalk.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Private include file of the UAVTalk library + * @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 UAVTALK_PRIV_H +#define UAVTALK_PRIV_H + +#include "uavobjectsinit.h" + +// Private types and constants +typedef struct { + uint8_t sync; + uint8_t type; + uint16_t size; + uint32_t objId; +} uavtalk_min_header; +#define UAVTALK_MIN_HEADER_LENGTH sizeof(uavtalk_min_header) + +typedef struct { + uint8_t sync; + uint8_t type; + uint16_t size; + uint32_t objId; + uint16_t instId; +} uavtalk_max_header; +#define UAVTALK_MAX_HEADER_LENGTH sizeof(uavtalk_max_header) + +typedef uint8_t uavtalk_checksum; +#define UAVTALK_CHECKSUM_LENGTH sizeof(uavtalk_checksum) +#define UAVTALK_MAX_PAYLOAD_LENGTH UAVOBJECTS_LARGEST +#define UAVTALK_MIN_PACKET_LENGTH UAVTALK_MAX_HEADER_LENGTH + UAVTALK_CHECKSUM_LENGTH +#define UAVTALK_MAX_PACKET_LENGTH UAVTALK_MIN_PACKET_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH + +typedef enum {UAVTALK_STATE_SYNC, UAVTALK_STATE_TYPE, UAVTALK_STATE_SIZE, UAVTALK_STATE_OBJID, UAVTALK_STATE_INSTID, UAVTALK_STATE_DATA, UAVTALK_STATE_CS} UAVTalkRxState; + +typedef struct { + UAVObjHandle obj; + uint8_t type; + uint16_t packet_size; + uint32_t objId; + uint16_t instId; + uint32_t length; + uint8_t cs; + int32_t rxCount; + UAVTalkRxState state; + uint16_t rxPacketLength; +} UAVTalkInputProcessor; + +typedef struct { + uint8_t canari; + UAVTalkOutputStream outStream; + xSemaphoreHandle lock; + xSemaphoreHandle transLock; + xSemaphoreHandle respSema; + UAVObjHandle respObj; + uint16_t respInstId; + UAVTalkStats stats; + UAVTalkInputProcessor iproc; + uint8_t *rxBuffer; + uint32_t txSize; + uint8_t *txBuffer; +} UAVTalkConnectionData; + +#define UAVTALK_CANARI 0xCA +#define UAVTALK_WAITFOREVER -1 +#define UAVTALK_NOWAIT 0 +#define UAVTALK_SYNC_VAL 0x3C +#define UAVTALK_TYPE_MASK 0xF8 +#define UAVTALK_TYPE_VER 0x20 +#define UAVTALK_TYPE_OBJ (UAVTALK_TYPE_VER | 0x00) +#define UAVTALK_TYPE_OBJ_REQ (UAVTALK_TYPE_VER | 0x01) +#define UAVTALK_TYPE_OBJ_ACK (UAVTALK_TYPE_VER | 0x02) +#define UAVTALK_TYPE_ACK (UAVTALK_TYPE_VER | 0x03) +#define UAVTALK_TYPE_NACK (UAVTALK_TYPE_VER | 0x04) + +//macros +#define CHECKCONHANDLE(handle,variable,failcommand) \ + variable = (UAVTalkConnectionData*) handle; \ + if (variable == NULL || variable->canari != UAVTALK_CANARI) { \ + failcommand; \ + } + +#endif // UAVTALK__PRIV_H +/** + * @} + * @} + */ diff --git a/flight/UAVTalk/uavtalk.c b/flight/UAVTalk/uavtalk.c index 36fdd9c12..236915b28 100644 --- a/flight/UAVTalk/uavtalk.c +++ b/flight/UAVTalk/uavtalk.c @@ -30,26 +30,16 @@ */ #include "openpilot.h" +#include "uavtalk_priv.h" -// Private constants -#define SYNC_VAL 0x3C -#define TYPE_MASK 0xF8 -#define TYPE_VER 0x20 -#define TYPE_OBJ (TYPE_VER | 0x00) -#define TYPE_OBJ_REQ (TYPE_VER | 0x01) -#define TYPE_OBJ_ACK (TYPE_VER | 0x02) -#define TYPE_ACK (TYPE_VER | 0x03) -#define TYPE_NACK (TYPE_VER | 0x04) - -// Private types // Private functions -static int32_t objectTransaction(UAVTalkConnection *connection, UAVObjHandle objectId, uint16_t instId, uint8_t type, int32_t timeout); -static int32_t sendObject(UAVTalkConnection *connection, UAVObjHandle obj, uint16_t instId, uint8_t type); -static int32_t sendSingleObject(UAVTalkConnection *connection, UAVObjHandle obj, uint16_t instId, uint8_t type); -static int32_t sendNack(UAVTalkConnection *connection, uint32_t objId); -static int32_t receiveObject(UAVTalkConnection *connection, uint8_t type, uint32_t objId, uint16_t instId, uint8_t* data, int32_t length); -static void updateAck(UAVTalkConnection *connection, UAVObjHandle obj, uint16_t instId); +static int32_t objectTransaction(UAVTalkConnectionData *connection, UAVObjHandle objectId, uint16_t instId, uint8_t type, int32_t timeout); +static int32_t sendObject(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId, uint8_t type); +static int32_t sendSingleObject(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId, uint8_t type); +static int32_t sendNack(UAVTalkConnectionData *connection, uint32_t objId); +static int32_t receiveObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, uint8_t* data, int32_t length); +static void updateAck(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId); /** * Initialize the UAVTalk library @@ -58,17 +48,28 @@ static void updateAck(UAVTalkConnection *connection, UAVObjHandle obj, uint16_t * \return 0 Success * \return -1 Failure */ -int32_t UAVTalkInitialize(UAVTalkConnection *connection, UAVTalkOutputStream outputStream) +UAVTalkConnection UAVTalkInitialize(UAVTalkOutputStream outputStream, uint32_t maxPacketSize) { + if (maxPacketSize<1) return 0; + // allocate object + UAVTalkConnectionData * connection = pvPortMalloc(sizeof(UAVTalkConnectionData)); + if (!connection) return 0; + connection->canari = UAVTALK_CANARI; connection->iproc.rxPacketLength = 0; connection->iproc.state = UAVTALK_STATE_SYNC; connection->outStream = outputStream; connection->lock = xSemaphoreCreateRecursiveMutex(); connection->transLock = xSemaphoreCreateRecursiveMutex(); + connection->txSize = maxPacketSize; + // allocate buffers + connection->rxBuffer = pvPortMalloc(UAVTALK_MAX_PACKET_LENGTH); + if (!connection->rxBuffer) return 0; + connection->txBuffer = pvPortMalloc(UAVTALK_MAX_PACKET_LENGTH); + if (!connection->txBuffer) return 0; vSemaphoreCreateBinary(connection->respSema); xSemaphoreTake(connection->respSema, 0); // reset to zero - UAVTalkResetStats(connection); - return 0; + UAVTalkResetStats( (UAVTalkConnection) connection ); + return (UAVTalkConnection) connection; } /** @@ -78,8 +79,12 @@ int32_t UAVTalkInitialize(UAVTalkConnection *connection, UAVTalkOutputStream out * \return 0 Success * \return -1 Failure */ -int32_t UAVTalkSetOutputStream(UAVTalkConnection *connection, UAVTalkOutputStream outputStream) +int32_t UAVTalkSetOutputStream(UAVTalkConnection connectionHandle, UAVTalkOutputStream outputStream) { + + UAVTalkConnectionData *connection; + CHECKCONHANDLE(connectionHandle,connection,return -1); + // Lock xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); @@ -98,8 +103,10 @@ int32_t UAVTalkSetOutputStream(UAVTalkConnection *connection, UAVTalkOutputStrea * \param[in] connection UAVTalkConnection to be used * @return UAVTarlkOutputStream the output stream used */ -UAVTalkOutputStream UAVTalkGetOutputStream(UAVTalkConnection *connection) +UAVTalkOutputStream UAVTalkGetOutputStream(UAVTalkConnection connectionHandle) { + UAVTalkConnectionData *connection; + CHECKCONHANDLE(connectionHandle,connection,return NULL); return connection->outStream; } @@ -108,8 +115,11 @@ UAVTalkOutputStream UAVTalkGetOutputStream(UAVTalkConnection *connection) * \param[in] connection UAVTalkConnection to be used * @param[out] statsOut Statistics counters */ -void UAVTalkGetStats(UAVTalkConnection *connection, UAVTalkStats* statsOut) +void UAVTalkGetStats(UAVTalkConnection connectionHandle, UAVTalkStats* statsOut) { + UAVTalkConnectionData *connection; + CHECKCONHANDLE(connectionHandle,connection,return ); + // Lock xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); @@ -124,8 +134,11 @@ void UAVTalkGetStats(UAVTalkConnection *connection, UAVTalkStats* statsOut) * Reset the statistics counters. * \param[in] connection UAVTalkConnection to be used */ -void UAVTalkResetStats(UAVTalkConnection *connection) +void UAVTalkResetStats(UAVTalkConnection connectionHandle) { + UAVTalkConnectionData *connection; + CHECKCONHANDLE(connectionHandle,connection,return); + // Lock xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); @@ -146,9 +159,11 @@ void UAVTalkResetStats(UAVTalkConnection *connection) * \return 0 Success * \return -1 Failure */ -int32_t UAVTalkSendObjectRequest(UAVTalkConnection *connection, UAVObjHandle obj, uint16_t instId, int32_t timeout) +int32_t UAVTalkSendObjectRequest(UAVTalkConnection connectionHandle, UAVObjHandle obj, uint16_t instId, int32_t timeout) { - return objectTransaction(connection, obj, instId, TYPE_OBJ_REQ, timeout); + UAVTalkConnectionData *connection; + CHECKCONHANDLE(connectionHandle,connection,return -1); + return objectTransaction(connection, obj, instId, UAVTALK_TYPE_OBJ_REQ, timeout); } /** @@ -161,16 +176,18 @@ int32_t UAVTalkSendObjectRequest(UAVTalkConnection *connection, UAVObjHandle obj * \return 0 Success * \return -1 Failure */ -int32_t UAVTalkSendObject(UAVTalkConnection *connection, UAVObjHandle obj, uint16_t instId, uint8_t acked, int32_t timeoutMs) +int32_t UAVTalkSendObject(UAVTalkConnection connectionHandle, UAVObjHandle obj, uint16_t instId, uint8_t acked, int32_t timeoutMs) { + UAVTalkConnectionData *connection; + CHECKCONHANDLE(connectionHandle,connection,return -1); // Send object if (acked == 1) { - return objectTransaction(connection, obj, instId, TYPE_OBJ_ACK, timeoutMs); + return objectTransaction(connection, obj, instId, UAVTALK_TYPE_OBJ_ACK, timeoutMs); } else { - return objectTransaction(connection, obj, instId, TYPE_OBJ, timeoutMs); + return objectTransaction(connection, obj, instId, UAVTALK_TYPE_OBJ, timeoutMs); } } @@ -180,18 +197,18 @@ int32_t UAVTalkSendObject(UAVTalkConnection *connection, UAVObjHandle obj, uint1 * \param[in] obj Object * \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances. * \param[in] type Transaction type - * TYPE_OBJ: send object, - * TYPE_OBJ_REQ: request object update - * TYPE_OBJ_ACK: send object with an ack + * UAVTALK_TYPE_OBJ: send object, + * UAVTALK_TYPE_OBJ_REQ: request object update + * UAVTALK_TYPE_OBJ_ACK: send object with an ack * \return 0 Success * \return -1 Failure */ -static int32_t objectTransaction(UAVTalkConnection *connection, UAVObjHandle obj, uint16_t instId, uint8_t type, int32_t timeoutMs) +static int32_t objectTransaction(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId, uint8_t type, int32_t timeoutMs) { int32_t respReceived; // Send object depending on if a response is needed - if (type == TYPE_OBJ_ACK || type == TYPE_OBJ_REQ) + if (type == UAVTALK_TYPE_OBJ_ACK || type == UAVTALK_TYPE_OBJ_REQ) { // Get transaction lock (will block if a transaction is pending) xSemaphoreTakeRecursive(connection->transLock, portMAX_DELAY); @@ -220,10 +237,10 @@ static int32_t objectTransaction(UAVTalkConnection *connection, UAVObjHandle obj return 0; } } - else if (type == TYPE_OBJ) + else if (type == UAVTALK_TYPE_OBJ) { xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); - sendObject(connection, obj, instId, TYPE_OBJ); + sendObject(connection, obj, instId, UAVTALK_TYPE_OBJ); xSemaphoreGiveRecursive(connection->lock); return 0; } @@ -240,8 +257,11 @@ static int32_t objectTransaction(UAVTalkConnection *connection, UAVObjHandle obj * \return 0 Success * \return -1 Failure */ -int32_t UAVTalkProcessInputStream(UAVTalkConnection *connection, uint8_t rxbyte) +int32_t UAVTalkProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rxbyte) { + UAVTalkConnectionData *connection; + CHECKCONHANDLE(connectionHandle,connection,return -1); + UAVTalkInputProcessor *iproc = &connection->iproc; ++connection->stats.rxBytes; @@ -252,7 +272,7 @@ int32_t UAVTalkProcessInputStream(UAVTalkConnection *connection, uint8_t rxbyte) switch (iproc->state) { case UAVTALK_STATE_SYNC: - if (rxbyte != SYNC_VAL) + if (rxbyte != UAVTALK_SYNC_VAL) break; // Initialize and update the CRC @@ -268,7 +288,7 @@ int32_t UAVTalkProcessInputStream(UAVTalkConnection *connection, uint8_t rxbyte) // update the CRC iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte); - if ((rxbyte & TYPE_MASK) != TYPE_VER) + if ((rxbyte & UAVTALK_TYPE_MASK) != UAVTALK_TYPE_VER) { iproc->state = UAVTALK_STATE_SYNC; break; @@ -322,7 +342,7 @@ int32_t UAVTalkProcessInputStream(UAVTalkConnection *connection, uint8_t rxbyte) // exist, in which case we'll send a NACK iproc->obj = UAVObjGetByID(iproc->objId); - if (iproc->obj == 0 && iproc->type != TYPE_OBJ_REQ) + if (iproc->obj == 0 && iproc->type != UAVTALK_TYPE_OBJ_REQ) { connection->stats.rxErrors++; iproc->state = UAVTALK_STATE_SYNC; @@ -330,7 +350,7 @@ int32_t UAVTalkProcessInputStream(UAVTalkConnection *connection, uint8_t rxbyte) } // Determine data length - if (iproc->type == TYPE_OBJ_REQ || iproc->type == TYPE_ACK || iproc->type == TYPE_NACK) + if (iproc->type == UAVTALK_TYPE_OBJ_REQ || iproc->type == UAVTALK_TYPE_ACK || iproc->type == UAVTALK_TYPE_NACK) iproc->length = 0; else iproc->length = UAVObjGetNumBytes(iproc->obj); @@ -449,7 +469,7 @@ int32_t UAVTalkProcessInputStream(UAVTalkConnection *connection, uint8_t rxbyte) /** * Receive an object. This function process objects received through the telemetry stream. * \param[in] connection UAVTalkConnection to be used - * \param[in] type Type of received message (TYPE_OBJ, TYPE_OBJ_REQ, TYPE_OBJ_ACK, TYPE_ACK, TYPE_NACK) + * \param[in] type Type of received message (UAVTALK_TYPE_OBJ, UAVTALK_TYPE_OBJ_REQ, UAVTALK_TYPE_OBJ_ACK, UAVTALK_TYPE_ACK, UAVTALK_TYPE_NACK) * \param[in] objId ID of the object to work on * \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances. * \param[in] data Data buffer @@ -457,7 +477,7 @@ int32_t UAVTalkProcessInputStream(UAVTalkConnection *connection, uint8_t rxbyte) * \return 0 Success * \return -1 Failure */ -static int32_t receiveObject(UAVTalkConnection *connection, uint8_t type, uint32_t objId, uint16_t instId, uint8_t* data, int32_t length) +static int32_t receiveObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, uint8_t* data, int32_t length) { UAVObjHandle obj; int32_t ret = 0; @@ -468,7 +488,7 @@ static int32_t receiveObject(UAVTalkConnection *connection, uint8_t type, uint32 // Process message type switch (type) { - case TYPE_OBJ: + case UAVTALK_TYPE_OBJ: // All instances, not allowed for OBJ messages if (instId != UAVOBJ_ALL_INSTANCES) { @@ -482,7 +502,7 @@ static int32_t receiveObject(UAVTalkConnection *connection, uint8_t type, uint32 ret = -1; } break; - case TYPE_OBJ_ACK: + case UAVTALK_TYPE_OBJ_ACK: // All instances, not allowed for OBJ_ACK messages if (instId != UAVOBJ_ALL_INSTANCES) { @@ -490,7 +510,7 @@ static int32_t receiveObject(UAVTalkConnection *connection, uint8_t type, uint32 if ( UAVObjUnpack(obj, instId, data) == 0 ) { // Transmit ACK - sendObject(connection, obj, instId, TYPE_ACK); + sendObject(connection, obj, instId, UAVTALK_TYPE_ACK); } else { @@ -502,17 +522,17 @@ static int32_t receiveObject(UAVTalkConnection *connection, uint8_t type, uint32 ret = -1; } break; - case TYPE_OBJ_REQ: + case UAVTALK_TYPE_OBJ_REQ: // Send requested object if message is of type OBJ_REQ if (obj == 0) sendNack(connection, objId); else - sendObject(connection, obj, instId, TYPE_OBJ); + sendObject(connection, obj, instId, UAVTALK_TYPE_OBJ); break; - case TYPE_NACK: + case UAVTALK_TYPE_NACK: // Do nothing on flight side, let it time out. break; - case TYPE_ACK: + case UAVTALK_TYPE_ACK: // All instances, not allowed for ACK messages if (instId != UAVOBJ_ALL_INSTANCES) { @@ -537,7 +557,7 @@ static int32_t receiveObject(UAVTalkConnection *connection, uint8_t type, uint32 * \param[in] obj Object * \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances. */ -static void updateAck(UAVTalkConnection *connection, UAVObjHandle obj, uint16_t instId) +static void updateAck(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId) { if (connection->respObj == obj && (connection->respInstId == instId || connection->respInstId == UAVOBJ_ALL_INSTANCES)) { @@ -555,7 +575,7 @@ static void updateAck(UAVTalkConnection *connection, UAVObjHandle obj, uint16_t * \return 0 Success * \return -1 Failure */ -static int32_t sendObject(UAVTalkConnection *connection, UAVObjHandle obj, uint16_t instId, uint8_t type) +static int32_t sendObject(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId, uint8_t type) { uint32_t numInst; uint32_t n; @@ -567,7 +587,7 @@ static int32_t sendObject(UAVTalkConnection *connection, UAVObjHandle obj, uint1 } // Process message type - if ( type == TYPE_OBJ || type == TYPE_OBJ_ACK ) + if ( type == UAVTALK_TYPE_OBJ || type == UAVTALK_TYPE_OBJ_ACK ) { if (instId == UAVOBJ_ALL_INSTANCES) { @@ -585,15 +605,15 @@ static int32_t sendObject(UAVTalkConnection *connection, UAVObjHandle obj, uint1 return sendSingleObject(connection, obj, instId, type); } } - else if (type == TYPE_OBJ_REQ) + else if (type == UAVTALK_TYPE_OBJ_REQ) { - return sendSingleObject(connection, obj, instId, TYPE_OBJ_REQ); + return sendSingleObject(connection, obj, instId, UAVTALK_TYPE_OBJ_REQ); } - else if (type == TYPE_ACK) + else if (type == UAVTALK_TYPE_ACK) { if ( instId != UAVOBJ_ALL_INSTANCES ) { - return sendSingleObject(connection, obj, instId, TYPE_ACK); + return sendSingleObject(connection, obj, instId, UAVTALK_TYPE_ACK); } else { @@ -615,7 +635,7 @@ static int32_t sendObject(UAVTalkConnection *connection, UAVObjHandle obj, uint1 * \return 0 Success * \return -1 Failure */ -static int32_t sendSingleObject(UAVTalkConnection *connection, UAVObjHandle obj, uint16_t instId, uint8_t type) +static int32_t sendSingleObject(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId, uint8_t type) { int32_t length; int32_t dataOffset; @@ -623,7 +643,7 @@ static int32_t sendSingleObject(UAVTalkConnection *connection, UAVObjHandle obj, // Setup type and object id fields objId = UAVObjGetID(obj); - connection->txBuffer[0] = SYNC_VAL; // sync byte + connection->txBuffer[0] = UAVTALK_SYNC_VAL; // sync byte connection->txBuffer[1] = type; // data length inserted here below connection->txBuffer[4] = (uint8_t)(objId & 0xFF); @@ -644,7 +664,7 @@ static int32_t sendSingleObject(UAVTalkConnection *connection, UAVObjHandle obj, } // Determine data length - if (type == TYPE_OBJ_REQ || type == TYPE_ACK) + if (type == UAVTALK_TYPE_OBJ_REQ || type == UAVTALK_TYPE_ACK) { length = 0; } @@ -674,9 +694,17 @@ static int32_t sendSingleObject(UAVTalkConnection *connection, UAVObjHandle obj, // Calculate checksum connection->txBuffer[dataOffset+length] = PIOS_CRC_updateCRC(0, connection->txBuffer, dataOffset+length); - - // Send buffer - if (connection->outStream!=NULL) (*connection->outStream)(connection->txBuffer, dataOffset+length+UAVTALK_CHECKSUM_LENGTH); + + // Send buffer (partially if needed) + uint32_t sent=0; + while (sent < dataOffset+length+UAVTALK_CHECKSUM_LENGTH) { + uint32_t sending = dataOffset+length+UAVTALK_CHECKSUM_LENGTH - sent; + if ( sending > connection->txSize ) sending = connection->txSize; + if ( connection->outStream != NULL ) { + (*connection->outStream)(connection->txBuffer+sent, sending); + } + sent += sending; + } // Update stats ++connection->stats.txObjects; @@ -694,12 +722,12 @@ static int32_t sendSingleObject(UAVTalkConnection *connection, UAVObjHandle obj, * \return 0 Success * \return -1 Failure */ -static int32_t sendNack(UAVTalkConnection *connection, uint32_t objId) +static int32_t sendNack(UAVTalkConnectionData *connection, uint32_t objId) { int32_t dataOffset; - connection->txBuffer[0] = SYNC_VAL; // sync byte - connection->txBuffer[1] = TYPE_NACK; + connection->txBuffer[0] = UAVTALK_SYNC_VAL; // sync byte + connection->txBuffer[1] = UAVTALK_TYPE_NACK; // data length inserted here below connection->txBuffer[4] = (uint8_t)(objId & 0xFF); connection->txBuffer[5] = (uint8_t)((objId >> 8) & 0xFF); diff --git a/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.cpp b/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.cpp index 8d3a30b30..3d9fd12b6 100644 --- a/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.cpp +++ b/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.cpp @@ -34,6 +34,7 @@ bool UAVObjectGeneratorFlight::generate(UAVObjectParser* parser,QString template <<"uint16_t" << "uint32_t" << "float" << "uint8_t"; QString flightObjInit,objInc,objFileNames,objNames; + qint32 sizeCalc; flightCodePath = QDir( templatepath + QString("flight/UAVObjects")); flightOutputPath = QDir( outputpath + QString("flight") ); flightOutputPath.mkpath(flightOutputPath.absolutePath()); @@ -41,6 +42,7 @@ bool UAVObjectGeneratorFlight::generate(UAVObjectParser* parser,QString template flightCodeTemplate = readFile( flightCodePath.absoluteFilePath("uavobjecttemplate.c") ); flightIncludeTemplate = readFile( flightCodePath.absoluteFilePath("inc/uavobjecttemplate.h") ); flightInitTemplate = readFile( flightCodePath.absoluteFilePath("uavobjectsinittemplate.c") ); + flightInitIncludeTemplate = readFile( flightCodePath.absoluteFilePath("inc/uavobjectsinittemplate.h") ); flightMakeTemplate = readFile( flightCodePath.absoluteFilePath("Makefiletemplate.inc") ); if ( flightCodeTemplate.isNull() || flightIncludeTemplate.isNull() || flightInitTemplate.isNull()) { @@ -48,6 +50,7 @@ bool UAVObjectGeneratorFlight::generate(UAVObjectParser* parser,QString template return false; } + sizeCalc = 0; for (int objidx = 0; objidx < parser->getNumObjects(); ++objidx) { ObjectInfo* info=parser->getObjectByIndex(objidx); process_object(info); @@ -57,6 +60,9 @@ bool UAVObjectGeneratorFlight::generate(UAVObjectParser* parser,QString template objInc.append("#include \"" + info->namelc + ".h\"\r\n"); objFileNames.append(" " + info->namelc); objNames.append(" " + info->name); + if (parser->getNumBytes(objidx)>sizeCalc) { + sizeCalc = parser->getNumBytes(objidx); + } } // Write the flight object inialization files @@ -65,7 +71,16 @@ bool UAVObjectGeneratorFlight::generate(UAVObjectParser* parser,QString template bool res = writeFileIfDiffrent( flightOutputPath.absolutePath() + "/uavobjectsinit.c", flightInitTemplate ); if (!res) { - cout << "Error: Could not write flight object init files" << endl; + cout << "Error: Could not write flight object init file" << endl; + return false; + } + + // Write the flight object initialization header + flightInitIncludeTemplate.replace( QString("$(SIZECALCULATION)"), QString().setNum(sizeCalc)); + res = writeFileIfDiffrent( flightOutputPath.absolutePath() + "/uavobjectsinit.h", + flightInitIncludeTemplate ); + if (!res) { + cout << "Error: Could not write flight object init header file" << endl; return false; } diff --git a/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.h b/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.h index 1050b2943..db728374b 100644 --- a/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.h +++ b/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.h @@ -34,7 +34,7 @@ class UAVObjectGeneratorFlight public: bool generate(UAVObjectParser* gen,QString templatepath,QString outputpath); QStringList fieldTypeStrC; - QString flightCodeTemplate, flightIncludeTemplate, flightInitTemplate, flightMakeTemplate; + QString flightCodeTemplate, flightIncludeTemplate, flightInitTemplate, flightInitIncludeTemplate, flightMakeTemplate; QDir flightCodePath; QDir flightOutputPath; From 333ab645ec9ff20920cc2a17cd790c0c92987e14 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sat, 30 Jul 2011 09:55:02 +0900 Subject: [PATCH 014/145] Dos2unix manualcontrol.c to facilitate merging --- flight/Modules/ManualControl/manualcontrol.c | 1274 +++++++++--------- 1 file changed, 637 insertions(+), 637 deletions(-) diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c index 37a5fa063..d496ba08f 100644 --- a/flight/Modules/ManualControl/manualcontrol.c +++ b/flight/Modules/ManualControl/manualcontrol.c @@ -1,637 +1,637 @@ -/** - ****************************************************************************** - * @addtogroup OpenPilotModules OpenPilot Modules - * @{ - * @addtogroup ManualControlModule Manual Control Module - * @brief Provide manual control or allow it alter flight mode. - * @{ - * - * Reads in the ManualControlCommand FlightMode setting from receiver then either - * pass the settings straght to ActuatorDesired object (manual mode) or to - * AttitudeDesired object (stabilized mode) - * - * @file manualcontrol.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief ManualControl module. Handles safety R/C link and flight mode. - * - * @see The GNU Public License (GPL) Version 3 - * - *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License - * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -#include "openpilot.h" -#include "manualcontrol.h" -#include "manualcontrolsettings.h" -#include "stabilizationsettings.h" -#include "manualcontrolcommand.h" -#include "actuatordesired.h" -#include "stabilizationdesired.h" -#include "flighttelemetrystats.h" -#include "flightstatus.h" -#include "accessorydesired.h" - -// Private constants -#if defined(PIOS_MANUAL_STACK_SIZE) -#define STACK_SIZE_BYTES PIOS_MANUAL_STACK_SIZE -#else -#define STACK_SIZE_BYTES 824 -#endif - -#define TASK_PRIORITY (tskIDLE_PRIORITY+4) -#define UPDATE_PERIOD_MS 20 -#define THROTTLE_FAILSAFE -0.1 -#define FLIGHT_MODE_LIMIT 1.0/3.0 -#define ARMED_TIME_MS 1000 -#define ARMED_THRESHOLD 0.50 -//safe band to allow a bit of calibration error or trim offset (in microseconds) -#define CONNECTION_OFFSET 150 - -// Private types -typedef enum -{ - ARM_STATE_DISARMED, - ARM_STATE_ARMING_MANUAL, - ARM_STATE_ARMED, - ARM_STATE_DISARMING_MANUAL, - ARM_STATE_DISARMING_TIMEOUT -} ArmState_t; - -// Private variables -static xTaskHandle taskHandle; -static ArmState_t armState; -static portTickType lastSysTime; - -// Private functions -static void updateActuatorDesired(ManualControlCommandData * cmd); -static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings); -static void processFlightMode(ManualControlSettingsData * settings, float flightMode); -static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings); - -static void manualControlTask(void *parameters); -static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral); -static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time); -static bool okToArm(void); -static bool validInputRange(int16_t min, int16_t max, uint16_t value); - -#define assumptions1 ( \ - ((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \ - ((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ - ((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \ - ) - -#define assumptions3 ( \ - ((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \ - ((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ - ((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \ - ) - -#define assumptions5 ( \ - ((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \ - ((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ - ((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \ - ) - - - -#define ARMING_CHANNEL_ROLL 0 -#define ARMING_CHANNEL_PITCH 1 -#define ARMING_CHANNEL_YAW 2 - -#define assumptions7 ( \ - ( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_ROLL) && \ - ( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_ROLL) && \ - ( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHFORWARD -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_PITCH) && \ - ( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHAFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_PITCH) && \ - ( ((int)MANUALCONTROLSETTINGS_ARMING_YAWLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_YAW) && \ - ( ((int)MANUALCONTROLSETTINGS_ARMING_YAWRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_YAW) \ - ) - -#define assumptions8 ( \ - ( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 == 0) && \ - ( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 != 0) && \ - ( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHFORWARD -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 == 0) && \ - ( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHAFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 != 0) && \ - ( ((int)MANUALCONTROLSETTINGS_ARMING_YAWLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 == 0) && \ - ( ((int)MANUALCONTROLSETTINGS_ARMING_YAWRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 != 0) \ - ) - - -#define assumptions_flightmode ( \ - ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_MANUAL == (int) FLIGHTSTATUS_FLIGHTMODE_MANUAL) && \ - ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED1 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED1) && \ - ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED2 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED2) && \ - ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED3 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED3) && \ - ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL == (int) FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) && \ - ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD == (int) FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) \ - ) - -#define assumptions (assumptions1 && assumptions3 && assumptions5 && assumptions7 && assumptions8 && assumptions_flightmode) - -/** - * Module initialization - */ -int32_t ManualControlInitialize() -{ - /* Check the assumptions about uavobject enum's are correct */ - if(!assumptions) - return -1; - - AccessoryDesiredInitialize(); - ManualControlSettingsInitialize(); - ManualControlCommandInitialize(); - FlightStatusInitialize(); - StabilizationDesiredInitialize(); - - // Start main task - xTaskCreate(manualControlTask, (signed char *)"ManualControl", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle); - TaskMonitorAdd(TASKINFO_RUNNING_MANUALCONTROL, taskHandle); - PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL); - - - return 0; -} - -/** - * Module task - */ -static void manualControlTask(void *parameters) -{ - ManualControlSettingsData settings; - ManualControlCommandData cmd; - FlightStatusData flightStatus; - float flightMode = 0; - - uint8_t disconnected_count = 0; - uint8_t connected_count = 0; - - // For now manual instantiate extra instances of Accessory Desired. In future should be done dynamically - // this includes not even registering it if not used - AccessoryDesiredCreateInstance(); - AccessoryDesiredCreateInstance(); - - // Make sure unarmed on power up - ManualControlCommandGet(&cmd); - FlightStatusGet(&flightStatus); - flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED; - armState = ARM_STATE_DISARMED; - - // Main task loop - lastSysTime = xTaskGetTickCount(); - while (1) { - float scaledChannel[MANUALCONTROLCOMMAND_CHANNEL_NUMELEM]; - - // Wait until next update - vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS); - PIOS_WDG_UpdateFlag(PIOS_WDG_MANUAL); - - // Read settings - ManualControlSettingsGet(&settings); - - if (ManualControlCommandReadOnly(&cmd)) { - FlightTelemetryStatsData flightTelemStats; - FlightTelemetryStatsGet(&flightTelemStats); - if(flightTelemStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) { - /* trying to fly via GCS and lost connection. fall back to transmitter */ - UAVObjMetadata metadata; - UAVObjGetMetadata(&cmd, &metadata); - metadata.access = ACCESS_READWRITE; - UAVObjSetMetadata(&cmd, &metadata); - } - } - - if (!ManualControlCommandReadOnly(&cmd)) { - - // Read channel values in us - // TODO: settings.InputMode is currently ignored because PIOS will not allow runtime - // selection of PWM and PPM. The configuration is currently done at compile time in - // the pios_config.h file. - for (int n = 0; n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) { -#if defined(PIOS_INCLUDE_PWM) - cmd.Channel[n] = PIOS_PWM_Get(n); -#elif defined(PIOS_INCLUDE_PPM) - cmd.Channel[n] = PIOS_PPM_Get(n); -#elif defined(PIOS_INCLUDE_SPEKTRUM) - cmd.Channel[n] = PIOS_SPEKTRUM_Get(n); -#endif - scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n], settings.ChannelMin[n], settings.ChannelNeutral[n]); - } - - // Check settings, if error raise alarm - if (settings.Roll >= MANUALCONTROLSETTINGS_ROLL_NONE || - settings.Pitch >= MANUALCONTROLSETTINGS_PITCH_NONE || - settings.Yaw >= MANUALCONTROLSETTINGS_YAW_NONE || - settings.Throttle >= MANUALCONTROLSETTINGS_THROTTLE_NONE || - settings.FlightMode >= MANUALCONTROLSETTINGS_FLIGHTMODE_NONE) { - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); - cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; - ManualControlCommandSet(&cmd); - continue; - } - - // decide if we have valid manual input or not - bool valid_input_detected = validInputRange(settings.ChannelMin[settings.Throttle], settings.ChannelMax[settings.Throttle], cmd.Channel[settings.Throttle]) && - validInputRange(settings.ChannelMin[settings.Roll], settings.ChannelMax[settings.Roll], cmd.Channel[settings.Roll]) && - validInputRange(settings.ChannelMin[settings.Yaw], settings.ChannelMax[settings.Yaw], cmd.Channel[settings.Yaw]) && - validInputRange(settings.ChannelMin[settings.Pitch], settings.ChannelMax[settings.Pitch], cmd.Channel[settings.Pitch]); - - // Implement hysteresis loop on connection status - if (valid_input_detected && (++connected_count > 10)) { - cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE; - connected_count = 0; - disconnected_count = 0; - } else if (!valid_input_detected && (++disconnected_count > 10)) { - cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; - connected_count = 0; - disconnected_count = 0; - } - - if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) { - cmd.Throttle = -1; // Shut down engine with no control - cmd.Roll = 0; - cmd.Yaw = 0; - cmd.Pitch = 0; - //cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO; // don't do until AUTO implemented and functioning - // Important: Throttle < 0 will reset Stabilization coefficients among other things. Either change this, - // or leave throttle at IDLE speed or above when going into AUTO-failsafe. - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); - ManualControlCommandSet(&cmd); - } else { - AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL); - - // Scale channels to -1 -> +1 range - cmd.Roll = scaledChannel[settings.Roll]; - cmd.Pitch = scaledChannel[settings.Pitch]; - cmd.Yaw = scaledChannel[settings.Yaw]; - cmd.Throttle = scaledChannel[settings.Throttle]; - flightMode = scaledChannel[settings.FlightMode]; - - AccessoryDesiredData accessory; - // Set Accessory 0 - if(settings.Accessory0 != MANUALCONTROLSETTINGS_ACCESSORY0_NONE) { - accessory.AccessoryVal = scaledChannel[settings.Accessory0]; - if(AccessoryDesiredInstSet(0, &accessory) != 0) - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); - } - // Set Accessory 1 - if(settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY1_NONE) { - accessory.AccessoryVal = scaledChannel[settings.Accessory1]; - if(AccessoryDesiredInstSet(1, &accessory) != 0) - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); - } - // Set Accsesory 2 - if(settings.Accessory2 != MANUALCONTROLSETTINGS_ACCESSORY2_NONE) { - accessory.AccessoryVal = scaledChannel[settings.Accessory2]; - if(AccessoryDesiredInstSet(2, &accessory) != 0) - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); - } - - - processFlightMode(&settings, flightMode); - processArm(&cmd, &settings); - - // Update cmd object - ManualControlCommandSet(&cmd); - - } - - } else { - ManualControlCommandGet(&cmd); /* Under GCS control */ - } - - - FlightStatusGet(&flightStatus); - - // Depending on the mode update the Stabilization or Actuator objects - switch(PARSE_FLIGHT_MODE(flightStatus.FlightMode)) { - case FLIGHTMODE_UNDEFINED: - // This reflects a bug in the code architecture! - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); - break; - case FLIGHTMODE_MANUAL: - updateActuatorDesired(&cmd); - break; - case FLIGHTMODE_STABILIZED: - updateStabilizationDesired(&cmd, &settings); - break; - case FLIGHTMODE_GUIDANCE: - // TODO: Implement - break; - } - } -} - -static void updateActuatorDesired(ManualControlCommandData * cmd) -{ - ActuatorDesiredData actuator; - ActuatorDesiredGet(&actuator); - actuator.Roll = cmd->Roll; - actuator.Pitch = cmd->Pitch; - actuator.Yaw = cmd->Yaw; - actuator.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle; - ActuatorDesiredSet(&actuator); -} - -static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings) -{ - StabilizationDesiredData stabilization; - StabilizationDesiredGet(&stabilization); - - StabilizationSettingsData stabSettings; - StabilizationSettingsGet(&stabSettings); - - uint8_t * stab_settings; - FlightStatusData flightStatus; - FlightStatusGet(&flightStatus); - switch(flightStatus.FlightMode) { - case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: - stab_settings = settings->Stabilization1Settings; - break; - case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: - stab_settings = settings->Stabilization2Settings; - break; - case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: - stab_settings = settings->Stabilization3Settings; - break; - default: - // Major error, this should not occur because only enter this block when one of these is true - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); - return; - } - - // TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order - stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = stab_settings[0]; - stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = stab_settings[1]; - stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = stab_settings[2]; - - stabilization.Roll = (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.RollMax : - 0; // this is an invalid mode - ; - stabilization.Pitch = (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Pitch * stabSettings.PitchMax : - 0; // this is an invalid mode - - stabilization.Yaw = (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? fmod(cmd->Yaw * 180.0, 360) : - 0; // this is an invalid mode - - stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle; - StabilizationDesiredSet(&stabilization); -} - -/** - * Convert channel from servo pulse duration (microseconds) to scaled -1/+1 range. - */ -static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral) -{ - float valueScaled; - - // Scale - if ((max > min && value >= neutral) || (min > max && value <= neutral)) - { - if (max != neutral) - valueScaled = (float)(value - neutral) / (float)(max - neutral); - else - valueScaled = 0; - } - else - { - if (min != neutral) - valueScaled = (float)(value - neutral) / (float)(neutral - min); - else - valueScaled = 0; - } - - // Bound - if (valueScaled > 1.0) valueScaled = 1.0; - else - if (valueScaled < -1.0) valueScaled = -1.0; - - return valueScaled; -} - -static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time) { - if(end_time > start_time) - return (end_time - start_time) * portTICK_RATE_MS; - return ((((portTICK_RATE_MS) -1) - start_time) + end_time) * portTICK_RATE_MS; -} - -/** - * @brief Determine if the aircraft is safe to arm - * @returns True if safe to arm, false otherwise - */ -static bool okToArm(void) -{ - // read alarms - SystemAlarmsData alarms; - SystemAlarmsGet(&alarms); - - - // Check each alarm - for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++) - { - if (alarms.Alarm[i] >= SYSTEMALARMS_ALARM_ERROR) - { // found an alarm thats set - if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY) - continue; - - return false; - } - } - - return true; -} - -/** - * @brief Update the flightStatus object only if value changed. Reduces callbacks - * @param[in] val The new value - */ -static void setArmedIfChanged(uint8_t val) { - FlightStatusData flightStatus; - FlightStatusGet(&flightStatus); - - if(flightStatus.Armed != val) { - flightStatus.Armed = val; - FlightStatusSet(&flightStatus); - } -} - -/** - * @brief Process the inputs and determine whether to arm or not - * @param[out] cmd The structure to set the armed in - * @param[in] settings Settings indicating the necessary position - */ -static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings) -{ - - bool lowThrottle = cmd->Throttle <= 0; - - if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSDISARMED) { - // In this configuration we always disarm - setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); - } else { - // Not really needed since this function not called when disconnected - if (cmd->Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) - return; - - // The throttle is not low, in case we where arming or disarming, abort - if (!lowThrottle) { - switch(armState) { - case ARM_STATE_DISARMING_MANUAL: - case ARM_STATE_DISARMING_TIMEOUT: - armState = ARM_STATE_ARMED; - break; - case ARM_STATE_ARMING_MANUAL: - armState = ARM_STATE_DISARMED; - break; - default: - // Nothing needs to be done in the other states - break; - } - return; - } - - // The rest of these cases throttle is low - if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSARMED) { - // In this configuration, we go into armed state as soon as the throttle is low, never disarm - setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED); - return; - } - - - // When the configuration is not "Always armed" and no "Always disarmed", - // the state will not be changed when the throttle is not low - static portTickType armedDisarmStart; - float armingInputLevel = 0; - - // Calc channel see assumptions7 - int8_t sign = ((settings->Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2) ? -1 : 1; - switch ( (settings->Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 ) { - case ARMING_CHANNEL_ROLL: armingInputLevel = sign * cmd->Roll; break; - case ARMING_CHANNEL_PITCH: armingInputLevel = sign * cmd->Pitch; break; - case ARMING_CHANNEL_YAW: armingInputLevel = sign * cmd->Yaw; break; - } - - bool manualArm = false; - bool manualDisarm = false; - - if (armingInputLevel <= -ARMED_THRESHOLD) - manualArm = true; - else if (armingInputLevel >= +ARMED_THRESHOLD) - manualDisarm = true; - - switch(armState) { - case ARM_STATE_DISARMED: - setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); - - // only allow arming if it's OK too - if (manualArm && okToArm()) { - armedDisarmStart = lastSysTime; - armState = ARM_STATE_ARMING_MANUAL; - } - break; - - case ARM_STATE_ARMING_MANUAL: - setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMING); - - if (manualArm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS)) - armState = ARM_STATE_ARMED; - else if (!manualArm) - armState = ARM_STATE_DISARMED; - break; - - case ARM_STATE_ARMED: - // When we get here, the throttle is low, - // we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled - armedDisarmStart = lastSysTime; - armState = ARM_STATE_DISARMING_TIMEOUT; - setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED); - break; - - case ARM_STATE_DISARMING_TIMEOUT: - // We get here when armed while throttle low, even when the arming timeout is not enabled - if ((settings->ArmedTimeout != 0) && (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings->ArmedTimeout)) - armState = ARM_STATE_DISARMED; - - // Switch to disarming due to manual control when needed - if (manualDisarm) { - armedDisarmStart = lastSysTime; - armState = ARM_STATE_DISARMING_MANUAL; - } - break; - - case ARM_STATE_DISARMING_MANUAL: - if (manualDisarm &&(timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS)) - armState = ARM_STATE_DISARMED; - else if (!manualDisarm) - armState = ARM_STATE_ARMED; - break; - } // End Switch - } -} - -/** - * @brief Determine which of three positions the flight mode switch is in and set flight mode accordingly - * @param[out] cmd Pointer to the command structure to set the flight mode in - * @param[in] settings The settings which indicate which position is which mode - * @param[in] flightMode the value of the switch position - */ -static void processFlightMode(ManualControlSettingsData * settings, float flightMode) -{ - FlightStatusData flightStatus; - FlightStatusGet(&flightStatus); - - uint8_t newMode; - // Note here the code is ass - if (flightMode < -FLIGHT_MODE_LIMIT) - newMode = settings->FlightModePosition[0]; - else if (flightMode > FLIGHT_MODE_LIMIT) - newMode = settings->FlightModePosition[2]; - else - newMode = settings->FlightModePosition[1]; - - if(flightStatus.FlightMode != newMode) { - flightStatus.FlightMode = newMode; - FlightStatusSet(&flightStatus); - } - -} - -/** - * @brief Determine if the manual input value is within acceptable limits - * @returns return TRUE if so, otherwise return FALSE - */ -bool validInputRange(int16_t min, int16_t max, uint16_t value) -{ - if (min > max) - { - int16_t tmp = min; - min = max; - max = tmp; - } - return (value >= min - CONNECTION_OFFSET && value <= max + CONNECTION_OFFSET); -} - -/** - * @} - * @} - */ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup ManualControlModule Manual Control Module + * @brief Provide manual control or allow it alter flight mode. + * @{ + * + * Reads in the ManualControlCommand FlightMode setting from receiver then either + * pass the settings straght to ActuatorDesired object (manual mode) or to + * AttitudeDesired object (stabilized mode) + * + * @file manualcontrol.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief ManualControl module. Handles safety R/C link and flight mode. + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include "openpilot.h" +#include "manualcontrol.h" +#include "manualcontrolsettings.h" +#include "stabilizationsettings.h" +#include "manualcontrolcommand.h" +#include "actuatordesired.h" +#include "stabilizationdesired.h" +#include "flighttelemetrystats.h" +#include "flightstatus.h" +#include "accessorydesired.h" + +// Private constants +#if defined(PIOS_MANUAL_STACK_SIZE) +#define STACK_SIZE_BYTES PIOS_MANUAL_STACK_SIZE +#else +#define STACK_SIZE_BYTES 824 +#endif + +#define TASK_PRIORITY (tskIDLE_PRIORITY+4) +#define UPDATE_PERIOD_MS 20 +#define THROTTLE_FAILSAFE -0.1 +#define FLIGHT_MODE_LIMIT 1.0/3.0 +#define ARMED_TIME_MS 1000 +#define ARMED_THRESHOLD 0.50 +//safe band to allow a bit of calibration error or trim offset (in microseconds) +#define CONNECTION_OFFSET 150 + +// Private types +typedef enum +{ + ARM_STATE_DISARMED, + ARM_STATE_ARMING_MANUAL, + ARM_STATE_ARMED, + ARM_STATE_DISARMING_MANUAL, + ARM_STATE_DISARMING_TIMEOUT +} ArmState_t; + +// Private variables +static xTaskHandle taskHandle; +static ArmState_t armState; +static portTickType lastSysTime; + +// Private functions +static void updateActuatorDesired(ManualControlCommandData * cmd); +static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings); +static void processFlightMode(ManualControlSettingsData * settings, float flightMode); +static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings); + +static void manualControlTask(void *parameters); +static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral); +static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time); +static bool okToArm(void); +static bool validInputRange(int16_t min, int16_t max, uint16_t value); + +#define assumptions1 ( \ + ((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \ + ((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ + ((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \ + ) + +#define assumptions3 ( \ + ((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \ + ((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ + ((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \ + ) + +#define assumptions5 ( \ + ((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \ + ((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ + ((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \ + ) + + + +#define ARMING_CHANNEL_ROLL 0 +#define ARMING_CHANNEL_PITCH 1 +#define ARMING_CHANNEL_YAW 2 + +#define assumptions7 ( \ + ( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_ROLL) && \ + ( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_ROLL) && \ + ( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHFORWARD -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_PITCH) && \ + ( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHAFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_PITCH) && \ + ( ((int)MANUALCONTROLSETTINGS_ARMING_YAWLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_YAW) && \ + ( ((int)MANUALCONTROLSETTINGS_ARMING_YAWRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_YAW) \ + ) + +#define assumptions8 ( \ + ( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 == 0) && \ + ( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 != 0) && \ + ( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHFORWARD -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 == 0) && \ + ( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHAFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 != 0) && \ + ( ((int)MANUALCONTROLSETTINGS_ARMING_YAWLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 == 0) && \ + ( ((int)MANUALCONTROLSETTINGS_ARMING_YAWRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 != 0) \ + ) + + +#define assumptions_flightmode ( \ + ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_MANUAL == (int) FLIGHTSTATUS_FLIGHTMODE_MANUAL) && \ + ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED1 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED1) && \ + ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED2 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED2) && \ + ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED3 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED3) && \ + ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL == (int) FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) && \ + ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD == (int) FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) \ + ) + +#define assumptions (assumptions1 && assumptions3 && assumptions5 && assumptions7 && assumptions8 && assumptions_flightmode) + +/** + * Module initialization + */ +int32_t ManualControlInitialize() +{ + /* Check the assumptions about uavobject enum's are correct */ + if(!assumptions) + return -1; + + AccessoryDesiredInitialize(); + ManualControlSettingsInitialize(); + ManualControlCommandInitialize(); + FlightStatusInitialize(); + StabilizationDesiredInitialize(); + + // Start main task + xTaskCreate(manualControlTask, (signed char *)"ManualControl", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle); + TaskMonitorAdd(TASKINFO_RUNNING_MANUALCONTROL, taskHandle); + PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL); + + + return 0; +} + +/** + * Module task + */ +static void manualControlTask(void *parameters) +{ + ManualControlSettingsData settings; + ManualControlCommandData cmd; + FlightStatusData flightStatus; + float flightMode = 0; + + uint8_t disconnected_count = 0; + uint8_t connected_count = 0; + + // For now manual instantiate extra instances of Accessory Desired. In future should be done dynamically + // this includes not even registering it if not used + AccessoryDesiredCreateInstance(); + AccessoryDesiredCreateInstance(); + + // Make sure unarmed on power up + ManualControlCommandGet(&cmd); + FlightStatusGet(&flightStatus); + flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED; + armState = ARM_STATE_DISARMED; + + // Main task loop + lastSysTime = xTaskGetTickCount(); + while (1) { + float scaledChannel[MANUALCONTROLCOMMAND_CHANNEL_NUMELEM]; + + // Wait until next update + vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS); + PIOS_WDG_UpdateFlag(PIOS_WDG_MANUAL); + + // Read settings + ManualControlSettingsGet(&settings); + + if (ManualControlCommandReadOnly(&cmd)) { + FlightTelemetryStatsData flightTelemStats; + FlightTelemetryStatsGet(&flightTelemStats); + if(flightTelemStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) { + /* trying to fly via GCS and lost connection. fall back to transmitter */ + UAVObjMetadata metadata; + UAVObjGetMetadata(&cmd, &metadata); + metadata.access = ACCESS_READWRITE; + UAVObjSetMetadata(&cmd, &metadata); + } + } + + if (!ManualControlCommandReadOnly(&cmd)) { + + // Read channel values in us + // TODO: settings.InputMode is currently ignored because PIOS will not allow runtime + // selection of PWM and PPM. The configuration is currently done at compile time in + // the pios_config.h file. + for (int n = 0; n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) { +#if defined(PIOS_INCLUDE_PWM) + cmd.Channel[n] = PIOS_PWM_Get(n); +#elif defined(PIOS_INCLUDE_PPM) + cmd.Channel[n] = PIOS_PPM_Get(n); +#elif defined(PIOS_INCLUDE_SPEKTRUM) + cmd.Channel[n] = PIOS_SPEKTRUM_Get(n); +#endif + scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n], settings.ChannelMin[n], settings.ChannelNeutral[n]); + } + + // Check settings, if error raise alarm + if (settings.Roll >= MANUALCONTROLSETTINGS_ROLL_NONE || + settings.Pitch >= MANUALCONTROLSETTINGS_PITCH_NONE || + settings.Yaw >= MANUALCONTROLSETTINGS_YAW_NONE || + settings.Throttle >= MANUALCONTROLSETTINGS_THROTTLE_NONE || + settings.FlightMode >= MANUALCONTROLSETTINGS_FLIGHTMODE_NONE) { + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); + cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; + ManualControlCommandSet(&cmd); + continue; + } + + // decide if we have valid manual input or not + bool valid_input_detected = validInputRange(settings.ChannelMin[settings.Throttle], settings.ChannelMax[settings.Throttle], cmd.Channel[settings.Throttle]) && + validInputRange(settings.ChannelMin[settings.Roll], settings.ChannelMax[settings.Roll], cmd.Channel[settings.Roll]) && + validInputRange(settings.ChannelMin[settings.Yaw], settings.ChannelMax[settings.Yaw], cmd.Channel[settings.Yaw]) && + validInputRange(settings.ChannelMin[settings.Pitch], settings.ChannelMax[settings.Pitch], cmd.Channel[settings.Pitch]); + + // Implement hysteresis loop on connection status + if (valid_input_detected && (++connected_count > 10)) { + cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE; + connected_count = 0; + disconnected_count = 0; + } else if (!valid_input_detected && (++disconnected_count > 10)) { + cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; + connected_count = 0; + disconnected_count = 0; + } + + if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) { + cmd.Throttle = -1; // Shut down engine with no control + cmd.Roll = 0; + cmd.Yaw = 0; + cmd.Pitch = 0; + //cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO; // don't do until AUTO implemented and functioning + // Important: Throttle < 0 will reset Stabilization coefficients among other things. Either change this, + // or leave throttle at IDLE speed or above when going into AUTO-failsafe. + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + ManualControlCommandSet(&cmd); + } else { + AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL); + + // Scale channels to -1 -> +1 range + cmd.Roll = scaledChannel[settings.Roll]; + cmd.Pitch = scaledChannel[settings.Pitch]; + cmd.Yaw = scaledChannel[settings.Yaw]; + cmd.Throttle = scaledChannel[settings.Throttle]; + flightMode = scaledChannel[settings.FlightMode]; + + AccessoryDesiredData accessory; + // Set Accessory 0 + if(settings.Accessory0 != MANUALCONTROLSETTINGS_ACCESSORY0_NONE) { + accessory.AccessoryVal = scaledChannel[settings.Accessory0]; + if(AccessoryDesiredInstSet(0, &accessory) != 0) + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } + // Set Accessory 1 + if(settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY1_NONE) { + accessory.AccessoryVal = scaledChannel[settings.Accessory1]; + if(AccessoryDesiredInstSet(1, &accessory) != 0) + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } + // Set Accsesory 2 + if(settings.Accessory2 != MANUALCONTROLSETTINGS_ACCESSORY2_NONE) { + accessory.AccessoryVal = scaledChannel[settings.Accessory2]; + if(AccessoryDesiredInstSet(2, &accessory) != 0) + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } + + + processFlightMode(&settings, flightMode); + processArm(&cmd, &settings); + + // Update cmd object + ManualControlCommandSet(&cmd); + + } + + } else { + ManualControlCommandGet(&cmd); /* Under GCS control */ + } + + + FlightStatusGet(&flightStatus); + + // Depending on the mode update the Stabilization or Actuator objects + switch(PARSE_FLIGHT_MODE(flightStatus.FlightMode)) { + case FLIGHTMODE_UNDEFINED: + // This reflects a bug in the code architecture! + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); + break; + case FLIGHTMODE_MANUAL: + updateActuatorDesired(&cmd); + break; + case FLIGHTMODE_STABILIZED: + updateStabilizationDesired(&cmd, &settings); + break; + case FLIGHTMODE_GUIDANCE: + // TODO: Implement + break; + } + } +} + +static void updateActuatorDesired(ManualControlCommandData * cmd) +{ + ActuatorDesiredData actuator; + ActuatorDesiredGet(&actuator); + actuator.Roll = cmd->Roll; + actuator.Pitch = cmd->Pitch; + actuator.Yaw = cmd->Yaw; + actuator.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle; + ActuatorDesiredSet(&actuator); +} + +static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings) +{ + StabilizationDesiredData stabilization; + StabilizationDesiredGet(&stabilization); + + StabilizationSettingsData stabSettings; + StabilizationSettingsGet(&stabSettings); + + uint8_t * stab_settings; + FlightStatusData flightStatus; + FlightStatusGet(&flightStatus); + switch(flightStatus.FlightMode) { + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: + stab_settings = settings->Stabilization1Settings; + break; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: + stab_settings = settings->Stabilization2Settings; + break; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: + stab_settings = settings->Stabilization3Settings; + break; + default: + // Major error, this should not occur because only enter this block when one of these is true + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); + return; + } + + // TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order + stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = stab_settings[0]; + stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = stab_settings[1]; + stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = stab_settings[2]; + + stabilization.Roll = (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.RollMax : + 0; // this is an invalid mode + ; + stabilization.Pitch = (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Pitch * stabSettings.PitchMax : + 0; // this is an invalid mode + + stabilization.Yaw = (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? fmod(cmd->Yaw * 180.0, 360) : + 0; // this is an invalid mode + + stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle; + StabilizationDesiredSet(&stabilization); +} + +/** + * Convert channel from servo pulse duration (microseconds) to scaled -1/+1 range. + */ +static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral) +{ + float valueScaled; + + // Scale + if ((max > min && value >= neutral) || (min > max && value <= neutral)) + { + if (max != neutral) + valueScaled = (float)(value - neutral) / (float)(max - neutral); + else + valueScaled = 0; + } + else + { + if (min != neutral) + valueScaled = (float)(value - neutral) / (float)(neutral - min); + else + valueScaled = 0; + } + + // Bound + if (valueScaled > 1.0) valueScaled = 1.0; + else + if (valueScaled < -1.0) valueScaled = -1.0; + + return valueScaled; +} + +static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time) { + if(end_time > start_time) + return (end_time - start_time) * portTICK_RATE_MS; + return ((((portTICK_RATE_MS) -1) - start_time) + end_time) * portTICK_RATE_MS; +} + +/** + * @brief Determine if the aircraft is safe to arm + * @returns True if safe to arm, false otherwise + */ +static bool okToArm(void) +{ + // read alarms + SystemAlarmsData alarms; + SystemAlarmsGet(&alarms); + + + // Check each alarm + for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++) + { + if (alarms.Alarm[i] >= SYSTEMALARMS_ALARM_ERROR) + { // found an alarm thats set + if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY) + continue; + + return false; + } + } + + return true; +} + +/** + * @brief Update the flightStatus object only if value changed. Reduces callbacks + * @param[in] val The new value + */ +static void setArmedIfChanged(uint8_t val) { + FlightStatusData flightStatus; + FlightStatusGet(&flightStatus); + + if(flightStatus.Armed != val) { + flightStatus.Armed = val; + FlightStatusSet(&flightStatus); + } +} + +/** + * @brief Process the inputs and determine whether to arm or not + * @param[out] cmd The structure to set the armed in + * @param[in] settings Settings indicating the necessary position + */ +static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings) +{ + + bool lowThrottle = cmd->Throttle <= 0; + + if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSDISARMED) { + // In this configuration we always disarm + setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); + } else { + // Not really needed since this function not called when disconnected + if (cmd->Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) + return; + + // The throttle is not low, in case we where arming or disarming, abort + if (!lowThrottle) { + switch(armState) { + case ARM_STATE_DISARMING_MANUAL: + case ARM_STATE_DISARMING_TIMEOUT: + armState = ARM_STATE_ARMED; + break; + case ARM_STATE_ARMING_MANUAL: + armState = ARM_STATE_DISARMED; + break; + default: + // Nothing needs to be done in the other states + break; + } + return; + } + + // The rest of these cases throttle is low + if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSARMED) { + // In this configuration, we go into armed state as soon as the throttle is low, never disarm + setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED); + return; + } + + + // When the configuration is not "Always armed" and no "Always disarmed", + // the state will not be changed when the throttle is not low + static portTickType armedDisarmStart; + float armingInputLevel = 0; + + // Calc channel see assumptions7 + int8_t sign = ((settings->Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2) ? -1 : 1; + switch ( (settings->Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 ) { + case ARMING_CHANNEL_ROLL: armingInputLevel = sign * cmd->Roll; break; + case ARMING_CHANNEL_PITCH: armingInputLevel = sign * cmd->Pitch; break; + case ARMING_CHANNEL_YAW: armingInputLevel = sign * cmd->Yaw; break; + } + + bool manualArm = false; + bool manualDisarm = false; + + if (armingInputLevel <= -ARMED_THRESHOLD) + manualArm = true; + else if (armingInputLevel >= +ARMED_THRESHOLD) + manualDisarm = true; + + switch(armState) { + case ARM_STATE_DISARMED: + setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); + + // only allow arming if it's OK too + if (manualArm && okToArm()) { + armedDisarmStart = lastSysTime; + armState = ARM_STATE_ARMING_MANUAL; + } + break; + + case ARM_STATE_ARMING_MANUAL: + setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMING); + + if (manualArm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS)) + armState = ARM_STATE_ARMED; + else if (!manualArm) + armState = ARM_STATE_DISARMED; + break; + + case ARM_STATE_ARMED: + // When we get here, the throttle is low, + // we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled + armedDisarmStart = lastSysTime; + armState = ARM_STATE_DISARMING_TIMEOUT; + setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED); + break; + + case ARM_STATE_DISARMING_TIMEOUT: + // We get here when armed while throttle low, even when the arming timeout is not enabled + if ((settings->ArmedTimeout != 0) && (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings->ArmedTimeout)) + armState = ARM_STATE_DISARMED; + + // Switch to disarming due to manual control when needed + if (manualDisarm) { + armedDisarmStart = lastSysTime; + armState = ARM_STATE_DISARMING_MANUAL; + } + break; + + case ARM_STATE_DISARMING_MANUAL: + if (manualDisarm &&(timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS)) + armState = ARM_STATE_DISARMED; + else if (!manualDisarm) + armState = ARM_STATE_ARMED; + break; + } // End Switch + } +} + +/** + * @brief Determine which of three positions the flight mode switch is in and set flight mode accordingly + * @param[out] cmd Pointer to the command structure to set the flight mode in + * @param[in] settings The settings which indicate which position is which mode + * @param[in] flightMode the value of the switch position + */ +static void processFlightMode(ManualControlSettingsData * settings, float flightMode) +{ + FlightStatusData flightStatus; + FlightStatusGet(&flightStatus); + + uint8_t newMode; + // Note here the code is ass + if (flightMode < -FLIGHT_MODE_LIMIT) + newMode = settings->FlightModePosition[0]; + else if (flightMode > FLIGHT_MODE_LIMIT) + newMode = settings->FlightModePosition[2]; + else + newMode = settings->FlightModePosition[1]; + + if(flightStatus.FlightMode != newMode) { + flightStatus.FlightMode = newMode; + FlightStatusSet(&flightStatus); + } + +} + +/** + * @brief Determine if the manual input value is within acceptable limits + * @returns return TRUE if so, otherwise return FALSE + */ +bool validInputRange(int16_t min, int16_t max, uint16_t value) +{ + if (min > max) + { + int16_t tmp = min; + min = max; + max = tmp; + } + return (value >= min - CONNECTION_OFFSET && value <= max + CONNECTION_OFFSET); +} + +/** + * @} + * @} + */ From 23e9b56839cc36d4c5464d26f7b976c9a3e13de6 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sat, 30 Jul 2011 10:06:35 +0900 Subject: [PATCH 015/145] Dos2unix flight/CopterControl/System/coptercontrol.c --- flight/CopterControl/System/coptercontrol.c | 194 ++++++++++---------- 1 file changed, 97 insertions(+), 97 deletions(-) diff --git a/flight/CopterControl/System/coptercontrol.c b/flight/CopterControl/System/coptercontrol.c index f66cf1cf5..aa0977c95 100644 --- a/flight/CopterControl/System/coptercontrol.c +++ b/flight/CopterControl/System/coptercontrol.c @@ -1,97 +1,97 @@ -/** - ****************************************************************************** - * @addtogroup OpenPilotSystem OpenPilot System - * @brief These files are the core system files of OpenPilot. - * They are the ground layer just above PiOS. In practice, OpenPilot actually starts - * in the main() function of openpilot.c - * @{ - * @addtogroup OpenPilotCore OpenPilot Core - * @brief This is where the OP firmware starts. Those files also define the compile-time - * options of the firmware. - * @{ - * @file openpilot.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Sets up and runs main OpenPilot tasks. - * @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 - */ - - -/* OpenPilot Includes */ -#include "openpilot.h" -#include "uavobjectsinit.h" -#include "systemmod.h" - -/* Task Priorities */ -#define PRIORITY_TASK_HOOKS (tskIDLE_PRIORITY + 3) - -/* Global Variables */ - -/* Prototype of PIOS_Board_Init() function */ -extern void PIOS_Board_Init(void); -extern void Stack_Change(void); - -/** -* OpenPilot Main function: -* -* Initialize PiOS
-* Create the "System" task (SystemModInitializein Modules/System/systemmod.c)
-* Start FreeRTOS Scheduler (vTaskStartScheduler) (Now handled by caller) -* If something goes wrong, blink LED1 and LED2 every 100ms -* -*/ -int main() -{ - /* NOTE: Do NOT modify the following start-up sequence */ - /* Any new initialization functions should be added in OpenPilotInit() */ - - /* Brings up System using CMSIS functions, enables the LEDs. */ - PIOS_SYS_Init(); - - /* Architecture dependant Hardware and - * core subsystem initialisation - * (see pios_board.c for your arch) - * */ - PIOS_Board_Init(); - - /* Initialize modules */ - MODULE_INITIALISE_ALL - - /* swap the stack to use the IRQ stack */ - Stack_Change(); - - /* Start the FreeRTOS scheduler which should never returns.*/ - vTaskStartScheduler(); - - /* If all is well we will never reach here as the scheduler will now be running. */ - - /* Do some indication to user that something bad just happened */ - PIOS_LED_Off(LED1); \ - for(;;) { \ - PIOS_LED_Toggle(LED1); \ - PIOS_DELAY_WaitmS(100); \ - }; - - return 0; -} - -/** - * @} - * @} - */ - +/** + ****************************************************************************** + * @addtogroup OpenPilotSystem OpenPilot System + * @brief These files are the core system files of OpenPilot. + * They are the ground layer just above PiOS. In practice, OpenPilot actually starts + * in the main() function of openpilot.c + * @{ + * @addtogroup OpenPilotCore OpenPilot Core + * @brief This is where the OP firmware starts. Those files also define the compile-time + * options of the firmware. + * @{ + * @file openpilot.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Sets up and runs main OpenPilot tasks. + * @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 + */ + + +/* OpenPilot Includes */ +#include "openpilot.h" +#include "uavobjectsinit.h" +#include "systemmod.h" + +/* Task Priorities */ +#define PRIORITY_TASK_HOOKS (tskIDLE_PRIORITY + 3) + +/* Global Variables */ + +/* Prototype of PIOS_Board_Init() function */ +extern void PIOS_Board_Init(void); +extern void Stack_Change(void); + +/** +* OpenPilot Main function: +* +* Initialize PiOS
+* Create the "System" task (SystemModInitializein Modules/System/systemmod.c)
+* Start FreeRTOS Scheduler (vTaskStartScheduler) (Now handled by caller) +* If something goes wrong, blink LED1 and LED2 every 100ms +* +*/ +int main() +{ + /* NOTE: Do NOT modify the following start-up sequence */ + /* Any new initialization functions should be added in OpenPilotInit() */ + + /* Brings up System using CMSIS functions, enables the LEDs. */ + PIOS_SYS_Init(); + + /* Architecture dependant Hardware and + * core subsystem initialisation + * (see pios_board.c for your arch) + * */ + PIOS_Board_Init(); + + /* Initialize modules */ + MODULE_INITIALISE_ALL + + /* swap the stack to use the IRQ stack */ + Stack_Change(); + + /* Start the FreeRTOS scheduler which should never returns.*/ + vTaskStartScheduler(); + + /* If all is well we will never reach here as the scheduler will now be running. */ + + /* Do some indication to user that something bad just happened */ + PIOS_LED_Off(LED1); \ + for(;;) { \ + PIOS_LED_Toggle(LED1); \ + PIOS_DELAY_WaitmS(100); \ + }; + + return 0; +} + +/** + * @} + * @} + */ + From b2d2114ede5f28d8448aba4c919e8051f7924235 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sat, 30 Jul 2011 10:24:46 +0900 Subject: [PATCH 016/145] Disable auto registration of all objects. Later the uavobj_init linker scripts should be deleted to not carry around old code. --- flight/CopterControl/System/pios_board_posix.c | 1 - flight/OpenPilot/System/pios_board.c | 1 - flight/OpenPilot/System/pios_board_posix.c | 1 - 3 files changed, 3 deletions(-) diff --git a/flight/CopterControl/System/pios_board_posix.c b/flight/CopterControl/System/pios_board_posix.c index 87d8e7402..30f949830 100644 --- a/flight/CopterControl/System/pios_board_posix.c +++ b/flight/CopterControl/System/pios_board_posix.c @@ -42,7 +42,6 @@ void PIOS_Board_Init(void) { /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); - UAVObjectsInitializeAll(); /* Initialize the alarms library */ AlarmsInitialize(); diff --git a/flight/OpenPilot/System/pios_board.c b/flight/OpenPilot/System/pios_board.c index ab3a99533..d23450235 100644 --- a/flight/OpenPilot/System/pios_board.c +++ b/flight/OpenPilot/System/pios_board.c @@ -1019,7 +1019,6 @@ void PIOS_Board_Init(void) { /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); - UAVObjectsInitializeAll(); #if defined(PIOS_INCLUDE_RTC) /* Initialize the real-time clock and its associated tick */ diff --git a/flight/OpenPilot/System/pios_board_posix.c b/flight/OpenPilot/System/pios_board_posix.c index 07cb6868d..f1c6719e4 100644 --- a/flight/OpenPilot/System/pios_board_posix.c +++ b/flight/OpenPilot/System/pios_board_posix.c @@ -53,7 +53,6 @@ void PIOS_Board_Init(void) { /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); - UAVObjectsInitializeAll(); /* Initialize the alarms library */ AlarmsInitialize(); From ae67c51632334d6453698434660c530208085748 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sat, 30 Jul 2011 11:28:21 +0900 Subject: [PATCH 017/145] Register some missing objects with their modules --- flight/CopterControl/System/alarms.c | 10 ++++++---- flight/Modules/Actuator/actuator.c | 1 + 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/flight/CopterControl/System/alarms.c b/flight/CopterControl/System/alarms.c index e899be779..01d79a1e5 100644 --- a/flight/CopterControl/System/alarms.c +++ b/flight/CopterControl/System/alarms.c @@ -41,10 +41,12 @@ static xSemaphoreHandle lock; static int32_t hasSeverity(SystemAlarmsAlarmOptions severity); /** - * Initialize the alarms library + * Initialize the alarms library */ int32_t AlarmsInitialize(void) { + SystemAlarmsInitialize(); + lock = xSemaphoreCreateRecursiveMutex(); //do not change the default states of the alarms, let the init code generated by the uavobjectgenerator handle that //AlarmsClearAll(); @@ -56,7 +58,7 @@ int32_t AlarmsInitialize(void) * Set an alarm * @param alarm The system alarm to be modified * @param severity The alarm severity - * @return 0 if success, -1 if an error + * @return 0 if success, -1 if an error */ int32_t AlarmsSet(SystemAlarmsAlarmElem alarm, SystemAlarmsAlarmOptions severity) { @@ -151,7 +153,7 @@ void AlarmsClearAll() /** * Check if there are any alarms with the given or higher severity - * @return 0 if no alarms are found, 1 if at least one alarm is found + * @return 0 if no alarms are found, 1 if at least one alarm is found */ int32_t AlarmsHasWarnings() { @@ -208,5 +210,5 @@ static int32_t hasSeverity(SystemAlarmsAlarmOptions severity) /** * @} * @} - */ + */ diff --git a/flight/Modules/Actuator/actuator.c b/flight/Modules/Actuator/actuator.c index 552205d4f..3c44f44fb 100644 --- a/flight/Modules/Actuator/actuator.c +++ b/flight/Modules/Actuator/actuator.c @@ -111,6 +111,7 @@ int32_t ActuatorInitialize() ActuatorSettingsInitialize(); ActuatorDesiredInitialize(); MixerSettingsInitialize(); + ActuatorCommandInitialize(); #if defined(DIAGNOSTICS) MixerStatusInitialize(); #endif From 132e19d613967b7ff057a2ef6bc876756639c2d9 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sat, 30 Jul 2011 11:29:29 +0900 Subject: [PATCH 018/145] Change the makefiles - always compile the optional objects but the flag indicates whether they are registered --- flight/CopterControl/Makefile | 6 ------ flight/OpenPilot/Makefile | 3 +++ 2 files changed, 3 insertions(+), 6 deletions(-) diff --git a/flight/CopterControl/Makefile b/flight/CopterControl/Makefile index f22b0a6e5..91241a525 100644 --- a/flight/CopterControl/Makefile +++ b/flight/CopterControl/Makefile @@ -168,16 +168,10 @@ SRC += $(OPUAVSYNTHDIR)/attitudesettings.c SRC += $(OPUAVSYNTHDIR)/camerastabsettings.c SRC += $(OPUAVSYNTHDIR)/hwsettings.c -ifeq ($(DIAGNOISTCS),YES) SRC += $(OPUAVSYNTHDIR)/taskinfo.c SRC += $(OPUAVSYNTHDIR)/mixerstatus.c SRC += $(OPUAVSYNTHDIR)/ratedesired.c -endif -#${wildcard ${OBJ}/$(shell echo $(VAR) | tr A-Z a-z)/*.c} -#SRC += ${foreach OBJ, ${UAVOBJECTS}, $(UAVOBJECTS)/$(OBJ).c} -# Cant use until i can automatically generate list of UAVObjects -#SRC += ${OUTDIR}/InitObjects.c endif ## PIOS Hardware (STM32F10x) diff --git a/flight/OpenPilot/Makefile b/flight/OpenPilot/Makefile index 60f9d6d30..85c56a3b3 100644 --- a/flight/OpenPilot/Makefile +++ b/flight/OpenPilot/Makefile @@ -388,6 +388,9 @@ ifeq ($(DEBUG),YES) CFLAGS = -g$(DEBUGF) -DDEBUG endif +# OP has enough memory to always enable optional objects +CFLAGS += -DDIAGNOSTICS + CFLAGS += -O$(OPT) CFLAGS += -mcpu=$(MCU) CFLAGS += $(CDEFS) From cb9c6dfb5e201ae93aca0b06c78f8a646d2f0ee6 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sat, 30 Jul 2011 11:31:45 +0900 Subject: [PATCH 019/145] Change the sequencing of telemetry starting to make sure all the objects that were registered in an initialize and then enumerated by telemetry --- flight/Modules/Telemetry/telemetry.c | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/flight/Modules/Telemetry/telemetry.c b/flight/Modules/Telemetry/telemetry.c index f0555f576..7a781e961 100644 --- a/flight/Modules/Telemetry/telemetry.c +++ b/flight/Modules/Telemetry/telemetry.c @@ -88,7 +88,13 @@ static void updateSettings(); */ int32_t TelemetryStart(void) { - + // Process all registered objects and connect queue for updates + UAVObjIterate(®isterObject); + + // Listen to objects of interest + GCSTelemetryStatsConnectQueue(priorityQueue); + TelemetrySettingsConnectQueue(priorityQueue); + // Start telemetry tasks xTaskCreate(telemetryTxTask, (signed char *)"TelTx", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY_TX, &telemetryTxTaskHandle); xTaskCreate(telemetryRxTask, (signed char *)"TelRx", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY_RX, &telemetryRxTaskHandle); @@ -111,7 +117,7 @@ int32_t TelemetryStart(void) int32_t TelemetryInitialize(void) { UAVObjEvent ev; - + FlightTelemetryStatsInitialize(); GCSTelemetryStatsInitialize(); TelemetrySettingsInitialize(); @@ -124,25 +130,19 @@ int32_t TelemetryInitialize(void) #if defined(PIOS_TELEM_PRIORITY_QUEUE) priorityQueue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); #endif - - // Get telemetry settings object - updateSettings(); + // Get telemetry settings object + updateSettings(); + // Initialise UAVTalk UAVTalkInitialize(&transmitData); - - // Process all registered objects and connect queue for updates - UAVObjIterate(®isterObject); - + // Create periodic event that will be used to update the telemetry stats txErrors = 0; txRetries = 0; memset(&ev, 0, sizeof(UAVObjEvent)); EventPeriodicQueueCreate(&ev, priorityQueue, STATS_UPDATE_PERIOD_MS); - - // Listen to objects of interest - GCSTelemetryStatsConnectQueue(priorityQueue); - TelemetrySettingsConnectQueue(priorityQueue); + return 0; } From 387a22398e74b2be611a2621b7ee96b84d523fb4 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sat, 30 Jul 2011 12:08:47 +0900 Subject: [PATCH 020/145] Make it possible to optionally enable the CameraStab module --- flight/CopterControl/Makefile | 2 +- flight/CopterControl/System/coptercontrol.c | 10 ++++++++++ .../OpenPilotOSX.xcodeproj/project.pbxproj | 2 ++ shared/uavobjectdefinition/hwsettings.xml | 17 +++++++++-------- 4 files changed, 22 insertions(+), 9 deletions(-) diff --git a/flight/CopterControl/Makefile b/flight/CopterControl/Makefile index 91241a525..814173731 100644 --- a/flight/CopterControl/Makefile +++ b/flight/CopterControl/Makefile @@ -65,7 +65,7 @@ endif FLASH_TOOL = OPENOCD # List of modules to include -MODULES = Attitude Stabilization Actuator ManualControl FirmwareIAP +MODULES = Attitude Stabilization Actuator ManualControl FirmwareIAP CameraStab # Telemetry must be last to grab the optional modules MODULES += Telemetry diff --git a/flight/CopterControl/System/coptercontrol.c b/flight/CopterControl/System/coptercontrol.c index aa0977c95..33cb096c2 100644 --- a/flight/CopterControl/System/coptercontrol.c +++ b/flight/CopterControl/System/coptercontrol.c @@ -35,6 +35,8 @@ /* OpenPilot Includes */ #include "openpilot.h" #include "uavobjectsinit.h" +#include "hwsettings.h" +#include "camerastab.h" #include "systemmod.h" /* Task Priorities */ @@ -72,6 +74,14 @@ int main() /* Initialize modules */ MODULE_INITIALISE_ALL + /* Optional module initialization. This code might want to go somewhere else as + * it grows */ + uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; + HwSettingsOptionalModulesGet(optionalModules); + if(optionalModules[HWSETTINGS_OPTIONALMODULES_CAMERASTABILIZATION] == HWSETTINGS_OPTIONALMODULES_ENABLED) { + CameraStabInitialize(); + } + /* swap the stack to use the IRQ stack */ Stack_Change(); diff --git a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj index 06b2f2d20..894b64da9 100644 --- a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj +++ b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj @@ -2696,6 +2696,7 @@ 65D2CA841248F9A400B1E7D6 /* mixersettings.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = mixersettings.xml; sourceTree = ""; }; 65D2CA851248F9A400B1E7D6 /* mixerstatus.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = mixerstatus.xml; sourceTree = ""; }; 65E410AE12F65AEA00725888 /* attitudesettings.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = attitudesettings.xml; sourceTree = ""; }; + 65E6D80713E3A4D0002A557A /* hwsettings.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = hwsettings.xml; sourceTree = ""; }; 65E6DF7112E02E8E00058553 /* Makefile */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.make; path = Makefile; sourceTree = ""; }; 65E6DF7312E02E8E00058553 /* alarms.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = alarms.c; sourceTree = ""; }; 65E6DF7412E02E8E00058553 /* coptercontrol.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = coptercontrol.c; sourceTree = ""; }; @@ -7426,6 +7427,7 @@ 65C35E4F12EFB2F3004811C2 /* uavobjectdefinition */ = { isa = PBXGroup; children = ( + 65E6D80713E3A4D0002A557A /* hwsettings.xml */, 65E8C788139AA2A800E1F979 /* accessorydesired.xml */, 65C35E5012EFB2F3004811C2 /* actuatorcommand.xml */, 65C35E5112EFB2F3004811C2 /* actuatordesired.xml */, diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index a3494f820..63c2a745a 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -1,11 +1,12 @@ - - Selection of optional hardware configurations. - - - - - - + + Selection of optional hardware configurations. + + + + + + + From 035b4103c4c344b28d6294826a51237c3ac67676 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sun, 31 Jul 2011 09:42:15 +0900 Subject: [PATCH 021/145] Need to initialize the HwSettings before using them. --- flight/CopterControl/System/pios_board.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/flight/CopterControl/System/pios_board.c b/flight/CopterControl/System/pios_board.c index 3da317097..785615280 100644 --- a/flight/CopterControl/System/pios_board.c +++ b/flight/CopterControl/System/pios_board.c @@ -908,6 +908,8 @@ void PIOS_Board_Init(void) { EventDispatcherInitialize(); UAVObjInitialize(); + HwSettingsInitialize(); + #if defined(PIOS_INCLUDE_RTC) /* Initialize the real-time clock and its associated tick */ PIOS_RTC_Init(&pios_rtc_main_cfg); From cdf4d112ce79ac49136ac42246c460235a480a3f Mon Sep 17 00:00:00 2001 From: zedamota Date: Mon, 1 Aug 2011 13:28:58 +0100 Subject: [PATCH 022/145] Finished refactoring airframe config widget --- .../plugins/config/configairframewidget.cpp | 96 +++++++++++++++---- .../src/plugins/config/configairframewidget.h | 2 +- .../src/plugins/config/configccpmwidget.h | 2 + .../src/plugins/config/configtaskwidget.cpp | 1 + 4 files changed, 80 insertions(+), 21 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/configairframewidget.cpp b/ground/openpilotgcs/src/plugins/config/configairframewidget.cpp index dcf743915..4c24bd1e0 100644 --- a/ground/openpilotgcs/src/plugins/config/configairframewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configairframewidget.cpp @@ -92,26 +92,7 @@ ConfigAirframeWidget::ConfigAirframeWidget(QWidget *parent) : ConfigTaskWidget(p m_aircraft->setupUi(this); setupButtons(m_aircraft->saveAircraftToRAM,m_aircraft->saveAircraftToSD); - addWidget(m_aircraft->customMixerTable); - addWidget(m_aircraft->customThrottle2Curve); - addWidget(m_aircraft->customThrottle1Curve); - addWidget(m_aircraft->multiThrottleCurve); - addWidget(m_aircraft->fixedWingThrottle); - addWidget(m_aircraft->fixedWingType); - addWidget(m_aircraft->feedForwardSlider); - addWidget(m_aircraft->accelTime); - addWidget(m_aircraft->decelTime); - addWidget(m_aircraft->maxAccelSlider); - addWidget(m_aircraft->multirotorFrameType); - addWidget(m_aircraft->multiMotor1); - addWidget(m_aircraft->multiMotor2); - addWidget(m_aircraft->multiMotor3); - addWidget(m_aircraft->multiMotor4); - addWidget(m_aircraft->multiMotor5); - addWidget(m_aircraft->multiMotor6); - addWidget(m_aircraft->multiMotor7); - addWidget(m_aircraft->multiMotor8); - addWidget(m_aircraft->triYawChannel); + addToDirtyMonitor(); addUAVObject("SystemSettings"); addUAVObject("MixerSettings"); addUAVObject("ActuatorSettings"); @@ -2139,3 +2120,78 @@ void ConfigAirframeWidget::openHelp() QDesktopServices::openUrl( QUrl("http://wiki.openpilot.org/display/Doc/Airframe+configuration", QUrl::StrictMode) ); } +void ConfigAirframeWidget::addToDirtyMonitor() +{ + addWidget(m_aircraft->customMixerTable); + addWidget(m_aircraft->customThrottle2Curve); + addWidget(m_aircraft->customThrottle1Curve); + addWidget(m_aircraft->multiThrottleCurve); + addWidget(m_aircraft->fixedWingThrottle); + addWidget(m_aircraft->fixedWingType); + addWidget(m_aircraft->feedForwardSlider); + addWidget(m_aircraft->accelTime); + addWidget(m_aircraft->decelTime); + addWidget(m_aircraft->maxAccelSlider); + addWidget(m_aircraft->multirotorFrameType); + addWidget(m_aircraft->multiMotor1); + addWidget(m_aircraft->multiMotor2); + addWidget(m_aircraft->multiMotor3); + addWidget(m_aircraft->multiMotor4); + addWidget(m_aircraft->multiMotor5); + addWidget(m_aircraft->multiMotor6); + addWidget(m_aircraft->multiMotor7); + addWidget(m_aircraft->multiMotor8); + addWidget(m_aircraft->triYawChannel); + addWidget(m_aircraft->aircraftType); + addWidget(m_aircraft->fwEngineChannel); + addWidget(m_aircraft->fwAileron1Channel); + addWidget(m_aircraft->fwAileron2Channel); + addWidget(m_aircraft->fwElevator1Channel); + addWidget(m_aircraft->fwElevator2Channel); + addWidget(m_aircraft->fwRudder1Channel); + addWidget(m_aircraft->fwRudder2Channel); + addWidget(m_aircraft->elevonSlider1); + addWidget(m_aircraft->elevonSlider2); + addWidget(m_aircraft->widget_3->m_ccpm->ccpmType); + addWidget(m_aircraft->widget_3->m_ccpm->TabObject); + addWidget(m_aircraft->widget_3->m_ccpm->ccpmTailChannel); + addWidget(m_aircraft->widget_3->m_ccpm->ccpmEngineChannel); + addWidget(m_aircraft->widget_3->m_ccpm->ccpmServoWChannel); + addWidget(m_aircraft->widget_3->m_ccpm->ccpmServoXChannel); + addWidget(m_aircraft->widget_3->m_ccpm->ccpmServoYChannel); + addWidget(m_aircraft->widget_3->m_ccpm->ccpmSingleServo); + addWidget(m_aircraft->widget_3->m_ccpm->ccpmServoZChannel); + addWidget(m_aircraft->widget_3->m_ccpm->ccpmAngleW); + addWidget(m_aircraft->widget_3->m_ccpm->ccpmAngleX); + addWidget(m_aircraft->widget_3->m_ccpm->ccpmCorrectionAngle); + addWidget(m_aircraft->widget_3->m_ccpm->ccpmAngleZ); + addWidget(m_aircraft->widget_3->m_ccpm->ccpmAngleY); + addWidget(m_aircraft->widget_3->m_ccpm->ccpmCollectivePassthrough); + addWidget(m_aircraft->widget_3->m_ccpm->ccpmLinkRoll); + addWidget(m_aircraft->widget_3->m_ccpm->ccpmLinkCyclic); + addWidget(m_aircraft->widget_3->m_ccpm->ccpmCollectiveChannel); + addWidget(m_aircraft->widget_3->m_ccpm->ccpmRevoSlider); + addWidget(m_aircraft->widget_3->m_ccpm->ccpmREVOspinBox); + addWidget(m_aircraft->widget_3->m_ccpm->ccpmCollectiveSlider); + addWidget(m_aircraft->widget_3->m_ccpm->ccpmCollectivespinBox); + addWidget(m_aircraft->widget_3->m_ccpm->ccpmCollectiveScale); + addWidget(m_aircraft->widget_3->m_ccpm->ccpmCollectiveScaleBox); + addWidget(m_aircraft->widget_3->m_ccpm->ccpmCyclicScale); + addWidget(m_aircraft->widget_3->m_ccpm->ccpmPitchScale); + addWidget(m_aircraft->widget_3->m_ccpm->ccpmPitchScaleBox); + addWidget(m_aircraft->widget_3->m_ccpm->ccpmRollScale); + addWidget(m_aircraft->widget_3->m_ccpm->ccpmRollScaleBox); + addWidget(m_aircraft->widget_3->m_ccpm->SwashLvlPositionSlider); + addWidget(m_aircraft->widget_3->m_ccpm->SwashLvlPositionSpinBox); + addWidget(m_aircraft->widget_3->m_ccpm->CurveType); + addWidget(m_aircraft->widget_3->m_ccpm->NumCurvePoints); + addWidget(m_aircraft->widget_3->m_ccpm->CurveValue1); + addWidget(m_aircraft->widget_3->m_ccpm->CurveValue2); + addWidget(m_aircraft->widget_3->m_ccpm->CurveValue3); + addWidget(m_aircraft->widget_3->m_ccpm->CurveToGenerate); + addWidget(m_aircraft->widget_3->m_ccpm->CurveSettings); + addWidget(m_aircraft->widget_3->m_ccpm->ThrottleCurve); + addWidget(m_aircraft->widget_3->m_ccpm->PitchCurve); + addWidget(m_aircraft->widget_3->m_ccpm->ccpmAdvancedSettingsTable); +} + diff --git a/ground/openpilotgcs/src/plugins/config/configairframewidget.h b/ground/openpilotgcs/src/plugins/config/configairframewidget.h index 234a114c1..1383350ee 100644 --- a/ground/openpilotgcs/src/plugins/config/configairframewidget.h +++ b/ground/openpilotgcs/src/plugins/config/configairframewidget.h @@ -58,7 +58,7 @@ private: void updateCustomAirframeUI(); bool setupMixer(double mixerFactors[8][3]); void setupMotors(QList motorList); - + void addToDirtyMonitor(); void resetField(UAVObjectField * field); void resetMixer (MixerCurveWidget *mixer, int numElements, double maxvalue); void resetActuators(); diff --git a/ground/openpilotgcs/src/plugins/config/configccpmwidget.h b/ground/openpilotgcs/src/plugins/config/configccpmwidget.h index d07da53c8..ab5425229 100644 --- a/ground/openpilotgcs/src/plugins/config/configccpmwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configccpmwidget.h @@ -82,6 +82,8 @@ public: ConfigccpmWidget(QWidget *parent = 0); ~ConfigccpmWidget(); + friend class ConfigAirframeWidget; + private: Ui_ccpmWidget *m_ccpm; QGraphicsSvgItem *SwashplateImg; diff --git a/ground/openpilotgcs/src/plugins/config/configtaskwidget.cpp b/ground/openpilotgcs/src/plugins/config/configtaskwidget.cpp index 8fd35c596..26524611f 100644 --- a/ground/openpilotgcs/src/plugins/config/configtaskwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configtaskwidget.cpp @@ -211,6 +211,7 @@ void ConfigTaskWidget::enableControls(bool enable) void ConfigTaskWidget::widgetsContentsChanged() { + qDebug()<<"dirty!!!"; dirty=true; } From 829b8b83f60cbc98d2d349fc40afe479e134854f Mon Sep 17 00:00:00 2001 From: Stacey Sheldon Date: Thu, 28 Jul 2011 08:22:47 -0400 Subject: [PATCH 023/145] rcvr: Add GCS receiver driver for rcvr via telemetry This allows the GCS to emulate a receiver device via the telemetry link. Select "GCS" as your input type in the manualcontrol config screen and calibrate it as normal. Note: The expected values for the channels are in microseconds just like a PWM or PPM input device. The channel values are validated against minimum/maximum pulse lengths just like normal receivers. --- flight/CopterControl/Makefile | 2 + flight/CopterControl/System/inc/pios_config.h | 1 + flight/CopterControl/System/pios_board.c | 20 +++++ flight/PiOS/Common/pios_gcsrcvr.c | 75 +++++++++++++++++++ flight/PiOS/inc/pios_gcsrcvr_priv.h | 47 ++++++++++++ flight/PiOS/inc/pios_spektrum_priv.h | 2 +- .../src/plugins/uavobjects/uavobjects.pro | 2 + shared/uavobjectdefinition/gcsreceiver.xml | 10 +++ .../manualcontrolsettings.xml | 2 +- 9 files changed, 159 insertions(+), 2 deletions(-) create mode 100644 flight/PiOS/Common/pios_gcsrcvr.c create mode 100644 flight/PiOS/inc/pios_gcsrcvr_priv.h create mode 100644 shared/uavobjectdefinition/gcsreceiver.xml diff --git a/flight/CopterControl/Makefile b/flight/CopterControl/Makefile index 78e62bb89..3ac59e59b 100644 --- a/flight/CopterControl/Makefile +++ b/flight/CopterControl/Makefile @@ -164,6 +164,7 @@ SRC += $(OPUAVSYNTHDIR)/mixerstatus.c SRC += $(OPUAVSYNTHDIR)/firmwareiapobj.c SRC += $(OPUAVSYNTHDIR)/attitudesettings.c SRC += $(OPUAVSYNTHDIR)/hwsettings.c +SRC += $(OPUAVSYNTHDIR)/gcsreceiver.c #${wildcard ${OBJ}/$(shell echo $(VAR) | tr A-Z a-z)/*.c} #SRC += ${foreach OBJ, ${UAVOBJECTS}, $(UAVOBJECTS)/$(OBJ).c} # Cant use until i can automatically generate list of UAVObjects @@ -207,6 +208,7 @@ SRC += $(PIOSCOMMON)/pios_i2c_esc.c SRC += $(PIOSCOMMON)/pios_iap.c SRC += $(PIOSCOMMON)/pios_bl_helper.c SRC += $(PIOSCOMMON)/pios_rcvr.c +SRC += $(PIOSCOMMON)/pios_gcsrcvr.c SRC += $(PIOSCOMMON)/printf-stdarg.c ## Libraries for flight calculations SRC += $(FLIGHTLIB)/fifo_buffer.c diff --git a/flight/CopterControl/System/inc/pios_config.h b/flight/CopterControl/System/inc/pios_config.h index 944fe38c8..c2d535709 100644 --- a/flight/CopterControl/System/inc/pios_config.h +++ b/flight/CopterControl/System/inc/pios_config.h @@ -50,6 +50,7 @@ #define PIOS_INCLUDE_SBUS //#define PIOS_INCLUDE_PPM #define PIOS_INCLUDE_PWM +#define PIOS_INCLUDE_GCSRCVR /* Supported USART-based PIOS modules */ #define PIOS_INCLUDE_TELEMETRY_RF diff --git a/flight/CopterControl/System/pios_board.c b/flight/CopterControl/System/pios_board.c index ee3deaf5c..50677de27 100644 --- a/flight/CopterControl/System/pios_board.c +++ b/flight/CopterControl/System/pios_board.c @@ -856,6 +856,10 @@ void PIOS_I2C_main_adapter_er_irq_handler(void) #endif /* PIOS_INCLUDE_I2C */ +#if defined(PIOS_INCLUDE_GCSRCVR) +#include "pios_gcsrcvr_priv.h" +#endif /* PIOS_INCLUDE_GCSRCVR */ + #if defined(PIOS_INCLUDE_RCVR) #include "pios_rcvr_priv.h" @@ -1142,6 +1146,22 @@ void PIOS_Board_Init(void) { } #endif /* PIOS_INCLUDE_SBUS */ break; + case MANUALCONTROLSETTINGS_INPUTMODE_GCS: +#if defined(PIOS_INCLUDE_GCSRCVR) + PIOS_GCSRCVR_Init(); + uint32_t pios_gcsrcvr_rcvr_id; + if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, 0)) { + PIOS_Assert(0); + } + for (uint8_t i = 0; + i < GCSRECEIVER_CHANNEL_NUMELEM && pios_rcvr_max_channel < NELEMENTS(pios_rcvr_channel_to_id_map); + i++) { + pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].id = pios_gcsrcvr_rcvr_id; + pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].channel = i; + pios_rcvr_max_channel++; + } +#endif /* PIOS_INCLUDE_GCSRCVR */ + break; } /* Remap AFIO pin */ diff --git a/flight/PiOS/Common/pios_gcsrcvr.c b/flight/PiOS/Common/pios_gcsrcvr.c new file mode 100644 index 000000000..18e6644a6 --- /dev/null +++ b/flight/PiOS/Common/pios_gcsrcvr.c @@ -0,0 +1,75 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_GCSRCVR GCS Receiver Input Functions + * @brief Code to read the channels within the GCS Receiver UAVObject + * @{ + * + * @file pios_gcsrcvr.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief GCS Input functions (STM32 dependent) + * @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 + */ + +/* Project Includes */ +#include "pios.h" + +#if defined(PIOS_INCLUDE_GCSRCVR) + +#include "pios_gcsrcvr_priv.h" + +static GCSReceiverData gcsreceiverdata; + +/* Provide a RCVR driver */ +static int32_t PIOS_GCSRCVR_Get(uint32_t rcvr_id, uint8_t channel); + +const struct pios_rcvr_driver pios_gcsrcvr_rcvr_driver = { + .read = PIOS_GCSRCVR_Get, +}; + +static void gcsreceiver_updated(UAVObjEvent * ev) +{ + if (ev->obj == GCSReceiverHandle()) { + GCSReceiverGet(&gcsreceiverdata); + } +} + +void PIOS_GCSRCVR_Init(void) +{ + /* Register uavobj callback */ + GCSReceiverConnectCallback (gcsreceiver_updated); +} + +static int32_t PIOS_GCSRCVR_Get(uint32_t rcvr_id, uint8_t channel) +{ + if (channel >= GCSRECEIVER_CHANNEL_NUMELEM) { + /* channel is out of range */ + return -1; + } + + return (gcsreceiverdata.Channel[channel]); +} + +#endif /* PIOS_INCLUDE_GCSRCVR */ + +/** + * @} + * @} + */ diff --git a/flight/PiOS/inc/pios_gcsrcvr_priv.h b/flight/PiOS/inc/pios_gcsrcvr_priv.h new file mode 100644 index 000000000..9a828cb95 --- /dev/null +++ b/flight/PiOS/inc/pios_gcsrcvr_priv.h @@ -0,0 +1,47 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_GCSRCVR GCS Receiver Functions + * @brief PIOS interface to read from GCS receiver port + * @{ + * + * @file pios_gcsrcvr_priv.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief GCS receiver private functions + * @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_GCSRCVR_PRIV_H +#define PIOS_GCSRCVR_PRIV_H + +#include + +#include "gcsreceiver.h" + +extern const struct pios_rcvr_driver pios_gcsrcvr_rcvr_driver; + +extern void PIOS_GCSRCVR_Init(void); + +#endif /* PIOS_GCSRCVR_PRIV_H */ + +/** + * @} + * @} + */ diff --git a/flight/PiOS/inc/pios_spektrum_priv.h b/flight/PiOS/inc/pios_spektrum_priv.h index 2d31a8027..eb82c4f44 100644 --- a/flight/PiOS/inc/pios_spektrum_priv.h +++ b/flight/PiOS/inc/pios_spektrum_priv.h @@ -44,7 +44,7 @@ extern const struct pios_rcvr_driver pios_spektrum_rcvr_driver; extern int32_t PIOS_SPEKTRUM_Init(uint32_t * spektrum_id, const struct pios_spektrum_cfg *cfg, const struct pios_com_driver * driver, uint32_t lower_id, bool bind); -#endif /* PIOS_PWM_PRIV_H */ +#endif /* PIOS_SPEKTRUM_PRIV_H */ /** * @} diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index 9c6188fad..3831a4ec3 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -69,6 +69,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/sonaraltitude.h \ $$UAVOBJECT_SYNTHETICS/flightstatus.h \ $$UAVOBJECT_SYNTHETICS/hwsettings.h \ + $$UAVOBJECT_SYNTHETICS/gcsreceiver.h \ $$UAVOBJECT_SYNTHETICS/attitudesettings.h SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ @@ -118,4 +119,5 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/uavobjectsinit.cpp \ $$UAVOBJECT_SYNTHETICS/flightstatus.cpp \ $$UAVOBJECT_SYNTHETICS/hwsettings.cpp \ + $$UAVOBJECT_SYNTHETICS/gcsreceiver.cpp \ $$UAVOBJECT_SYNTHETICS/attitudesettings.cpp diff --git a/shared/uavobjectdefinition/gcsreceiver.xml b/shared/uavobjectdefinition/gcsreceiver.xml new file mode 100644 index 000000000..d21598e48 --- /dev/null +++ b/shared/uavobjectdefinition/gcsreceiver.xml @@ -0,0 +1,10 @@ + + + A receiver channel group carried over the telemetry link. + + + + + + + diff --git a/shared/uavobjectdefinition/manualcontrolsettings.xml b/shared/uavobjectdefinition/manualcontrolsettings.xml index bfd692f91..3b76d70f8 100644 --- a/shared/uavobjectdefinition/manualcontrolsettings.xml +++ b/shared/uavobjectdefinition/manualcontrolsettings.xml @@ -1,7 +1,7 @@ Settings to indicate how to decode receiver input by @ref ManualControlModule. - + From 06cdeb7b61dc83f9cf245e9851686013655bbc58 Mon Sep 17 00:00:00 2001 From: Stacey Sheldon Date: Sat, 30 Jul 2011 15:04:24 -0400 Subject: [PATCH 024/145] rcvr: support multiple simultaneous receivers Now also supports multiple instances of the Spektrum driver. These are configured as Spektrum1 and Spektrum2. --- flight/CopterControl/System/pios_board.c | 124 +++---- flight/Modules/ManualControl/manualcontrol.c | 63 ++-- flight/OpenPilot/System/pios_board.c | 219 ++++++------- flight/OpenPilot/UAVObjects.inc | 1 + flight/PiOS/Boards/STM32103CB_CC_Rev1.h | 3 +- flight/PiOS/Boards/STM3210E_OP.h | 1 + flight/PiOS/STM32F10x/pios_spektrum.c | 303 +++++++++++------- flight/PiOS/inc/pios_rcvr.h | 7 - shared/uavobjectdefinition/hwsettings.xml | 10 +- .../manualcontrolsettings.xml | 24 +- 10 files changed, 413 insertions(+), 342 deletions(-) diff --git a/flight/CopterControl/System/pios_board.c b/flight/CopterControl/System/pios_board.c index 50677de27..59b66ea3a 100644 --- a/flight/CopterControl/System/pios_board.c +++ b/flight/CopterControl/System/pios_board.c @@ -863,8 +863,12 @@ void PIOS_I2C_main_adapter_er_irq_handler(void) #if defined(PIOS_INCLUDE_RCVR) #include "pios_rcvr_priv.h" -struct pios_rcvr_channel_map pios_rcvr_channel_to_id_map[PIOS_RCVR_MAX_CHANNELS]; -uint32_t pios_rcvr_max_channel; +/* One slot per selectable receiver group. + * eg. PWM, PPM, GCS, SPEKTRUM1, SPEKTRUM2, SBUS + * NOTE: No slot in this map for NONE. + */ +uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE]; + #endif /* PIOS_INCLUDE_RCVR */ #if defined(PIOS_INCLUDE_USB_HID) @@ -984,7 +988,8 @@ void PIOS_Board_Init(void) { } #endif /* PIOS_INCLUDE_GPS */ break; - case HWSETTINGS_CC_MAINPORT_SPEKTRUM: + case HWSETTINGS_CC_MAINPORT_SPEKTRUM1: + case HWSETTINGS_CC_MAINPORT_SPEKTRUM2: #if defined(PIOS_INCLUDE_SPEKTRUM) { uint32_t pios_usart_spektrum_id; @@ -996,6 +1001,16 @@ void PIOS_Board_Init(void) { if (PIOS_SPEKTRUM_Init(&pios_spektrum_id, &pios_spektrum_main_cfg, &pios_usart_com_driver, pios_usart_spektrum_id, false)) { PIOS_Assert(0); } + + uint32_t pios_spektrum_rcvr_id; + if (PIOS_RCVR_Init(&pios_spektrum_rcvr_id, &pios_spektrum_rcvr_driver, pios_spektrum_id)) { + PIOS_Assert(0); + } + if (hwsettings_cc_mainport == HWSETTINGS_CC_MAINPORT_SPEKTRUM1) { + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SPEKTRUM1] = pios_spektrum_rcvr_id; + } else { + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SPEKTRUM2] = pios_spektrum_rcvr_id; + } } #endif /* PIOS_INCLUDE_SPEKTRUM */ break; @@ -1046,7 +1061,8 @@ void PIOS_Board_Init(void) { } #endif /* PIOS_INCLUDE_GPS */ break; - case HWSETTINGS_CC_FLEXIPORT_SPEKTRUM: + case HWSETTINGS_CC_FLEXIPORT_SPEKTRUM1: + case HWSETTINGS_CC_FLEXIPORT_SPEKTRUM2: #if defined(PIOS_INCLUDE_SPEKTRUM) { uint32_t pios_usart_spektrum_id; @@ -1058,6 +1074,16 @@ void PIOS_Board_Init(void) { if (PIOS_SPEKTRUM_Init(&pios_spektrum_id, &pios_spektrum_flexi_cfg, &pios_usart_com_driver, pios_usart_spektrum_id, false)) { PIOS_Assert(0); } + + uint32_t pios_spektrum_rcvr_id; + if (PIOS_RCVR_Init(&pios_spektrum_rcvr_id, &pios_spektrum_rcvr_driver, pios_spektrum_id)) { + PIOS_Assert(0); + } + if (hwsettings_cc_flexiport == HWSETTINGS_CC_FLEXIPORT_SPEKTRUM1) { + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SPEKTRUM1] = pios_spektrum_rcvr_id; + } else { + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SPEKTRUM2] = pios_spektrum_rcvr_id; + } } #endif /* PIOS_INCLUDE_SPEKTRUM */ break; @@ -1074,96 +1100,44 @@ void PIOS_Board_Init(void) { break; } - /* Configure the selected receiver */ - uint8_t manualcontrolsettings_inputmode; - ManualControlSettingsInputModeGet(&manualcontrolsettings_inputmode); + /* Configure the rcvr port */ + uint8_t hwsettings_rcvrport; + HwSettingsRcvrPortGet(&hwsettings_rcvrport); - switch (manualcontrolsettings_inputmode) { - case MANUALCONTROLSETTINGS_INPUTMODE_PWM: + switch (hwsettings_rcvrport) { + case HWSETTINGS_RCVRPORT_DISABLED: + break; + case HWSETTINGS_RCVRPORT_PWM: #if defined(PIOS_INCLUDE_PWM) PIOS_PWM_Init(); uint32_t pios_pwm_rcvr_id; if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, 0)) { PIOS_Assert(0); } - for (uint8_t i = 0; - i < PIOS_PWM_NUM_INPUTS && pios_rcvr_max_channel < NELEMENTS(pios_rcvr_channel_to_id_map); - i++) { - pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].id = pios_pwm_rcvr_id; - pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].channel = i; - pios_rcvr_max_channel++; - } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id; #endif /* PIOS_INCLUDE_PWM */ break; - case MANUALCONTROLSETTINGS_INPUTMODE_PPM: + case HWSETTINGS_RCVRPORT_PPM: #if defined(PIOS_INCLUDE_PPM) PIOS_PPM_Init(); uint32_t pios_ppm_rcvr_id; if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, 0)) { PIOS_Assert(0); } - for (uint8_t i = 0; - i < PIOS_PPM_NUM_INPUTS && pios_rcvr_max_channel < NELEMENTS(pios_rcvr_channel_to_id_map); - i++) { - pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].id = pios_ppm_rcvr_id; - pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].channel = i; - pios_rcvr_max_channel++; - } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; #endif /* PIOS_INCLUDE_PPM */ break; - case MANUALCONTROLSETTINGS_INPUTMODE_SPEKTRUM: -#if defined(PIOS_INCLUDE_SPEKTRUM) - if (hwsettings_cc_mainport == HWSETTINGS_CC_MAINPORT_SPEKTRUM || - hwsettings_cc_flexiport == HWSETTINGS_CC_FLEXIPORT_SPEKTRUM) { - uint32_t pios_spektrum_rcvr_id; - if (PIOS_RCVR_Init(&pios_spektrum_rcvr_id, &pios_spektrum_rcvr_driver, 0)) { - PIOS_Assert(0); - } - for (uint8_t i = 0; - i < PIOS_SPEKTRUM_NUM_INPUTS && pios_rcvr_max_channel < NELEMENTS(pios_rcvr_channel_to_id_map); - i++) { - pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].id = pios_spektrum_rcvr_id; - pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].channel = i; - pios_rcvr_max_channel++; - } - } -#endif /* PIOS_INCLUDE_SPEKTRUM */ - break; - case MANUALCONTROLSETTINGS_INPUTMODE_SBUS: -#if defined(PIOS_INCLUDE_SBUS) - if (hwsettings_cc_mainport == HWSETTINGS_CC_MAINPORT_SBUS) { - uint32_t pios_sbus_rcvr_id; - if (PIOS_RCVR_Init(&pios_sbus_rcvr_id, &pios_sbus_rcvr_driver, 0)) { - PIOS_Assert(0); - } - for (uint8_t i = 0; - i < SBUS_NUMBER_OF_CHANNELS && pios_rcvr_max_channel < NELEMENTS(pios_rcvr_channel_to_id_map); - i++) { - pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].id = pios_sbus_rcvr_id; - pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].channel = i; - pios_rcvr_max_channel++; - } - } -#endif /* PIOS_INCLUDE_SBUS */ - break; - case MANUALCONTROLSETTINGS_INPUTMODE_GCS: -#if defined(PIOS_INCLUDE_GCSRCVR) - PIOS_GCSRCVR_Init(); - uint32_t pios_gcsrcvr_rcvr_id; - if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, 0)) { - PIOS_Assert(0); - } - for (uint8_t i = 0; - i < GCSRECEIVER_CHANNEL_NUMELEM && pios_rcvr_max_channel < NELEMENTS(pios_rcvr_channel_to_id_map); - i++) { - pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].id = pios_gcsrcvr_rcvr_id; - pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].channel = i; - pios_rcvr_max_channel++; - } -#endif /* PIOS_INCLUDE_GCSRCVR */ - break; } +#if defined(PIOS_INCLUDE_GCSRCVR) + PIOS_GCSRCVR_Init(); + uint32_t pios_gcsrcvr_rcvr_id; + if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, 0)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id; +#endif /* PIOS_INCLUDE_GCSRCVR */ + /* Remap AFIO pin */ GPIO_PinRemapConfig( GPIO_Remap_SWJ_NoJTRST, ENABLE); PIOS_Servo_Init(); diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c index 19b6bc293..7c16ac172 100644 --- a/flight/Modules/ManualControl/manualcontrol.c +++ b/flight/Modules/ManualControl/manualcontrol.c @@ -141,7 +141,7 @@ static void manualControlTask(void *parameters) // Main task loop lastSysTime = xTaskGetTickCount(); while (1) { - float scaledChannel[MANUALCONTROLCOMMAND_CHANNEL_NUMELEM]; + float scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM]; // Wait until next update vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS); @@ -165,22 +165,28 @@ static void manualControlTask(void *parameters) if (!ManualControlCommandReadOnly(&cmd)) { // Read channel values in us - for (int n = 0; n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) { - if (pios_rcvr_channel_to_id_map[n].id) { - cmd.Channel[n] = PIOS_RCVR_Read(pios_rcvr_channel_to_id_map[n].id, - pios_rcvr_channel_to_id_map[n].channel); - } else { + for (uint8_t n = 0; + n < MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM && n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; + ++n) { + extern uint32_t pios_rcvr_group_map[]; + + if (settings.ChannelGroups[n] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { cmd.Channel[n] = -1; + } else if (!pios_rcvr_group_map[settings.ChannelGroups[n]]) { + cmd.Channel[n] = -2; + } else { + cmd.Channel[n] = PIOS_RCVR_Read(pios_rcvr_group_map[settings.ChannelGroups[n]], + settings.ChannelNumber[n]); } scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n], settings.ChannelMin[n], settings.ChannelNeutral[n]); } // Check settings, if error raise alarm - if (settings.Roll >= MANUALCONTROLSETTINGS_ROLL_NONE || - settings.Pitch >= MANUALCONTROLSETTINGS_PITCH_NONE || - settings.Yaw >= MANUALCONTROLSETTINGS_YAW_NONE || - settings.Throttle >= MANUALCONTROLSETTINGS_THROTTLE_NONE || - settings.FlightMode >= MANUALCONTROLSETTINGS_FLIGHTMODE_NONE) { + if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE || + settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE || + settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE || + settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE || + settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; ManualControlCommandSet(&cmd); @@ -188,10 +194,10 @@ static void manualControlTask(void *parameters) } // decide if we have valid manual input or not - bool valid_input_detected = validInputRange(settings.ChannelMin[settings.Throttle], settings.ChannelMax[settings.Throttle], cmd.Channel[settings.Throttle]) && - validInputRange(settings.ChannelMin[settings.Roll], settings.ChannelMax[settings.Roll], cmd.Channel[settings.Roll]) && - validInputRange(settings.ChannelMin[settings.Yaw], settings.ChannelMax[settings.Yaw], cmd.Channel[settings.Yaw]) && - validInputRange(settings.ChannelMin[settings.Pitch], settings.ChannelMax[settings.Pitch], cmd.Channel[settings.Pitch]); + bool valid_input_detected = validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]) && + validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]) && + validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]) && + validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]); // Implement hysteresis loop on connection status if (valid_input_detected && (++connected_count > 10)) { @@ -218,28 +224,31 @@ static void manualControlTask(void *parameters) AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL); // Scale channels to -1 -> +1 range - cmd.Roll = scaledChannel[settings.Roll]; - cmd.Pitch = scaledChannel[settings.Pitch]; - cmd.Yaw = scaledChannel[settings.Yaw]; - cmd.Throttle = scaledChannel[settings.Throttle]; - flightMode = scaledChannel[settings.FlightMode]; + cmd.Roll = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]; + cmd.Pitch = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]; + cmd.Yaw = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]; + cmd.Throttle = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]; + flightMode = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE]; AccessoryDesiredData accessory; // Set Accessory 0 - if(settings.Accessory0 != MANUALCONTROLSETTINGS_ACCESSORY0_NONE) { - accessory.AccessoryVal = scaledChannel[settings.Accessory0]; + if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] != + MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0]; if(AccessoryDesiredInstSet(0, &accessory) != 0) AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); } // Set Accessory 1 - if(settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY1_NONE) { - accessory.AccessoryVal = scaledChannel[settings.Accessory1]; + if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] != + MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1]; if(AccessoryDesiredInstSet(1, &accessory) != 0) AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); } - // Set Accsesory 2 - if(settings.Accessory2 != MANUALCONTROLSETTINGS_ACCESSORY2_NONE) { - accessory.AccessoryVal = scaledChannel[settings.Accessory2]; + // Set Accessory 2 + if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] != + MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2]; if(AccessoryDesiredInstSet(2, &accessory) != 0) AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); } diff --git a/flight/OpenPilot/System/pios_board.c b/flight/OpenPilot/System/pios_board.c index 822b7a8ae..9a9397acb 100644 --- a/flight/OpenPilot/System/pios_board.c +++ b/flight/OpenPilot/System/pios_board.c @@ -30,6 +30,7 @@ #include #include #include +#include #include "manualcontrolsettings.h" //#define I2C_DEBUG_PIN 0 @@ -496,7 +497,6 @@ static const struct pios_usart_cfg pios_usart_spektrum_cfg = { }, }; -#include static const struct pios_spektrum_cfg pios_spektrum_cfg = { .bind = { .gpio = GPIOA, @@ -964,8 +964,12 @@ static const struct stm32_gpio pios_debug_pins[] = { #if defined(PIOS_INCLUDE_RCVR) #include "pios_rcvr_priv.h" -struct pios_rcvr_channel_map pios_rcvr_channel_to_id_map[PIOS_RCVR_MAX_CHANNELS]; -uint32_t pios_rcvr_max_channel; +/* One slot per selectable receiver group. + * eg. PWM, PPM, GCS, SPEKTRUM1, SPEKTRUM2, SBUS + * NOTE: No slot in this map for NONE. + */ +uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE]; + #endif /* PIOS_INCLUDE_RCVR */ #if defined(PIOS_INCLUDE_USB_HID) @@ -1047,126 +1051,125 @@ void PIOS_Board_Init(void) { /* Bind the AHRS comms layer to the AHRS SPI link */ AhrsConnect(pios_spi_ahrs_id); - /* Initialize the PiOS library */ -#if defined(PIOS_INCLUDE_COM) + /* Configure the main IO port */ + uint8_t hwsettings_op_mainport; + HwSettingsOP_MainPortGet(&hwsettings_op_mainport); + + switch (hwsettings_op_mainport) { + case HWSETTINGS_OP_MAINPORT_DISABLED: + break; + case HWSETTINGS_OP_MAINPORT_TELEMETRY: #if defined(PIOS_INCLUDE_TELEMETRY_RF) - { - uint32_t pios_usart_telem_rf_id; - if (PIOS_USART_Init(&pios_usart_telem_rf_id, &pios_usart_telem_cfg)) { - PIOS_Assert(0); - } + { + uint32_t pios_usart_telem_rf_id; + if (PIOS_USART_Init(&pios_usart_telem_rf_id, &pios_usart_telem_cfg)) { + PIOS_Assert(0); + } - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN); - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_telem_rf_id, - rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) { - PIOS_Assert(0); + uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN); + uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_telem_rf_id, + rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN, + tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) { + PIOS_Assert(0); + } } - } #endif /* PIOS_INCLUDE_TELEMETRY_RF */ - -#if defined(PIOS_INCLUDE_GPS) - { - uint32_t pios_usart_gps_id; - if (PIOS_USART_Init(&pios_usart_gps_id, &pios_usart_gps_cfg)) { - PIOS_Assert(0); - } - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN); - PIOS_Assert(rx_buffer); - if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id, - rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, - NULL, 0)) { - PIOS_Assert(0); - } + break; } + + /* Configure the flexi port */ + uint8_t hwsettings_op_flexiport; + HwSettingsOP_FlexiPortGet(&hwsettings_op_flexiport); + + switch (hwsettings_op_flexiport) { + case HWSETTINGS_OP_FLEXIPORT_DISABLED: + break; + case HWSETTINGS_OP_FLEXIPORT_GPS: +#if defined(PIOS_INCLUDE_GPS) + { + uint32_t pios_usart_gps_id; + if (PIOS_USART_Init(&pios_usart_gps_id, &pios_usart_gps_cfg)) { + PIOS_Assert(0); + } + uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN); + PIOS_Assert(rx_buffer); + if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id, + rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, + NULL, 0)) { + PIOS_Assert(0); + } + } #endif /* PIOS_INCLUDE_GPS */ -#endif + break; + } PIOS_Servo_Init(); PIOS_ADC_Init(); PIOS_GPIO_Init(); - /* Configure the selected receiver */ - uint8_t manualcontrolsettings_inputmode; - ManualControlSettingsInputModeGet(&manualcontrolsettings_inputmode); + /* Configure the aux port */ + uint8_t hwsettings_op_auxport; + HwSettingsOP_AuxPortGet(&hwsettings_op_auxport); - switch (manualcontrolsettings_inputmode) { - case MANUALCONTROLSETTINGS_INPUTMODE_PWM: -#if defined(PIOS_INCLUDE_PWM) -#if (PIOS_PWM_NUM_INPUTS > PIOS_RCVR_MAX_CHANNELS) -#error More receiver inputs than available devices -#endif - PIOS_PWM_Init(); - uint32_t pios_pwm_rcvr_id; - if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, 0)) { - PIOS_Assert(0); - } - for (uint8_t i = 0; - i < PIOS_PWM_NUM_INPUTS && pios_rcvr_max_channel < NELEMENTS(pios_rcvr_channel_to_id_map); - i++) { - pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].id = pios_pwm_rcvr_id; - pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].channel = i; - pios_rcvr_max_channel++; - } -#endif /* PIOS_INCLUDE_PWM */ - break; - case MANUALCONTROLSETTINGS_INPUTMODE_PPM: -#if defined(PIOS_INCLUDE_PPM) -#if (PIOS_PPM_NUM_INPUTS > PIOS_RCVR_MAX_CHANNELS) -#error More receiver inputs than available devices -#endif - PIOS_PPM_Init(); - uint32_t pios_ppm_rcvr_id; - if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, 0)) { - PIOS_Assert(0); - } - for (uint8_t i = 0; - i < PIOS_PPM_NUM_INPUTS && pios_rcvr_max_channel < NELEMENTS(pios_rcvr_channel_to_id_map); - i++) { - pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].id = pios_ppm_rcvr_id; - pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].channel = i; - pios_rcvr_max_channel++; - } -#endif /* PIOS_INCLUDE_PPM */ - break; - case MANUALCONTROLSETTINGS_INPUTMODE_SPEKTRUM: + switch (hwsettings_op_auxport) { + case HWSETTINGS_OP_AUXPORT_DISABLED: + break; + case HWSETTINGS_OP_AUXPORT_DEBUG: + /* Not supported yet */ + break; + case HWSETTINGS_OP_AUXPORT_SPEKTRUM1: #if defined(PIOS_INCLUDE_SPEKTRUM) -#if (PIOS_SPEKTRUM_NUM_INPUTS > PIOS_RCVR_MAX_CHANNELS) -#error More receiver inputs than available devices -#endif - { - uint32_t pios_usart_spektrum_id; - if (PIOS_USART_Init(&pios_usart_spektrum_id, &pios_usart_spektrum_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_spektrum_id; - if (PIOS_SPEKTRUM_Init(&pios_spektrum_id, &pios_spektrum_cfg, &pios_usart_com_driver, pios_usart_spektrum_id, false)) { - PIOS_Assert(0); - } - - uint32_t pios_spektrum_rcvr_id; - if (PIOS_RCVR_Init(&pios_spektrum_rcvr_id, &pios_spektrum_rcvr_driver, 0)) { - PIOS_Assert(0); - } - for (uint8_t i = 0; - i < PIOS_SPEKTRUM_NUM_INPUTS && pios_rcvr_max_channel < NELEMENTS(pios_rcvr_channel_to_id_map); - i++) { - pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].id = pios_spektrum_rcvr_id; - pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].channel = i; - pios_rcvr_max_channel++; - } + { + uint32_t pios_usart_spektrum_id; + if (PIOS_USART_Init(&pios_usart_spektrum_id, &pios_usart_spektrum_cfg)) { + PIOS_Assert(0); } + + uint32_t pios_spektrum_id; + if (PIOS_SPEKTRUM_Init(&pios_spektrum_id, &pios_spektrum_cfg, &pios_usart_com_driver, pios_usart_spektrum_id, false)) { + PIOS_Assert(0); + } + + uint32_t pios_spektrum_rcvr_id; + if (PIOS_RCVR_Init(&pios_spektrum_rcvr_id, &pios_spektrum_rcvr_driver, pios_spektrum_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SPEKTRUM1] = pios_spektrum_rcvr_id; + } #endif - break; - case MANUALCONTROLSETTINGS_INPUTMODE_SBUS: -#if defined(PIOS_INCLUDE_SBUS) -#error SBUS NOT ON OP YET -#endif /* PIOS_INCLUDE_SBUS */ - break; + break; + } + + /* Configure the rcvr port */ + uint8_t hwsettings_rcvrport; + HwSettingsRcvrPortGet(&hwsettings_rcvrport); + + switch (hwsettings_rcvrport) { + case HWSETTINGS_RCVRPORT_DISABLED: + break; + case HWSETTINGS_RCVRPORT_PWM: +#if defined(PIOS_INCLUDE_PWM) + PIOS_PWM_Init(); + uint32_t pios_pwm_rcvr_id; + if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, 0)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id; +#endif /* PIOS_INCLUDE_PWM */ + break; + case HWSETTINGS_RCVRPORT_PPM: +#if defined(PIOS_INCLUDE_PPM) + PIOS_PPM_Init(); + uint32_t pios_ppm_rcvr_id; + if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, 0)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; +#endif /* PIOS_INCLUDE_PPM */ + break; } #if defined(PIOS_INCLUDE_USB_HID) diff --git a/flight/OpenPilot/UAVObjects.inc b/flight/OpenPilot/UAVObjects.inc index 3eb2fa571..a30dcf540 100644 --- a/flight/OpenPilot/UAVObjects.inc +++ b/flight/OpenPilot/UAVObjects.inc @@ -69,6 +69,7 @@ UAVOBJSRCFILENAMES += velocityactual UAVOBJSRCFILENAMES += velocitydesired UAVOBJSRCFILENAMES += watchdogstatus UAVOBJSRCFILENAMES += flightstatus +UAVOBJSRCFILENAMES += hwsettings UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(UAVOBJSYNTHDIR)/$(UAVOBJSRCFILE).c ) UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) ) diff --git a/flight/PiOS/Boards/STM32103CB_CC_Rev1.h b/flight/PiOS/Boards/STM32103CB_CC_Rev1.h index 18e0da624..8c1467b0d 100644 --- a/flight/PiOS/Boards/STM32103CB_CC_Rev1.h +++ b/flight/PiOS/Boards/STM32103CB_CC_Rev1.h @@ -206,7 +206,7 @@ extern uint32_t pios_com_telem_usb_id; // PIOS_RCVR // See also pios_board.c //------------------------ -#define PIOS_RCVR_MAX_DEVS 1 +#define PIOS_RCVR_MAX_DEVS 3 #define PIOS_RCVR_MAX_CHANNELS 12 //------------------------- @@ -222,6 +222,7 @@ extern uint32_t pios_com_telem_usb_id; //------------------------- // Receiver SPEKTRUM input //------------------------- +#define PIOS_SPEKTRUM_MAX_DEVS 2 #define PIOS_SPEKTRUM_NUM_INPUTS 12 //------------------------- diff --git a/flight/PiOS/Boards/STM3210E_OP.h b/flight/PiOS/Boards/STM3210E_OP.h index 3098ee259..159ebbd88 100644 --- a/flight/PiOS/Boards/STM3210E_OP.h +++ b/flight/PiOS/Boards/STM3210E_OP.h @@ -195,6 +195,7 @@ extern uint32_t pios_com_aux_id; //------------------------- // Receiver SPEKTRUM input //------------------------- +#define PIOS_SPEKTRUM_MAX_DEVS 1 #define PIOS_SPEKTRUM_NUM_INPUTS 12 //------------------------- diff --git a/flight/PiOS/STM32F10x/pios_spektrum.c b/flight/PiOS/STM32F10x/pios_spektrum.c index 8b0f38c6b..e062e6314 100644 --- a/flight/PiOS/STM32F10x/pios_spektrum.c +++ b/flight/PiOS/STM32F10x/pios_spektrum.c @@ -31,10 +31,11 @@ /* Project Includes */ #include "pios.h" -#include "pios_spektrum_priv.h" #if defined(PIOS_INCLUDE_SPEKTRUM) +#include "pios_spektrum_priv.h" + /** * @Note Framesyncing: * The code resets the watchdog timer whenever a single byte is received, so what watchdog code @@ -52,22 +53,80 @@ const struct pios_rcvr_driver pios_spektrum_rcvr_driver = { .read = PIOS_SPEKTRUM_Get, }; -/* Local Variables */ -static uint16_t CaptureValue[PIOS_SPEKTRUM_NUM_INPUTS],CaptureValueTemp[PIOS_SPEKTRUM_NUM_INPUTS]; -static uint8_t prev_byte = 0xFF, sync = 0, bytecount = 0, datalength=0, frame_error=0, byte_array[20] = { 0 }; -uint8_t sync_of = 0; -uint16_t supv_timer=0; +enum pios_spektrum_dev_magic { + PIOS_SPEKTRUM_DEV_MAGIC = 0xa9b9c9d9, +}; + +struct pios_spektrum_fsm { + uint16_t channel; + uint16_t CaptureValue[PIOS_SPEKTRUM_NUM_INPUTS]; + uint16_t CaptureValueTemp[PIOS_SPEKTRUM_NUM_INPUTS]; + uint8_t prev_byte; + uint8_t sync; + uint8_t bytecount; + uint8_t datalength; + uint8_t frame_error; + uint8_t sync_of; +}; + +struct pios_spektrum_dev { + enum pios_spektrum_dev_magic magic; + const struct pios_spektrum_cfg * cfg; + + struct pios_spektrum_fsm fsm; + + uint16_t supv_timer; +}; + +static bool PIOS_SPEKTRUM_validate(struct pios_spektrum_dev * spektrum_dev) +{ + return (spektrum_dev->magic == PIOS_SPEKTRUM_DEV_MAGIC); +} + +#if defined(PIOS_INCLUDE_FREERTOS) && 0 +static struct pios_spektrum_dev * PIOS_SPEKTRUM_alloc(void) +{ + struct pios_spektrum_dev * spektrum_dev; + + spektrum_dev = (struct pios_spektrum_dev *)malloc(sizeof(*spektrum_dev)); + if (!spektrum_dev) return(NULL); + + spektrum_dev->magic = PIOS_SPEKTRUM_DEV_MAGIC; + return(spektrum_dev); +} +#else +static struct pios_spektrum_dev pios_spektrum_devs[PIOS_SPEKTRUM_MAX_DEVS]; +static uint8_t pios_spektrum_num_devs; +static struct pios_spektrum_dev * PIOS_SPEKTRUM_alloc(void) +{ + struct pios_spektrum_dev * spektrum_dev; + + if (pios_spektrum_num_devs >= PIOS_SPEKTRUM_MAX_DEVS) { + return (NULL); + } + + spektrum_dev = &pios_spektrum_devs[pios_spektrum_num_devs++]; + spektrum_dev->magic = PIOS_SPEKTRUM_DEV_MAGIC; + + return (spektrum_dev); +} +#endif static void PIOS_SPEKTRUM_Supervisor(uint32_t spektrum_id); static bool PIOS_SPEKTRUM_Bind(const struct pios_spektrum_cfg * cfg); -static int32_t PIOS_SPEKTRUM_Decode(uint8_t b); +static int32_t PIOS_SPEKTRUM_UpdateFSM(struct pios_spektrum_fsm * fsm, uint8_t b); static uint16_t PIOS_SPEKTRUM_RxInCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield) { + struct pios_spektrum_dev * spektrum_dev = (struct pios_spektrum_dev *)context; + + bool valid = PIOS_SPEKTRUM_validate(spektrum_dev); + PIOS_Assert(valid); + /* process byte(s) and clear receive timer */ for (uint8_t i = 0; i < buf_len; i++) { - PIOS_SPEKTRUM_Decode(buf[i]); - supv_timer = 0; + PIOS_SPEKTRUM_UpdateFSM(&(spektrum_dev->fsm), buf[i]); + spektrum_dev->supv_timer = 0; } /* Always signal that we can accept another byte */ @@ -82,23 +141,118 @@ static uint16_t PIOS_SPEKTRUM_RxInCallback(uint32_t context, uint8_t * buf, uint return (buf_len); } +static void PIOS_SPEKTRUM_ResetFSM(struct pios_spektrum_fsm * fsm) +{ + fsm->channel = 0; + fsm->prev_byte = 0xFF; + fsm->sync = 0; + fsm->bytecount = 0; + fsm->datalength = 0; + fsm->frame_error = 0; + fsm->sync_of = 0; +} + +/** +* Decodes a byte +* \param[in] b byte which should be spektrum decoded +* \return 0 if no error +* \return -1 if USART not available +* \return -2 if buffer full (retry) +*/ +static int32_t PIOS_SPEKTRUM_UpdateFSM(struct pios_spektrum_fsm * fsm, uint8_t b) +{ + fsm->bytecount++; + if (fsm->sync == 0) { + /* Known sync bytes, 0x01, 0x02, 0x12 */ + if (fsm->bytecount == 2) { + if (b == 0x01) { + fsm->datalength=0; // 10bit + fsm->sync = 1; + fsm->bytecount = 2; + } + else if(b == 0x02) { + fsm->datalength=0; // 10bit + fsm->sync = 1; + fsm->bytecount = 2; + } + else if(b == 0x12) { + fsm->datalength=1; // 11bit + fsm->sync = 1; + fsm->bytecount = 2; + } + else + { + fsm->bytecount = 0; + } + } + } else { + if ((fsm->bytecount % 2) == 0) { + uint16_t data; + uint8_t channeln; + + fsm->channel = (fsm->prev_byte << 8) + b; + channeln = (fsm->channel >> (10+fsm->datalength)) & 0x0F; + data = fsm->channel & (0x03FF+(0x0400*fsm->datalength)); + if(channeln==0 && data<10) // discard frame if throttle misbehaves + { + fsm->frame_error=1; + } + if (channeln < PIOS_SPEKTRUM_NUM_INPUTS && !fsm->frame_error) + fsm->CaptureValueTemp[channeln] = data; + } + } + if (fsm->bytecount == 16) { + fsm->bytecount = 0; + fsm->sync = 0; + fsm->sync_of = 0; + if (!fsm->frame_error) + { + for(int i=0;iCaptureValue[i] = fsm->CaptureValueTemp[i]; + } + } + fsm->frame_error=0; + } + fsm->prev_byte = b; + return 0; +} + /** * Bind and Initialise Spektrum satellite receiver */ int32_t PIOS_SPEKTRUM_Init(uint32_t * spektrum_id, const struct pios_spektrum_cfg *cfg, const struct pios_com_driver * driver, uint32_t lower_id, bool bind) { - // TODO: need setting flag for bind on next powerup + PIOS_DEBUG_Assert(spektrum_id); + PIOS_DEBUG_Assert(cfg); + PIOS_DEBUG_Assert(driver); + + struct pios_spektrum_dev * spektrum_dev; + + spektrum_dev = (struct pios_spektrum_dev *) PIOS_SPEKTRUM_alloc(); + if (!spektrum_dev) goto out_fail; + + /* Bind the configuration to the device instance */ + spektrum_dev->cfg = cfg; + if (bind) { PIOS_SPEKTRUM_Bind(cfg); } - (driver->bind_rx_cb)(lower_id, PIOS_SPEKTRUM_RxInCallback, 0); + PIOS_SPEKTRUM_ResetFSM(&(spektrum_dev->fsm)); - if (!PIOS_RTC_RegisterTickCallback(PIOS_SPEKTRUM_Supervisor, 0)) { + *spektrum_id = (uint32_t)spektrum_dev; + + (driver->bind_rx_cb)(lower_id, PIOS_SPEKTRUM_RxInCallback, *spektrum_id); + + if (!PIOS_RTC_RegisterTickCallback(PIOS_SPEKTRUM_Supervisor, *spektrum_id)) { PIOS_DEBUG_Assert(0); } return (0); + +out_fail: + return(-1); } /** @@ -109,11 +263,16 @@ int32_t PIOS_SPEKTRUM_Init(uint32_t * spektrum_id, const struct pios_spektrum_cf */ static int32_t PIOS_SPEKTRUM_Get(uint32_t rcvr_id, uint8_t channel) { + struct pios_spektrum_dev * spektrum_dev = (struct pios_spektrum_dev *)rcvr_id; + + bool valid = PIOS_SPEKTRUM_validate(spektrum_dev); + PIOS_Assert(valid); + /* Return error if channel not available */ if (channel >= PIOS_SPEKTRUM_NUM_INPUTS) { return -1; } - return CaptureValue[channel]; + return spektrum_dev->fsm.CaptureValue[channel]; } /** @@ -145,114 +304,38 @@ static bool PIOS_SPEKTRUM_Bind(const struct pios_spektrum_cfg * cfg) return true; } -/** -* Decodes a byte -* \param[in] b byte which should be spektrum decoded -* \return 0 if no error -* \return -1 if USART not available -* \return -2 if buffer full (retry) -* \note Applications shouldn't call these functions directly -*/ -static int32_t PIOS_SPEKTRUM_Decode(uint8_t b) -{ - static uint16_t channel = 0; /*, sync_word = 0;*/ - uint8_t channeln = 0, frame = 0; - uint16_t data = 0; - byte_array[bytecount] = b; - bytecount++; - if (sync == 0) { - //sync_word = (prev_byte << 8) + b; -#if 0 - /* maybe create object to show this data */ - if(bytecount==1) - { - /* record losscounter into channel8 */ - CaptureValueTemp[7]=b; - /* instant write */ - CaptureValue[7]=b; - } -#endif - /* Known sync bytes, 0x01, 0x02, 0x12 */ - if (bytecount == 2) { - if (b == 0x01) { - datalength=0; // 10bit - //frames=1; - sync = 1; - bytecount = 2; - } - else if(b == 0x02) { - datalength=0; // 10bit - //frames=2; - sync = 1; - bytecount = 2; - } - else if(b == 0x12) { - datalength=1; // 11bit - //frames=2; - sync = 1; - bytecount = 2; - } - else - { - bytecount = 0; - } - } - } else { - if ((bytecount % 2) == 0) { - channel = (prev_byte << 8) + b; - frame = channel >> 15; - channeln = (channel >> (10+datalength)) & 0x0F; - data = channel & (0x03FF+(0x0400*datalength)); - if(channeln==0 && data<10) // discard frame if throttle misbehaves - { - frame_error=1; - } - if (channeln < PIOS_SPEKTRUM_NUM_INPUTS && !frame_error) - CaptureValueTemp[channeln] = data; - } - } - if (bytecount == 16) { - //PIOS_COM_SendBufferNonBlocking(PIOS_COM_TELEM_RF,byte_array,16); //00 2c 58 84 b0 dc ff - bytecount = 0; - sync = 0; - sync_of = 0; - if (!frame_error) - { - for(int i=0;i 5) { + spektrum_dev->supv_timer++; + if(spektrum_dev->supv_timer > 5) { /* sync between frames */ - sync = 0; - bytecount = 0; - prev_byte = 0xFF; - frame_error = 0; - sync_of++; + struct pios_spektrum_fsm * fsm = &(spektrum_dev->fsm); + + fsm->sync = 0; + fsm->bytecount = 0; + fsm->prev_byte = 0xFF; + fsm->frame_error = 0; + fsm->sync_of++; /* watchdog activated after 100ms silence */ - if (sync_of > 12) { + if (fsm->sync_of > 12) { /* signal lost */ - sync_of = 0; + fsm->sync_of = 0; for (int i = 0; i < PIOS_SPEKTRUM_NUM_INPUTS; i++) { - CaptureValue[i] = 0; - CaptureValueTemp[i] = 0; + fsm->CaptureValue[i] = 0; + fsm->CaptureValueTemp[i] = 0; } } - supv_timer = 0; + spektrum_dev->supv_timer = 0; } } diff --git a/flight/PiOS/inc/pios_rcvr.h b/flight/PiOS/inc/pios_rcvr.h index dfec004f5..0f4e6a973 100644 --- a/flight/PiOS/inc/pios_rcvr.h +++ b/flight/PiOS/inc/pios_rcvr.h @@ -31,13 +31,6 @@ #ifndef PIOS_RCVR_H #define PIOS_RCVR_H -struct pios_rcvr_channel_map { - uint32_t id; - uint8_t channel; -}; - -extern struct pios_rcvr_channel_map pios_rcvr_channel_to_id_map[]; - struct pios_rcvr_driver { void (*init)(uint32_t id); int32_t (*read)(uint32_t id, uint8_t channel); diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index a3494f820..7b4ed7ec4 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -1,8 +1,14 @@ Selection of optional hardware configurations. - - + + + + + + + + diff --git a/shared/uavobjectdefinition/manualcontrolsettings.xml b/shared/uavobjectdefinition/manualcontrolsettings.xml index 3b76d70f8..d6d7b90a3 100644 --- a/shared/uavobjectdefinition/manualcontrolsettings.xml +++ b/shared/uavobjectdefinition/manualcontrolsettings.xml @@ -1,15 +1,18 @@ Settings to indicate how to decode receiver input by @ref ManualControlModule. - - - - - - - - - + + + + + + @@ -20,9 +23,6 @@ - - - From 1299e110c4e462d1d11e494a4eaea56a7c3cee53 Mon Sep 17 00:00:00 2001 From: Stacey Sheldon Date: Sun, 31 Jul 2011 19:17:28 -0400 Subject: [PATCH 025/145] gcs: update CC hw settings to use hwsettings RcvrPort field --- .../src/plugins/config/cc_hw_settings.ui | 4 ++-- .../src/plugins/config/config_cc_hw_widget.cpp | 18 +----------------- 2 files changed, 3 insertions(+), 19 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/cc_hw_settings.ui b/ground/openpilotgcs/src/plugins/config/cc_hw_settings.ui index 29052623c..eed5fa232 100644 --- a/ground/openpilotgcs/src/plugins/config/cc_hw_settings.ui +++ b/ground/openpilotgcs/src/plugins/config/cc_hw_settings.ui @@ -87,7 +87,7 @@ - Receiver type + RcvrPort Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter @@ -95,7 +95,7 @@ - + diff --git a/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.cpp b/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.cpp index 62e2d59e2..5c7a035b4 100644 --- a/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.cpp +++ b/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.cpp @@ -42,7 +42,7 @@ ConfigCCHWWidget::ConfigCCHWWidget(QWidget *parent) : ConfigTaskWidget(parent) addUAVObjectToWidgetRelation("TelemetrySettings","Speed",m_telemetry->telemetrySpeed); addUAVObjectToWidgetRelation("HwSettings","CC_FlexiPort",m_telemetry->cbFlexi); addUAVObjectToWidgetRelation("HwSettings","CC_MainPort",m_telemetry->cbTele); - addUAVObjectToWidgetRelation("ManualControlSettings","InputMode",m_telemetry->receiverType); + addUAVObjectToWidgetRelation("HwSettings","RcvrPort",m_telemetry->cbRcvr); enableControls(false); populateWidgets(); refreshWidgetsValues(); @@ -65,22 +65,6 @@ void ConfigCCHWWidget::widgetsContentsChanged() { m_telemetry->problems->setText("Warning: you have configured the MainPort and the FlexiPort for the same function, this is currently not suported"); } - else if((m_telemetry->cbTele->currentText()=="Spektrum" ||m_telemetry->cbFlexi->currentText()=="Spektrum") && m_telemetry->receiverType->currentText()!="Spektrum") - { - m_telemetry->problems->setText("Warning: you have a port configured as 'Spektrum' however that is not your selected receiver type"); - } - else if(m_telemetry->cbTele->currentText()=="S.Bus" && m_telemetry->receiverType->currentText()!="S.Bus") - { - m_telemetry->problems->setText("Warning: you have a port configured as 'S.Bus' however that is not your selected receiver type"); - } - else if(m_telemetry->cbTele->currentText()!="S.Bus" && m_telemetry->receiverType->currentText()=="S.Bus") - { - m_telemetry->problems->setText("Warning: you have selected 'S.Bus' as your receiver type however you have no port configured for that protocol"); - } - else if((m_telemetry->cbTele->currentText()!="Spektrum" && m_telemetry->cbFlexi->currentText()!="Spektrum") && m_telemetry->receiverType->currentText()=="Spektrum") - { - m_telemetry->problems->setText("Warning: you have selected 'Spektrum' as your receiver type however you have no port configured for that protocol"); - } else { m_telemetry->problems->setText(""); From 90b2625debed95a1ffde3b04fd30eb159f397bed Mon Sep 17 00:00:00 2001 From: Stacey Sheldon Date: Tue, 2 Aug 2011 01:27:37 -0400 Subject: [PATCH 026/145] rcvractivity: Add tracking of Rx channel activity --- flight/CopterControl/Makefile | 1 + flight/Modules/ManualControl/manualcontrol.c | 146 +++++++++++++++++- flight/OpenPilot/UAVObjects.inc | 1 + .../src/plugins/uavobjects/uavobjects.pro | 2 + .../uavobjectdefinition/receiveractivity.xml | 14 ++ 5 files changed, 163 insertions(+), 1 deletion(-) create mode 100644 shared/uavobjectdefinition/receiveractivity.xml diff --git a/flight/CopterControl/Makefile b/flight/CopterControl/Makefile index 3ac59e59b..d437d567d 100644 --- a/flight/CopterControl/Makefile +++ b/flight/CopterControl/Makefile @@ -165,6 +165,7 @@ SRC += $(OPUAVSYNTHDIR)/firmwareiapobj.c SRC += $(OPUAVSYNTHDIR)/attitudesettings.c SRC += $(OPUAVSYNTHDIR)/hwsettings.c SRC += $(OPUAVSYNTHDIR)/gcsreceiver.c +SRC += $(OPUAVSYNTHDIR)/receiveractivity.c #${wildcard ${OBJ}/$(shell echo $(VAR) | tr A-Z a-z)/*.c} #SRC += ${foreach OBJ, ${UAVOBJECTS}, $(UAVOBJECTS)/$(OBJ).c} # Cant use until i can automatically generate list of UAVObjects diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c index 7c16ac172..153738dc1 100644 --- a/flight/Modules/ManualControl/manualcontrol.c +++ b/flight/Modules/ManualControl/manualcontrol.c @@ -43,6 +43,7 @@ #include "flighttelemetrystats.h" #include "flightstatus.h" #include "accessorydesired.h" +#include "receiveractivity.h" // Private constants #if defined(PIOS_MANUAL_STACK_SIZE) @@ -87,6 +88,18 @@ static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time) static bool okToArm(void); static bool validInputRange(int16_t min, int16_t max, uint16_t value); +#define RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP 12 +#define RCVR_ACTIVITY_MONITOR_MIN_RANGE 5 +struct rcvr_activity_fsm { + ManualControlSettingsChannelGroupsOptions group; + uint16_t prev[RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP]; + uint8_t sample_count; +}; +static struct rcvr_activity_fsm activity_fsm; + +static void resetRcvrActivity(struct rcvr_activity_fsm * fsm); +static bool updateRcvrActivity(struct rcvr_activity_fsm * fsm); + #define assumptions (assumptions1 && assumptions3 && assumptions5 && assumptions7 && assumptions8 && assumptions_flightmode) /** @@ -138,6 +151,10 @@ static void manualControlTask(void *parameters) flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED; armState = ARM_STATE_DISARMED; + /* Initialize the RcvrActivty FSM */ + portTickType lastActivityTime = xTaskGetTickCount(); + resetRcvrActivity(&activity_fsm); + // Main task loop lastSysTime = xTaskGetTickCount(); while (1) { @@ -150,6 +167,18 @@ static void manualControlTask(void *parameters) // Read settings ManualControlSettingsGet(&settings); + /* Update channel activity monitor */ + if (flightStatus.Armed == ARM_STATE_DISARMED) { + if (updateRcvrActivity(&activity_fsm)) { + /* Reset the aging timer because activity was detected */ + lastActivityTime = lastSysTime; + } + } + if (timeDifferenceMs(lastActivityTime, lastSysTime) > 5000) { + resetRcvrActivity(&activity_fsm); + lastActivityTime = lastSysTime; + } + if (ManualControlCommandReadOnly(&cmd)) { FlightTelemetryStatsData flightTelemStats; FlightTelemetryStatsGet(&flightTelemStats); @@ -266,7 +295,6 @@ static void manualControlTask(void *parameters) ManualControlCommandGet(&cmd); /* Under GCS control */ } - FlightStatusGet(&flightStatus); // Depending on the mode update the Stabilization or Actuator objects @@ -288,6 +316,122 @@ static void manualControlTask(void *parameters) } } +static void resetRcvrActivity(struct rcvr_activity_fsm * fsm) +{ + ReceiverActivityData data; + bool updated = false; + + /* Clear all channel activity flags */ + ReceiverActivityGet(&data); + if (data.ActiveGroup != RECEIVERACTIVITY_ACTIVEGROUP_NONE && + data.ActiveChannel != 255) { + data.ActiveGroup = RECEIVERACTIVITY_ACTIVEGROUP_NONE; + data.ActiveChannel = 255; + updated = true; + } + if (updated) { + ReceiverActivitySet(&data); + } + + /* Reset the FSM state */ + fsm->group = 0; + fsm->sample_count = 0; +} + +static bool updateRcvrActivity(struct rcvr_activity_fsm * fsm) +{ + bool activity_updated = false; + + if (fsm->group >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + /* We're out of range, reset things */ + resetRcvrActivity(fsm); + } + + extern uint32_t pios_rcvr_group_map[]; + if (!pios_rcvr_group_map[fsm->group]) { + /* Unbound group, skip it */ + goto group_completed; + } + + /* Sample the channel */ + for (uint8_t channel = 0; + channel < RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP; + channel++) { + if (fsm->sample_count == 0) { + fsm->prev[channel] = PIOS_RCVR_Read(pios_rcvr_group_map[fsm->group], channel); + } else { + uint16_t delta; + uint16_t prev = fsm->prev[channel]; + uint16_t curr = PIOS_RCVR_Read(pios_rcvr_group_map[fsm->group], channel); + if (curr > prev) { + delta = curr - prev; + } else { + delta = prev - curr; + } + + if (delta > RCVR_ACTIVITY_MONITOR_MIN_RANGE) { + /* Mark this channel as active */ + ReceiverActivityActiveGroupOptions group; + + /* Don't assume manualcontrolsettings and receiveractivity are in the same order. */ + switch (fsm->group) { + case MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM: + group = RECEIVERACTIVITY_ACTIVEGROUP_PWM; + break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM: + group = RECEIVERACTIVITY_ACTIVEGROUP_PPM; + break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_SPEKTRUM1: + group = RECEIVERACTIVITY_ACTIVEGROUP_SPEKTRUM1; + break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_SPEKTRUM2: + group = RECEIVERACTIVITY_ACTIVEGROUP_SPEKTRUM2; + break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS: + group = RECEIVERACTIVITY_ACTIVEGROUP_SBUS; + break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS: + group = RECEIVERACTIVITY_ACTIVEGROUP_GCS; + break; + default: + PIOS_Assert(0); + break; + } + + ReceiverActivityActiveGroupSet(&group); + ReceiverActivityActiveChannelSet(&channel); + activity_updated = true; + } + } + } + + if (++fsm->sample_count < 2) { + return (activity_updated); + } + + /* Reset the sample counter */ + fsm->sample_count = 0; + + /* + * Group Completed + */ + +group_completed: + /* Start over at channel zero */ + if (++fsm->group < MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + return (activity_updated); + } + + /* + * All groups completed + */ + + /* Start over at group zero */ + fsm->group = 0; + + return (activity_updated); +} + static void updateActuatorDesired(ManualControlCommandData * cmd) { ActuatorDesiredData actuator; diff --git a/flight/OpenPilot/UAVObjects.inc b/flight/OpenPilot/UAVObjects.inc index a30dcf540..3bb868a7d 100644 --- a/flight/OpenPilot/UAVObjects.inc +++ b/flight/OpenPilot/UAVObjects.inc @@ -70,6 +70,7 @@ UAVOBJSRCFILENAMES += velocitydesired UAVOBJSRCFILENAMES += watchdogstatus UAVOBJSRCFILENAMES += flightstatus UAVOBJSRCFILENAMES += hwsettings +UAVOBJSRCFILENAMES += receiveractivity UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(UAVOBJSYNTHDIR)/$(UAVOBJSRCFILE).c ) UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) ) diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index 3831a4ec3..f5e17d3ca 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -70,6 +70,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/flightstatus.h \ $$UAVOBJECT_SYNTHETICS/hwsettings.h \ $$UAVOBJECT_SYNTHETICS/gcsreceiver.h \ + $$UAVOBJECT_SYNTHETICS/receiveractivity.h \ $$UAVOBJECT_SYNTHETICS/attitudesettings.h SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ @@ -120,4 +121,5 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/flightstatus.cpp \ $$UAVOBJECT_SYNTHETICS/hwsettings.cpp \ $$UAVOBJECT_SYNTHETICS/gcsreceiver.cpp \ + $$UAVOBJECT_SYNTHETICS/receiveractivity.cpp \ $$UAVOBJECT_SYNTHETICS/attitudesettings.cpp diff --git a/shared/uavobjectdefinition/receiveractivity.xml b/shared/uavobjectdefinition/receiveractivity.xml new file mode 100644 index 000000000..2e98c1362 --- /dev/null +++ b/shared/uavobjectdefinition/receiveractivity.xml @@ -0,0 +1,14 @@ + + + Monitors which receiver channels have been active within the last second. + + + + + + + + From 1b34376797cb7b9cb776959d0c8adecf84e9a8f8 Mon Sep 17 00:00:00 2001 From: zedamota Date: Tue, 2 Aug 2011 17:06:17 +0100 Subject: [PATCH 027/145] Added "changes not saved" message warning to config gadget --- .../plugins/config/configairframewidget.cpp | 8 +- .../plugins/config/configccattitudewidget.cpp | 39 +++------ .../plugins/config/configccattitudewidget.h | 6 +- .../src/plugins/config/configgadgetwidget.cpp | 20 +++++ .../src/plugins/config/configgadgetwidget.h | 5 +- .../src/plugins/config/configoutputwidget.cpp | 85 ++++++++++--------- .../src/plugins/config/configoutputwidget.h | 6 +- .../config/configstabilizationwidget.cpp | 63 ++++++++------ .../config/configstabilizationwidget.h | 5 +- .../src/plugins/config/configtaskwidget.cpp | 27 ++++-- .../src/plugins/config/configtaskwidget.h | 4 + .../src/plugins/config/fancytabwidget.cpp | 9 +- .../src/plugins/config/fancytabwidget.h | 5 +- .../src/plugins/config/smartsavebutton.cpp | 1 + 14 files changed, 165 insertions(+), 118 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/configairframewidget.cpp b/ground/openpilotgcs/src/plugins/config/configairframewidget.cpp index 4c24bd1e0..05eb98c4d 100644 --- a/ground/openpilotgcs/src/plugins/config/configairframewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configairframewidget.cpp @@ -92,7 +92,7 @@ ConfigAirframeWidget::ConfigAirframeWidget(QWidget *parent) : ConfigTaskWidget(p m_aircraft->setupUi(this); setupButtons(m_aircraft->saveAircraftToRAM,m_aircraft->saveAircraftToSD); - addToDirtyMonitor(); + addUAVObject("SystemSettings"); addUAVObject("MixerSettings"); addUAVObject("ActuatorSettings"); @@ -198,7 +198,7 @@ ConfigAirframeWidget::ConfigAirframeWidget(QWidget *parent) : ConfigTaskWidget(p // Connect the help button connect(m_aircraft->airframeHelp, SIGNAL(clicked()), this, SLOT(openHelp())); - + addToDirtyMonitor(); } ConfigAirframeWidget::~ConfigAirframeWidget() @@ -443,6 +443,7 @@ void ConfigAirframeWidget::updateCustomThrottle2CurveValue(QList list, d */ void ConfigAirframeWidget::refreshWidgetsValues() { + bool dirty=isDirty(); // Get the Airframe type from the system settings: UAVDataObject* obj = dynamic_cast(getObjectManager()->getObject(QString("SystemSettings"))); Q_ASSERT(obj); @@ -894,6 +895,7 @@ void ConfigAirframeWidget::refreshWidgetsValues() } updateCustomAirframeUI(); + setDirty(dirty); } /** @@ -902,6 +904,7 @@ void ConfigAirframeWidget::refreshWidgetsValues() */ void ConfigAirframeWidget::setupAirframeUI(QString frameType) { + bool dirty=isDirty(); if (frameType == "FixedWing" || frameType == "Elevator aileron rudder") { // Setup the UI m_aircraft->aircraftType->setCurrentIndex(m_aircraft->aircraftType->findText("Fixed Wing")); @@ -1099,6 +1102,7 @@ void ConfigAirframeWidget::setupAirframeUI(QString frameType) } m_aircraft->quadShape->setSceneRect(quad->boundingRect()); m_aircraft->quadShape->fitInView(quad, Qt::KeepAspectRatio); + setDirty(dirty); } /** diff --git a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp index 51b63fadb..a88bcf2ac 100644 --- a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp @@ -40,22 +40,21 @@ ConfigCCAttitudeWidget::ConfigCCAttitudeWidget(QWidget *parent) : { ui->setupUi(this); connect(ui->zeroBias,SIGNAL(clicked()),this,SLOT(startAccelCalibration())); - connect(ui->saveButton,SIGNAL(clicked()),this,SLOT(saveAttitudeSettings())); - connect(ui->applyButton,SIGNAL(clicked()),this,SLOT(applyAttitudeSettings())); - // Make it smart: - connect(parent, SIGNAL(autopilotConnected()),this, SLOT(onAutopilotConnect())); - connect(parent, SIGNAL(autopilotDisconnected()), this, SLOT(onAutopilotDisconnect())); - enableControls(true); - refreshValues(); // The 1st time this panel is instanciated, the autopilot is already connected. + setupButtons(ui->applyButton,ui->saveButton); + addUAVObject("AttitudeSettings"); + + refreshWidgetsValues(); // The 1st time this panel is instanciated, the autopilot is already connected. UAVObject * settings = AttitudeSettings::GetInstance(getObjectManager()); - connect(settings,SIGNAL(objectUpdated(UAVObject*)), this, SLOT(refreshValues())); // Connect the help button connect(ui->ccAttitudeHelp, SIGNAL(clicked()), this, SLOT(openHelp())); - - + addWidget(ui->rollBias); + addWidget(ui->pitchBias); + addWidget(ui->yawBias); + addWidget(ui->zeroBias); + addWidget(ui->zeroGyroBiasOnArming); } ConfigCCAttitudeWidget::~ConfigCCAttitudeWidget() @@ -63,12 +62,6 @@ ConfigCCAttitudeWidget::~ConfigCCAttitudeWidget() delete ui; } -void ConfigCCAttitudeWidget::enableControls(bool enable) -{ - //ui->applyButton->setEnabled(enable); - ui->saveButton->setEnabled(enable); -} - void ConfigCCAttitudeWidget::attitudeRawUpdated(UAVObject * obj) { QMutexLocker locker(&startStop); @@ -130,7 +123,7 @@ void ConfigCCAttitudeWidget::timeout() { } -void ConfigCCAttitudeWidget::applyAttitudeSettings() { +void ConfigCCAttitudeWidget::updateObjectsFromWidgets() { AttitudeSettings::DataFields attitudeSettingsData = AttitudeSettings::GetInstance(getObjectManager())->getData(); attitudeSettingsData.BoardRotation[AttitudeSettings::BOARDROTATION_ROLL] = ui->rollBias->value(); attitudeSettingsData.BoardRotation[AttitudeSettings::BOARDROTATION_PITCH] = ui->pitchBias->value(); @@ -140,14 +133,15 @@ void ConfigCCAttitudeWidget::applyAttitudeSettings() { AttitudeSettings::GetInstance(getObjectManager())->setData(attitudeSettingsData); } -void ConfigCCAttitudeWidget::refreshValues() { +void ConfigCCAttitudeWidget::refreshWidgetsValues() { + bool dirty=isDirty(); AttitudeSettings::DataFields attitudeSettingsData = AttitudeSettings::GetInstance(getObjectManager())->getData(); ui->rollBias->setValue(attitudeSettingsData.BoardRotation[0]); ui->pitchBias->setValue(attitudeSettingsData.BoardRotation[1]); ui->yawBias->setValue(attitudeSettingsData.BoardRotation[2]); ui->zeroGyroBiasOnArming->setChecked(attitudeSettingsData.ZeroDuringArming == AttitudeSettings::ZERODURINGARMING_TRUE); - + setDirty(dirty); } void ConfigCCAttitudeWidget::startAccelCalibration() { @@ -184,13 +178,6 @@ void ConfigCCAttitudeWidget::startAccelCalibration() { } -void ConfigCCAttitudeWidget::saveAttitudeSettings() { - applyAttitudeSettings(); - - UAVDataObject * obj = dynamic_cast(getObjectManager()->getObject(QString("AttitudeSettings"))); - saveObjectToSD(obj); -} - void ConfigCCAttitudeWidget::openHelp() { diff --git a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h index 27176b22f..209d7ed28 100644 --- a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h +++ b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h @@ -50,9 +50,8 @@ private slots: void attitudeRawUpdated(UAVObject * obj); void timeout(); void startAccelCalibration(); - void saveAttitudeSettings(); - void applyAttitudeSettings(); - virtual void refreshValues(); + void updateObjectsFromWidgets(); + virtual void refreshWidgetsValues(); void openHelp(); private: @@ -69,7 +68,6 @@ private: static const int NUM_ACCEL_UPDATES = 60; static const float ACCEL_SCALE = 0.004f * 9.81f; - virtual void enableControls(bool enable); }; diff --git a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp index f1f93c9d7..31eaf966b 100644 --- a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp @@ -99,6 +99,8 @@ ConfigGadgetWidget::ConfigGadgetWidget(QWidget *parent) : QWidget(parent) onAutopilotConnect(); help = 0; + connect(ftw->m_tabBar,SIGNAL(aboutToChange(bool*)),this,SLOT(tabAboutToChange(bool*)));//,Qt::BlockingQueuedConnection); + } ConfigGadgetWidget::~ConfigGadgetWidget() @@ -153,5 +155,23 @@ void ConfigGadgetWidget::onAutopilotConnect() { emit autopilotConnected(); } +void ConfigGadgetWidget::tabAboutToChange(bool * proceed) +{ + *proceed=true; + ConfigTaskWidget * wid=qobject_cast(ftw->currentWidget()); + if(!wid) + return; + if(wid->isDirty()) + { + int ans=QMessageBox::warning(this,tr("Unsaved changes"),tr("The tab you are leaving has unsaved changes," + "if you proceed they will be lost." + "Do you still want to proceed?"),QMessageBox::Yes,QMessageBox::No); + if(ans==QMessageBox::No) + *proceed=false; + else + wid->setDirty(false); + } +} + diff --git a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.h b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.h index 84d65b383..376d85c08 100644 --- a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.h @@ -37,9 +37,9 @@ //#include #include #include "utils/pathutils.h" - +#include #include "fancytabwidget.h" - +#include "configtaskwidget.h" class ConfigGadgetWidget: public QWidget { @@ -54,6 +54,7 @@ public: public slots: void onAutopilotConnect(); void onAutopilotDisconnect(); + void tabAboutToChange(bool *); signals: void autopilotConnected(); diff --git a/ground/openpilotgcs/src/plugins/config/configoutputwidget.cpp b/ground/openpilotgcs/src/plugins/config/configoutputwidget.cpp index 1a961805b..77138a5a0 100644 --- a/ground/openpilotgcs/src/plugins/config/configoutputwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configoutputwidget.cpp @@ -46,6 +46,8 @@ ConfigOutputWidget::ConfigOutputWidget(QWidget *parent) : ConfigTaskWidget(paren ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); + setupButtons(m_config->saveRCOutputToRAM,m_config->saveRCOutputToSD); + addUAVObject("ActuatorSettings"); // First of all, put all the channel widgets into lists, so that we can // manipulate those: @@ -106,10 +108,7 @@ ConfigOutputWidget::ConfigOutputWidget(QWidget *parent) : ConfigTaskWidget(paren << m_config->ch7Link; // Register for ActuatorSettings changes: - UAVDataObject * obj = dynamic_cast(objManager->getObject(QString("ActuatorSettings"))); - connect(obj,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(refreshValues())); - - for (int i = 0; i < 8; i++) { + for (int i = 0; i < 8; i++) { connect(outMin[i], SIGNAL(editingFinished()), this, SLOT(setChOutRange())); connect(outMax[i], SIGNAL(editingFinished()), this, SLOT(setChOutRange())); connect(reversals[i], SIGNAL(toggled(bool)), this, SLOT(reverseChannel(bool))); @@ -124,13 +123,8 @@ ConfigOutputWidget::ConfigOutputWidget(QWidget *parent) : ConfigTaskWidget(paren for (int i = 0; i < links.count(); i++) connect(links[i], SIGNAL(toggled(bool)), this, SLOT(linkToggled(bool))); - connect(m_config->saveRCOutputToSD, SIGNAL(clicked()), this, SLOT(saveRCOutputObject())); - connect(m_config->saveRCOutputToRAM, SIGNAL(clicked()), this, SLOT(sendRCOutputUpdate())); - enableControls(false); - refreshValues(); - connect(parent, SIGNAL(autopilotConnected()),this, SLOT(onAutopilotConnect())); - connect(parent, SIGNAL(autopilotDisconnected()), this, SLOT(onAutopilotDisconnect())); + refreshWidgetsValues(); firstUpdate = true; @@ -138,6 +132,43 @@ ConfigOutputWidget::ConfigOutputWidget(QWidget *parent) : ConfigTaskWidget(paren // Connect the help button connect(m_config->outputHelp, SIGNAL(clicked()), this, SLOT(openHelp())); + addWidget(m_config->outputRate3); + addWidget(m_config->outputRate2); + addWidget(m_config->outputRate1); + addWidget(m_config->ch0OutMin); + addWidget(m_config->ch0OutSlider); + addWidget(m_config->ch0OutMax); + addWidget(m_config->ch0Rev); + addWidget(m_config->ch0Link); + addWidget(m_config->ch1OutMin); + addWidget(m_config->ch1OutSlider); + addWidget(m_config->ch1OutMax); + addWidget(m_config->ch1Rev); + addWidget(m_config->ch2OutMin); + addWidget(m_config->ch2OutSlider); + addWidget(m_config->ch2OutMax); + addWidget(m_config->ch2Rev); + addWidget(m_config->ch3OutMin); + addWidget(m_config->ch3OutSlider); + addWidget(m_config->ch3OutMax); + addWidget(m_config->ch3Rev); + addWidget(m_config->ch4OutMin); + addWidget(m_config->ch4OutSlider); + addWidget(m_config->ch4OutMax); + addWidget(m_config->ch4Rev); + addWidget(m_config->ch5OutMin); + addWidget(m_config->ch5OutSlider); + addWidget(m_config->ch5OutMax); + addWidget(m_config->ch5Rev); + addWidget(m_config->ch6OutMin); + addWidget(m_config->ch6OutSlider); + addWidget(m_config->ch6OutMax); + addWidget(m_config->ch6Rev); + addWidget(m_config->ch7OutMin); + addWidget(m_config->ch7OutSlider); + addWidget(m_config->ch7OutMax); + addWidget(m_config->ch7Rev); + addWidget(m_config->spinningArmed); } ConfigOutputWidget::~ConfigOutputWidget() @@ -146,14 +177,6 @@ ConfigOutputWidget::~ConfigOutputWidget() } -// ************************************ - -void ConfigOutputWidget::enableControls(bool enable) -{ - m_config->saveRCOutputToSD->setEnabled(enable); - //m_config->saveRCOutputToRAM->setEnabled(enable); -} - // ************************************ /** @@ -355,8 +378,9 @@ void ConfigOutputWidget::sendChannelTest(int value) /** Request the current config from the board (RC Output) */ -void ConfigOutputWidget::refreshValues() +void ConfigOutputWidget::refreshWidgetsValues() { + bool dirty=isDirty(); ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); @@ -444,14 +468,14 @@ void ConfigOutputWidget::refreshValues() outSliders[i]->setValue(value); outLabels[i]->setText(QString::number(value)); } - + setDirty(dirty); } /** * Sends the config to the board, without saving to the SD card (RC Output) */ -void ConfigOutputWidget::sendRCOutputUpdate() +void ConfigOutputWidget::updateObjectsFromWidgets() { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); @@ -479,27 +503,8 @@ void ConfigOutputWidget::sendRCOutputUpdate() field->setValue(m_config->outputRate2->value(),1); field->setValue(m_config->outputRate3->value(),2); field->setValue(m_config->outputRate4->value(),3); - - // ... and send to the OP Board - obj->updated(); - } - -/** - Sends the config to the board and request saving into the SD card (RC Output) - */ -void ConfigOutputWidget::saveRCOutputObject() -{ - // Send update so that the latest value is saved - sendRCOutputUpdate(); - UAVDataObject* obj = dynamic_cast(getObjectManager()->getObject(QString("ActuatorSettings"))); - Q_ASSERT(obj); - saveObjectToSD(obj); - -} - - /** Sets the minimum/maximum value of the channel 0 to seven output sliders. Have to do it here because setMinimum is not a slot. diff --git a/ground/openpilotgcs/src/plugins/config/configoutputwidget.h b/ground/openpilotgcs/src/plugins/config/configoutputwidget.h index 2a197e3ac..492109df1 100644 --- a/ground/openpilotgcs/src/plugins/config/configoutputwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configoutputwidget.h @@ -69,12 +69,10 @@ private: bool firstUpdate; - virtual void enableControls(bool enable); private slots: - virtual void refreshValues(); - void sendRCOutputUpdate(); - void saveRCOutputObject(); + virtual void refreshWidgetsValues(); + void updateObjectsFromWidgets(); void runChannelTests(bool state); void sendChannelTest(int value); void setChOutRange(); diff --git a/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.cpp b/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.cpp index 83ac65523..22ad60b0e 100644 --- a/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.cpp @@ -43,21 +43,15 @@ ConfigStabilizationWidget::ConfigStabilizationWidget(QWidget *parent) : ConfigTa m_stabilization->setupUi(this); - connect(m_stabilization->saveStabilizationToSD, SIGNAL(clicked()), this, SLOT(saveStabilizationUpdate())); - connect(m_stabilization->saveStabilizationToRAM, SIGNAL(clicked()), this, SLOT(sendStabilizationUpdate())); + setupButtons(m_stabilization->saveStabilizationToRAM,m_stabilization->saveStabilizationToSD); - enableControls(false); - refreshValues(); - connect(parent, SIGNAL(autopilotConnected()),this, SLOT(onAutopilotConnect())); - connect(parent, SIGNAL(autopilotDisconnected()),this, SLOT(onAutopilotDisconnect())); + addUAVObject("StabilizationSettings"); - // Now connect the widget to the StabilizationSettings object - UAVObject *obj = getObjectManager()->getObject(QString("StabilizationSettings")); - connect(obj,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(refreshValues())); + refreshWidgetsValues(); // Create a timer to regularly send the object update in case // we want realtime updates. - connect(&updateTimer, SIGNAL(timeout()), this, SLOT(sendStabilizationUpdate())); + connect(&updateTimer, SIGNAL(timeout()), this, SLOT(updateObjectsFromWidgets())); connect(m_stabilization->realTimeUpdates, SIGNAL(toggled(bool)), this, SLOT(realtimeUpdateToggle(bool))); // Connect the updates of the stab values @@ -79,6 +73,33 @@ ConfigStabilizationWidget::ConfigStabilizationWidget(QWidget *parent) : ConfigTa // Connect the help button connect(m_stabilization->stabilizationHelp, SIGNAL(clicked()), this, SLOT(openHelp())); + addWidget(m_stabilization->rateRollKp); + addWidget(m_stabilization->rateRollKi); + addWidget(m_stabilization->rateRollILimit); + addWidget(m_stabilization->ratePitchKp); + addWidget(m_stabilization->ratePitchKi); + addWidget(m_stabilization->ratePitchILimit); + addWidget(m_stabilization->rateYawKp); + addWidget(m_stabilization->rateYawKi); + addWidget(m_stabilization->rateYawILimit); + addWidget(m_stabilization->rollKp); + addWidget(m_stabilization->rollKi); + addWidget(m_stabilization->rollILimit); + addWidget(m_stabilization->yawILimit); + addWidget(m_stabilization->yawKi); + addWidget(m_stabilization->yawKp); + addWidget(m_stabilization->pitchKp); + addWidget(m_stabilization->pitchKi); + addWidget(m_stabilization->pitchILimit); + addWidget(m_stabilization->rollMax); + addWidget(m_stabilization->pitchMax); + addWidget(m_stabilization->yawMax); + addWidget(m_stabilization->manualRoll); + addWidget(m_stabilization->manualPitch); + addWidget(m_stabilization->manualYaw); + addWidget(m_stabilization->maximumRoll); + addWidget(m_stabilization->maximumPitch); + addWidget(m_stabilization->maximumYaw); } ConfigStabilizationWidget::~ConfigStabilizationWidget() @@ -187,8 +208,9 @@ void ConfigStabilizationWidget::updatePitchILimit(double val) /** Request stabilization settings from the board */ -void ConfigStabilizationWidget::refreshValues() +void ConfigStabilizationWidget::refreshWidgetsValues() { + bool dirty=isDirty(); // Not needed anymore as this slot is only called whenever we get // a signal that the object was just updated // stabSettings->requestUpdate(); @@ -229,7 +251,7 @@ void ConfigStabilizationWidget::refreshValues() m_stabilization->maximumRoll->setValue(stabData.MaximumRate[StabilizationSettings::MAXIMUMRATE_ROLL]); m_stabilization->maximumPitch->setValue(stabData.MaximumRate[StabilizationSettings::MAXIMUMRATE_PITCH]); m_stabilization->maximumYaw->setValue(stabData.MaximumRate[StabilizationSettings::MAXIMUMRATE_YAW]); - + setDirty(dirty); } @@ -237,7 +259,7 @@ void ConfigStabilizationWidget::refreshValues() Send telemetry settings to the board */ -void ConfigStabilizationWidget::sendStabilizationUpdate() +void ConfigStabilizationWidget::updateObjectsFromWidgets() { StabilizationSettings::DataFields stabData = stabSettings->getData(); @@ -280,21 +302,6 @@ void ConfigStabilizationWidget::sendStabilizationUpdate() stabSettings->setData(stabData); // this is atomic } - -/** - Send telemetry settings to the board and request saving to SD card - */ - -void ConfigStabilizationWidget::saveStabilizationUpdate() -{ - // Send update so that the latest value is saved - sendStabilizationUpdate(); - UAVDataObject* obj = dynamic_cast(getObjectManager()->getObject(QString("StabilizationSettings"))); - Q_ASSERT(obj); - saveObjectToSD(obj); -} - - void ConfigStabilizationWidget::realtimeUpdateToggle(bool state) { if (state) { diff --git a/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.h b/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.h index 98db5530f..f17ac593e 100644 --- a/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.h @@ -52,9 +52,8 @@ private: virtual void enableControls(bool enable); private slots: - virtual void refreshValues(); - void sendStabilizationUpdate(); - void saveStabilizationUpdate(); + virtual void refreshWidgetsValues(); + void updateObjectsFromWidgets(); void realtimeUpdateToggle(bool); void openHelp(); diff --git a/ground/openpilotgcs/src/plugins/config/configtaskwidget.cpp b/ground/openpilotgcs/src/plugins/config/configtaskwidget.cpp index 26524611f..87309ca63 100644 --- a/ground/openpilotgcs/src/plugins/config/configtaskwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configtaskwidget.cpp @@ -88,6 +88,15 @@ void ConfigTaskWidget::addUAVObjectToWidgetRelation(QString object, QString fiel { connect(cb,SIGNAL(valueChanged(double)),this,SLOT(widgetsContentsChanged())); } + else if(QCheckBox * cb=qobject_cast(widget)) + { + connect(cb,SIGNAL(clicked()),this,SLOT(widgetsContentsChanged())); + } + else if(QPushButton * cb=qobject_cast(widget)) + { + connect(cb,SIGNAL(clicked()),this,SLOT(widgetsContentsChanged())); + } + } @@ -137,6 +146,7 @@ void ConfigTaskWidget::onAutopilotConnect() void ConfigTaskWidget::populateWidgets() { + bool dirtyBack=dirty; foreach(objectToWidget * ow,objOfInterest) { if(ow->object==NULL || ow->field==NULL) @@ -153,11 +163,12 @@ void ConfigTaskWidget::populateWidgets() cb->setText(ow->field->getValue().toString()); } } - dirty=false; + setDirty(dirtyBack); } void ConfigTaskWidget::refreshWidgetsValues() { + bool dirtyBack=dirty; foreach(objectToWidget * ow,objOfInterest) { if(ow->object==NULL || ow->field==NULL) @@ -173,6 +184,7 @@ void ConfigTaskWidget::refreshWidgetsValues() cb->setText(ow->field->getValue().toString()); } } + setDirty(dirtyBack); } void ConfigTaskWidget::updateObjectsFromWidgets() @@ -197,7 +209,7 @@ void ConfigTaskWidget::updateObjectsFromWidgets() void ConfigTaskWidget::setupButtons(QPushButton *update, QPushButton *save) { smartsave=new smartSaveButton(update,save); - connect(smartsave, SIGNAL(preProcessOperations()), this, SLOT(updateObjectsFromWidgets())); + connect(smartsave,SIGNAL(preProcessOperations()), this, SLOT(updateObjectsFromWidgets())); connect(smartsave,SIGNAL(saveSuccessfull()),this,SLOT(clearDirty())); connect(smartsave,SIGNAL(beginOp()),this,SLOT(disableObjUpdates())); connect(smartsave,SIGNAL(endOp()),this,SLOT(enableObjUpdates())); @@ -211,15 +223,18 @@ void ConfigTaskWidget::enableControls(bool enable) void ConfigTaskWidget::widgetsContentsChanged() { - qDebug()<<"dirty!!!"; - dirty=true; + setDirty(true); } void ConfigTaskWidget::clearDirty() { - dirty=false; + setDirty(false); +} +void ConfigTaskWidget::setDirty(bool value) +{ + qDebug()<<"dirty="< #include #include +#include +#include + class ConfigTaskWidget: public QWidget { Q_OBJECT @@ -63,6 +66,7 @@ public: void addUAVObjectToWidgetRelation(QString object,QString field,QWidget * widget); void setupButtons(QPushButton * update,QPushButton * save); bool isDirty(); + void setDirty(bool value); public slots: void onAutopilotDisconnect(); void onAutopilotConnect(); diff --git a/ground/openpilotgcs/src/plugins/config/fancytabwidget.cpp b/ground/openpilotgcs/src/plugins/config/fancytabwidget.cpp index 97ae45303..bb0b65b09 100644 --- a/ground/openpilotgcs/src/plugins/config/fancytabwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/fancytabwidget.cpp @@ -284,6 +284,10 @@ void FancyTabBar::paintTab(QPainter *painter, int tabIndex) const } void FancyTabBar::setCurrentIndex(int index) { + bool proceed=true; + emit aboutToChange(&proceed); + if(!proceed) + return; m_currentIndex = index; update(); emit currentChanged(index); @@ -319,7 +323,6 @@ FancyTabWidget::FancyTabWidget(QWidget *parent, bool isVertical) : QWidget(parent) { m_tabBar = new FancyTabBar(this, isVertical); - m_selectionWidget = new QWidget(this); QBoxLayout *selectionLayout; if (isVertical) { @@ -477,3 +480,7 @@ void FancyTabWidget::setTabToolTip(int index, const QString &toolTip) { m_tabBar->setTabToolTip(index, toolTip); } +QWidget * FancyTabWidget::currentWidget() +{ + return m_modesStack->currentWidget(); +} diff --git a/ground/openpilotgcs/src/plugins/config/fancytabwidget.h b/ground/openpilotgcs/src/plugins/config/fancytabwidget.h index 47c7ddeb5..73da2aae8 100644 --- a/ground/openpilotgcs/src/plugins/config/fancytabwidget.h +++ b/ground/openpilotgcs/src/plugins/config/fancytabwidget.h @@ -91,6 +91,7 @@ public: signals: void currentChanged(int); + void aboutToChange(bool *); public slots: void updateHover(); @@ -116,7 +117,7 @@ class FancyTabWidget : public QWidget public: FancyTabWidget(QWidget *parent = 0, bool isVertical = false); - + FancyTabBar *m_tabBar; void insertTab(int index, QWidget *tab, const QIcon &icon, const QString &label); void removeTab(int index); void setBackgroundBrush(const QBrush &brush); @@ -132,6 +133,7 @@ public: int currentIndex() const; QStatusBar *statusBar() const; + QWidget * currentWidget(); signals: void currentAboutToShow(int index); void currentChanged(int index); @@ -143,7 +145,6 @@ private slots: void showWidget(int index); private: - FancyTabBar *m_tabBar; QWidget *m_cornerWidgetContainer; QStackedLayout *m_modesStack; QWidget *m_selectionWidget; diff --git a/ground/openpilotgcs/src/plugins/config/smartsavebutton.cpp b/ground/openpilotgcs/src/plugins/config/smartsavebutton.cpp index ed5abf2c7..708c1b5c1 100644 --- a/ground/openpilotgcs/src/plugins/config/smartsavebutton.cpp +++ b/ground/openpilotgcs/src/plugins/config/smartsavebutton.cpp @@ -111,6 +111,7 @@ void smartSaveButton::saving_finished(int id, bool result) if(id==current_objectID) { sv_result=result; + qDebug()<<"saving_finished result="< Date: Thu, 4 Aug 2011 07:47:50 -0500 Subject: [PATCH 028/145] ManualControlSettings must be also initialized in pios_board.c --- flight/CopterControl/System/pios_board.c | 1 + flight/Modules/ManualControl/manualcontrol.c | 4 +++- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/flight/CopterControl/System/pios_board.c b/flight/CopterControl/System/pios_board.c index 785615280..d512ae989 100644 --- a/flight/CopterControl/System/pios_board.c +++ b/flight/CopterControl/System/pios_board.c @@ -909,6 +909,7 @@ void PIOS_Board_Init(void) { UAVObjInitialize(); HwSettingsInitialize(); + ManualControlSettingsInitialize(); #if defined(PIOS_INCLUDE_RTC) /* Initialize the real-time clock and its associated tick */ diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c index 5b9907864..bdd9f6b34 100644 --- a/flight/Modules/ManualControl/manualcontrol.c +++ b/flight/Modules/ManualControl/manualcontrol.c @@ -113,11 +113,13 @@ int32_t ManualControlInitialize() return -1; AccessoryDesiredInitialize(); - ManualControlSettingsInitialize(); ManualControlCommandInitialize(); FlightStatusInitialize(); StabilizationDesiredInitialize(); + // ManualControlSettingsInitialize(); // this is initialized in + // pios_board.c + return 0; } MODULE_INITCALL(ManualControlInitialize, ManualControlStart) From 5d0095f00dccd4e9fa4670794f7074b901a55dcf Mon Sep 17 00:00:00 2001 From: zedamota Date: Thu, 4 Aug 2011 16:58:41 +0100 Subject: [PATCH 029/145] Started working on the multi receiver support --- .../src/plugins/config/config.pro | 9 +- .../src/plugins/config/configinputwidget.cpp | 691 +------------ .../src/plugins/config/configinputwidget.h | 32 +- .../src/plugins/config/configtaskwidget.cpp | 41 +- .../src/plugins/config/configtaskwidget.h | 7 +- .../openpilotgcs/src/plugins/config/input.ui | 908 +----------------- .../src/plugins/config/inputchannelform.cpp | 29 + .../src/plugins/config/inputchannelform.h | 23 + .../src/plugins/config/inputchannelform.ui | 119 +++ .../src/plugins/config/smartsavebutton.cpp | 2 + 10 files changed, 260 insertions(+), 1601 deletions(-) create mode 100644 ground/openpilotgcs/src/plugins/config/inputchannelform.cpp create mode 100644 ground/openpilotgcs/src/plugins/config/inputchannelform.h create mode 100644 ground/openpilotgcs/src/plugins/config/inputchannelform.ui diff --git a/ground/openpilotgcs/src/plugins/config/config.pro b/ground/openpilotgcs/src/plugins/config/config.pro index 79ddea046..62bdc327e 100644 --- a/ground/openpilotgcs/src/plugins/config/config.pro +++ b/ground/openpilotgcs/src/plugins/config/config.pro @@ -37,7 +37,8 @@ HEADERS += configplugin.h \ calibration.h \ defaultattitudewidget.h \ smartsavebutton.h \ - defaulthwsettingswidget.h + defaulthwsettingswidget.h \ + inputchannelform.h SOURCES += configplugin.cpp \ configgadgetconfiguration.cpp \ @@ -65,7 +66,8 @@ SOURCES += configplugin.cpp \ alignment-calibration.cpp \ defaultattitudewidget.cpp \ smartsavebutton.cpp \ - defaulthwsettingswidget.cpp + defaulthwsettingswidget.cpp \ + inputchannelform.cpp FORMS += \ airframe.ui \ @@ -78,6 +80,7 @@ FORMS += \ output.ui \ ccattitude.ui \ defaultattitude.ui \ - defaulthwsettings.ui + defaulthwsettings.ui \ + inputchannelform.ui RESOURCES += configgadget.qrc diff --git a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp index 83e1a02ac..18841b9df 100644 --- a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp @@ -46,606 +46,53 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) : ConfigTaskWidget(parent) m_config = new Ui_InputWidget(); m_config->setupUi(this); + setupButtons(m_config->saveRCInputToRAM,m_config->saveRCInputToSD); + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); - - // First of all, put all the channel widgets into lists, so that we can - // manipulate those: - - - inMaxLabels << m_config->ch0Max - << m_config->ch1Max - << m_config->ch2Max - << m_config->ch3Max - << m_config->ch4Max - << m_config->ch5Max - << m_config->ch6Max - << m_config->ch7Max; - - inMinLabels << m_config->ch0Min - << m_config->ch1Min - << m_config->ch2Min - << m_config->ch3Min - << m_config->ch4Min - << m_config->ch5Min - << m_config->ch6Min - << m_config->ch7Min; - - inSliders << m_config->inSlider0 - << m_config->inSlider1 - << m_config->inSlider2 - << m_config->inSlider3 - << m_config->inSlider4 - << m_config->inSlider5 - << m_config->inSlider6 - << m_config->inSlider7; - - inRevCheckboxes << m_config->ch0Rev - << m_config->ch1Rev - << m_config->ch2Rev - << m_config->ch3Rev - << m_config->ch4Rev - << m_config->ch5Rev - << m_config->ch6Rev - << m_config->ch7Rev; - - inChannelAssign << m_config->ch0Assign - << m_config->ch1Assign - << m_config->ch2Assign - << m_config->ch3Assign - << m_config->ch4Assign - << m_config->ch5Assign - << m_config->ch6Assign - << m_config->ch7Assign; - - // Now connect the widget to the ManualControlCommand / Channel UAVObject - UAVDataObject* obj = dynamic_cast(objManager->getObject(QString("ManualControlCommand"))); - connect(obj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(updateChannels(UAVObject*))); - - // Register for ManualControlSettings changes: - obj = dynamic_cast(objManager->getObject(QString("ManualControlSettings"))); - connect(obj,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(refreshValues())); - - - // Get the receiver types supported by OpenPilot and fill the corresponding - // dropdown menu: - obj = dynamic_cast(objManager->getObject(QString("ManualControlSettings"))); - UAVObjectField * field; - // Fill in the dropdown menus for the channel RC Input assignement. - QStringList channelsList; - channelsList << "None"; - QList fieldList = obj->getFields(); - foreach (UAVObjectField* field, fieldList) { - if (field->getUnits().contains("channel")) { - channelsList.append(field->getName()); - } + UAVDataObject * obj = dynamic_cast(objManager->getObject(QString("ManualControlSettings"))); + UAVObjectField * field = obj->getField(QString("ChannelGroups")); + int index=0; + foreach(QString name,field->getElementNames()) + { + inputChannelForm * inp=new inputChannelForm(this,index==0); + m_config->advancedPage->layout()->addWidget(inp); + inp->ui->channelName->setText(name); + addUAVObjectToWidgetRelation("ManualControlSettings","ChannelGroups",inp->ui->channelGroup,index); + addUAVObjectToWidgetRelation("ManualControlSettings","ChannelNumber",inp->ui->channelNumber,index); + addUAVObjectToWidgetRelation("ManualControlSettings","ChannelMin",inp->ui->channelMin,index); + addUAVObjectToWidgetRelation("ManualControlSettings","ChannelNeutral",inp->ui->channelNeutral,index); + addUAVObjectToWidgetRelation("ManualControlSettings","ChannelMax",inp->ui->channelMax,index); + ++index; } - m_config->ch0Assign->addItems(channelsList); - m_config->ch1Assign->addItems(channelsList); - m_config->ch2Assign->addItems(channelsList); - m_config->ch3Assign->addItems(channelsList); - m_config->ch4Assign->addItems(channelsList); - m_config->ch5Assign->addItems(channelsList); - m_config->ch6Assign->addItems(channelsList); - m_config->ch7Assign->addItems(channelsList); + addUAVObjectToWidgetRelation("ManualControlSettings","FlightModePosition",m_config->fmsModePos1,0); + addUAVObjectToWidgetRelation("ManualControlSettings","FlightModePosition",m_config->fmsModePos2,1); + addUAVObjectToWidgetRelation("ManualControlSettings","FlightModePosition",m_config->fmsModePos3,2); - // And the flight mode settings: - field = obj->getField(QString("FlightModePosition")); - m_config->fmsModePos1->addItems(field->getOptions()); - m_config->fmsModePos2->addItems(field->getOptions()); - m_config->fmsModePos3->addItems(field->getOptions()); - field = obj->getField(QString("Stabilization1Settings")); - channelsList.clear(); - channelsList.append(field->getOptions()); - m_config->fmsSsPos1Roll->addItems(channelsList); - m_config->fmsSsPos1Pitch->addItems(channelsList); - m_config->fmsSsPos1Yaw->addItems(channelsList); - m_config->fmsSsPos2Roll->addItems(channelsList); - m_config->fmsSsPos2Pitch->addItems(channelsList); - m_config->fmsSsPos2Yaw->addItems(channelsList); - m_config->fmsSsPos3Roll->addItems(channelsList); - m_config->fmsSsPos3Pitch->addItems(channelsList); - m_config->fmsSsPos3Yaw->addItems(channelsList); + addUAVObjectToWidgetRelation("ManualControlSettings","Stabilization1Settings",m_config->fmsSsPos1Roll,"Roll"); + addUAVObjectToWidgetRelation("ManualControlSettings","Stabilization2Settings",m_config->fmsSsPos2Roll,"Roll"); + addUAVObjectToWidgetRelation("ManualControlSettings","Stabilization3Settings",m_config->fmsSsPos3Roll,"Roll"); + addUAVObjectToWidgetRelation("ManualControlSettings","Stabilization1Settings",m_config->fmsSsPos1Pitch,"Pitch"); + addUAVObjectToWidgetRelation("ManualControlSettings","Stabilization2Settings",m_config->fmsSsPos2Pitch,"Pitch"); + addUAVObjectToWidgetRelation("ManualControlSettings","Stabilization3Settings",m_config->fmsSsPos3Pitch,"Pitch"); + addUAVObjectToWidgetRelation("ManualControlSettings","Stabilization1Settings",m_config->fmsSsPos1Yaw,"Yaw"); + addUAVObjectToWidgetRelation("ManualControlSettings","Stabilization2Settings",m_config->fmsSsPos2Yaw,"Yaw"); + addUAVObjectToWidgetRelation("ManualControlSettings","Stabilization3Settings",m_config->fmsSsPos3Yaw,"Yaw"); - // And the Armin configurations: - field = obj->getField(QString("Arming")); - m_config->armControl->clear(); - m_config->armControl->addItems(field->getOptions()); - - - connect(m_config->saveRCInputToSD, SIGNAL(clicked()), this, SLOT(saveRCInputObject())); - connect(m_config->saveRCInputToRAM, SIGNAL(clicked()), this, SLOT(sendRCInputUpdate())); - - enableControls(false); - refreshValues(); - connect(parent, SIGNAL(autopilotConnected()),this, SLOT(onAutopilotConnect())); - connect(parent, SIGNAL(autopilotDisconnected()), this, SLOT(onAutopilotDisconnect())); - - connect(m_config->ch0Rev, SIGNAL(toggled(bool)), this, SLOT(reverseCheckboxClicked(bool))); - connect(m_config->ch1Rev, SIGNAL(toggled(bool)), this, SLOT(reverseCheckboxClicked(bool))); - connect(m_config->ch2Rev, SIGNAL(toggled(bool)), this, SLOT(reverseCheckboxClicked(bool))); - connect(m_config->ch3Rev, SIGNAL(toggled(bool)), this, SLOT(reverseCheckboxClicked(bool))); - connect(m_config->ch4Rev, SIGNAL(toggled(bool)), this, SLOT(reverseCheckboxClicked(bool))); - connect(m_config->ch5Rev, SIGNAL(toggled(bool)), this, SLOT(reverseCheckboxClicked(bool))); - connect(m_config->ch6Rev, SIGNAL(toggled(bool)), this, SLOT(reverseCheckboxClicked(bool))); - connect(m_config->ch7Rev, SIGNAL(toggled(bool)), this, SLOT(reverseCheckboxClicked(bool))); - connect(m_config->doRCInputCalibration,SIGNAL(stateChanged(int)),this,SLOT(updateTips(int))); - firstUpdate = true; + addUAVObjectToWidgetRelation("ManualControlSettings","Arming",m_config->armControl); + addUAVObjectToWidgetRelation("ManualControlSettings","armTimeout",m_config->armTimeout,0,1000); + populateWidgets(); + refreshWidgetsValues(); // Connect the help button connect(m_config->inputHelp, SIGNAL(clicked()), this, SLOT(openHelp())); - updateTips(Qt::Unchecked); } ConfigInputWidget::~ConfigInputWidget() { - // Do nothing -} - -/** - Slot called whenever we revert a signal - */ -void ConfigInputWidget::reverseCheckboxClicked(bool state) -{ - QObject* obj = sender(); - int i = inRevCheckboxes.indexOf((QCheckBox*)obj); - - inSliders[i]->setInvertedAppearance(state); - int max = inMaxLabels[i]->text().toInt(); - int min = inMinLabels[i]->text().toInt(); - if ((state && (max>min)) || - (!state && (max < min))) { - inMaxLabels[i]->setText(QString::number(min)); - inMinLabels[i]->setText(QString::number(max)); - } -} - - -// ************************************ - -/* - Enable or disable some controls depending on whether we are connected - or not to the board. Actually, this i mostly useless IMHO, I don't - know who added this into the code (Ed's note) - */ -void ConfigInputWidget::enableControls(bool enable) -{ - //m_config->saveRCInputToRAM->setEnabled(enable); - m_config->saveRCInputToSD->setEnabled(enable); - m_config->doRCInputCalibration->setEnabled(enable); -} - - -/******************************** - * Input settings - *******************************/ - -/** - Request the current config from the board - */ -void ConfigInputWidget::refreshValues() -{ - UAVDataObject* obj = dynamic_cast(getObjectManager()->getObject(QString("ManualControlSettings"))); - Q_ASSERT(obj); - //obj->requestUpdate(); - UAVObjectField *field; - - // Now update all the slider values: - - UAVObjectField *field_max = obj->getField(QString("ChannelMax")); - UAVObjectField *field_min = obj->getField(QString("ChannelMin")); - UAVObjectField *field_neu = obj->getField(QString("ChannelNeutral")); - Q_ASSERT(field_max); - Q_ASSERT(field_min); - Q_ASSERT(field_neu); - for (int i = 0; i < 8; i++) { - QVariant max = field_max->getValue(i); - QVariant min = field_min->getValue(i); - QVariant neutral = field_neu->getValue(i); - inMaxLabels[i]->setText(max.toString()); - inMinLabels[i]->setText(min.toString()); - if (max.toInt()> min.toInt()) { - inRevCheckboxes[i]->setChecked(false); - inSliders[i]->setMaximum(max.toInt()); - inSliders[i]->setMinimum(min.toInt()); - } else { - inRevCheckboxes[i]->setChecked(true); - inSliders[i]->setMaximum(min.toInt()); - inSliders[i]->setMinimum(max.toInt()); - } - inSliders[i]->setValue(neutral.toInt()); - } - - // Update receiver type - field = obj->getField(QString("InputMode")); - m_config->receiverType->setText(field->getValue().toString()); - - // Reset all channel assignement dropdowns: - foreach (QComboBox *combo, inChannelAssign) { - combo->setCurrentIndex(0); - } - - // Update all channels assignements - QList fieldList = obj->getFields(); - foreach (UAVObjectField *field, fieldList) { - if (field->getUnits().contains("channel")) - assignChannel(obj, field->getName()); - } - - // Update all the flight mode settingsin the relevant tab - field = obj->getField(QString("FlightModePosition")); - m_config->fmsModePos1->setCurrentIndex((m_config->fmsModePos1->findText(field->getValue(0).toString()))); - m_config->fmsModePos2->setCurrentIndex((m_config->fmsModePos2->findText(field->getValue(1).toString()))); - m_config->fmsModePos3->setCurrentIndex((m_config->fmsModePos3->findText(field->getValue(2).toString()))); - - field = obj->getField(QString("Stabilization1Settings")); - m_config->fmsSsPos1Roll->setCurrentIndex(m_config->fmsSsPos1Roll->findText(field->getValue(field->getElementNames().indexOf("Roll")).toString())); - m_config->fmsSsPos1Pitch->setCurrentIndex(m_config->fmsSsPos1Pitch->findText(field->getValue(field->getElementNames().indexOf("Pitch")).toString())); - m_config->fmsSsPos1Yaw->setCurrentIndex(m_config->fmsSsPos1Yaw->findText(field->getValue(field->getElementNames().indexOf("Yaw")).toString())); - field = obj->getField(QString("Stabilization2Settings")); - m_config->fmsSsPos2Roll->setCurrentIndex(m_config->fmsSsPos2Roll->findText(field->getValue(field->getElementNames().indexOf("Roll")).toString())); - m_config->fmsSsPos2Pitch->setCurrentIndex(m_config->fmsSsPos2Pitch->findText(field->getValue(field->getElementNames().indexOf("Pitch")).toString())); - m_config->fmsSsPos2Yaw->setCurrentIndex(m_config->fmsSsPos2Yaw->findText(field->getValue(field->getElementNames().indexOf("Yaw")).toString())); - field = obj->getField(QString("Stabilization3Settings")); - m_config->fmsSsPos3Roll->setCurrentIndex(m_config->fmsSsPos3Roll->findText(field->getValue(field->getElementNames().indexOf("Roll")).toString())); - m_config->fmsSsPos3Pitch->setCurrentIndex(m_config->fmsSsPos3Pitch->findText(field->getValue(field->getElementNames().indexOf("Pitch")).toString())); - m_config->fmsSsPos3Yaw->setCurrentIndex(m_config->fmsSsPos3Yaw->findText(field->getValue(field->getElementNames().indexOf("Yaw")).toString())); - - // Load the arming settings - field = obj->getField(QString("Arming")); - m_config->armControl->setCurrentIndex(m_config->armControl->findText(field->getValue().toString())); - field = obj->getField(QString("ArmedTimeout")); - m_config->armTimeout->setValue(field->getValue().toInt()/1000); -} - - -/** - * Sends the config to the board, without saving to the SD card - */ -void ConfigInputWidget::sendRCInputUpdate() -{ - ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - UAVObjectManager *objManager = pm->getObject(); - UAVDataObject* obj = dynamic_cast(objManager->getObject(QString("ManualControlSettings"))); - Q_ASSERT(obj); - // Now update all fields from the sliders: - QString fieldName = QString("ChannelMax"); - UAVObjectField * field = obj->getField(fieldName); - for (int i = 0; i < 8; i++) { - field->setValue(inMaxLabels[i]->text().toInt(), i); - } - - fieldName = QString("ChannelMin"); - field = obj->getField(fieldName); - for (int i = 0; i < 8; i++) { - field->setValue(inMinLabels[i]->text().toInt(), i); - } - - fieldName = QString("ChannelNeutral"); - field = obj->getField(fieldName); - for (int i = 0; i < 8; i++) - field->setValue(inSliders[i]->value(), i); - - // Set Roll/Pitch/Yaw/Etc assignement: - // Rule: if two channels have the same setting (which is wrong!) the higher channel - // will get the setting. - - // First, reset all channel assignements: - QList fieldList = obj->getFields(); - foreach (UAVObjectField* field, fieldList) { - if (field->getUnits().contains("channel")) { - field->setValue(field->getOptions().last()); - } - } - - // Then assign according to current GUI state: - if (m_config->ch0Assign->currentIndex() != 0) { - field = obj->getField(m_config->ch0Assign->currentText()); - field->setValue(field->getOptions().at(0)); // -> This way we don't depend on channel naming convention - } - if (m_config->ch1Assign->currentIndex() != 0) { - field = obj->getField(m_config->ch1Assign->currentText()); - field->setValue(field->getOptions().at(1)); - } - if (m_config->ch2Assign->currentIndex() != 0) { - field = obj->getField(m_config->ch2Assign->currentText()); - field->setValue(field->getOptions().at(2)); - } - if (m_config->ch3Assign->currentIndex() != 0) { - field = obj->getField(m_config->ch3Assign->currentText()); - field->setValue(field->getOptions().at(3)); - } - if (m_config->ch4Assign->currentIndex() != 0) { - field = obj->getField(m_config->ch4Assign->currentText()); - field->setValue(field->getOptions().at(4)); - } - if (m_config->ch5Assign->currentIndex() != 0) { - field = obj->getField(m_config->ch5Assign->currentText()); - field->setValue(field->getOptions().at(5)); - } - if (m_config->ch6Assign->currentIndex() != 0) { - field = obj->getField(m_config->ch6Assign->currentText()); - field->setValue(field->getOptions().at(6)); - } - if (m_config->ch7Assign->currentIndex() != 0) { - field = obj->getField(m_config->ch7Assign->currentText()); - field->setValue(field->getOptions().at(7)); - } - - // Send all the flight mode settings - field = obj->getField(QString("FlightModePosition")); - field->setValue(m_config->fmsModePos1->currentText(),0); - field->setValue(m_config->fmsModePos2->currentText(),1); - field->setValue(m_config->fmsModePos3->currentText(),2); - - field = obj->getField(QString("Stabilization1Settings")); - field->setValue(m_config->fmsSsPos1Roll->currentText(), field->getElementNames().indexOf("Roll")); - field->setValue(m_config->fmsSsPos1Pitch->currentText(), field->getElementNames().indexOf("Pitch")); - field->setValue(m_config->fmsSsPos1Yaw->currentText(), field->getElementNames().indexOf("Yaw")); - field = obj->getField(QString("Stabilization2Settings")); - field->setValue(m_config->fmsSsPos2Roll->currentText(), field->getElementNames().indexOf("Roll")); - field->setValue(m_config->fmsSsPos2Pitch->currentText(), field->getElementNames().indexOf("Pitch")); - field->setValue(m_config->fmsSsPos2Yaw->currentText(), field->getElementNames().indexOf("Yaw")); - field = obj->getField(QString("Stabilization3Settings")); - field->setValue(m_config->fmsSsPos3Roll->currentText(), field->getElementNames().indexOf("Roll")); - field->setValue(m_config->fmsSsPos3Pitch->currentText(), field->getElementNames().indexOf("Pitch")); - field->setValue(m_config->fmsSsPos3Yaw->currentText(), field->getElementNames().indexOf("Yaw")); - - // Save the arming settings - field = obj->getField(QString("Arming")); - field->setValue(m_config->armControl->currentText()); - field = obj->getField(QString("ArmedTimeout")); - field->setValue(m_config->armTimeout->value()*1000); - - // ... and send to the OP Board - obj->updated(); - -} - -/** - Sends the config to the board and request saving into the SD card - */ -void ConfigInputWidget::saveRCInputObject() -{ - // Send update so that the latest value is saved - sendRCInputUpdate(); - UAVDataObject* obj = dynamic_cast(getObjectManager()->getObject(QString("ManualControlSettings"))); - Q_ASSERT(obj); - saveObjectToSD(obj); -} - - -/** - * Set the dropdown option for a channel Input assignement - */ -void ConfigInputWidget::assignChannel(UAVDataObject *obj, QString str) -{ - UAVObjectField* field = obj->getField(str); - QStringList options = field->getOptions(); - switch (options.indexOf(field->getValue().toString())) { - case 0: - m_config->ch0Assign->setCurrentIndex(m_config->ch0Assign->findText(str)); - break; - case 1: - m_config->ch1Assign->setCurrentIndex(m_config->ch0Assign->findText(str)); - break; - case 2: - m_config->ch2Assign->setCurrentIndex(m_config->ch0Assign->findText(str)); - break; - case 3: - m_config->ch3Assign->setCurrentIndex(m_config->ch0Assign->findText(str)); - break; - case 4: - m_config->ch4Assign->setCurrentIndex(m_config->ch0Assign->findText(str)); - break; - case 5: - m_config->ch5Assign->setCurrentIndex(m_config->ch0Assign->findText(str)); - break; - case 6: - m_config->ch6Assign->setCurrentIndex(m_config->ch0Assign->findText(str)); - break; - case 7: - m_config->ch7Assign->setCurrentIndex(m_config->ch0Assign->findText(str)); - break; - } -} - -/** - * Updates the slider positions and min/max values - * - */ -void ConfigInputWidget::updateChannels(UAVObject* controlCommand) -{ - - QString fieldName = QString("Connected"); - UAVObjectField *field = controlCommand->getField(fieldName); - if (field->getValue().toBool()) - { - m_config->RCInputConnected->setText("RC Receiver connected"); - m_config->lblMissingInputs->setText(""); - } - else - { - m_config->RCInputConnected->setText("RC Receiver not connected or invalid input configuration (missing channels)"); - receiverHelp(); - } - if (m_config->doRCInputCalibration->isChecked()) { - if (firstUpdate) { - // Increase the data rate from the board so that the sliders - // move faster - UAVObject::Metadata mdata = controlCommand->getMetadata(); - mdata.flightTelemetryUpdateMode = UAVObject::UPDATEMODE_PERIODIC; - mccDataRate = mdata.flightTelemetryUpdatePeriod; - mdata.flightTelemetryUpdatePeriod = 150; - controlCommand->setMetadata(mdata); - - // Also protect the user by setting all values to zero - // and making the ActuatorCommand object readonly - UAVDataObject* obj = dynamic_cast(getObjectManager()->getObject(QString("ActuatorCommand"))); - mdata = obj->getMetadata(); - mdata.flightAccess = UAVObject::ACCESS_READONLY; - obj->setMetadata(mdata); - UAVObjectField *field = obj->getField("Channel"); - for (uint i=0; i< field->getNumElements(); i++) { - field->setValue(0,i); - } - obj->updated(); - - // OP-534: make sure the airframe can NEVER arm - obj = dynamic_cast(getObjectManager()->getObject(QString("ManualControlSettings"))); - field = obj->getField("Arming"); - field->setValue("Always Disarmed"); - obj->updated(); - - // Last, make sure the user won't apply/save during calibration - m_config->saveRCInputToRAM->setEnabled(false); - m_config->saveRCInputToSD->setEnabled(false); - - // Reset all slider values to zero - field = controlCommand->getField(QString("Channel")); - for (int i = 0; i < 8; i++) - updateChannelInSlider(inSliders[i], inMinLabels[i], inMaxLabels[i], field->getValue(i).toInt(),inRevCheckboxes[i]->isChecked()); - firstUpdate = false; - // Tell a few things to the user: - QMessageBox msgBox; - msgBox.setText(tr("Arming Settings are now set to Always Disarmed for your safety.")); - msgBox.setDetailedText(tr("You will have to reconfigure arming settings yourself afterwards.")); - msgBox.setStandardButtons(QMessageBox::Ok); - msgBox.setDefaultButton(QMessageBox::Ok); - msgBox.exec(); - - } - - field = controlCommand->getField(QString("Channel")); - for (int i = 0; i < 8; i++) - updateChannelInSlider(inSliders[i], inMinLabels[i], inMaxLabels[i], field->getValue(i).toInt(),inRevCheckboxes[i]->isChecked()); - } - else { - if (!firstUpdate) { - // Restore original data rate from the board: - UAVObject::Metadata mdata = controlCommand->getMetadata(); - mdata.flightTelemetryUpdateMode = UAVObject::UPDATEMODE_PERIODIC; - mdata.flightTelemetryUpdatePeriod = mccDataRate; - controlCommand->setMetadata(mdata); - - UAVDataObject* obj = dynamic_cast(getObjectManager()->getObject(QString("ActuatorCommand"))); - mdata = obj->getMetadata(); - mdata.flightAccess = UAVObject::ACCESS_READWRITE; - obj->setMetadata(mdata); - - // Set some slider values to better defaults - // Find some channels first - int throttleChannel = -1; - int fmChannel = -1; - for (int i=0; i < inChannelAssign.length(); i++) { - if (inChannelAssign.at(i)->currentText() == "Throttle") { - // TODO: this is very ugly, because this relies on the name of the - // channel input, everywhere else in the gadget we don't rely on the - // naming... - throttleChannel = i; - } - if (inChannelAssign.at(i)->currentText() == "FlightMode") { - // TODO: this is very ugly, because this relies on the name of the - // channel input, everywhere else in the gadget we don't rely on the - // naming... - fmChannel = i; - } - } - - // Throttle neutral defaults to 2% of range - if (throttleChannel > -1) { - inSliders.at(throttleChannel)->setValue( - inSliders.at(throttleChannel)->minimum() + - (inSliders.at(throttleChannel)->maximum()- - inSliders.at(throttleChannel)->minimum())*0.02); - } - - // Flight mode at 50% of range: - if (fmChannel > -1) { - inSliders.at(fmChannel)->setValue( - inSliders.at(fmChannel)->minimum()+ - (inSliders.at(fmChannel)->maximum()- - inSliders.at(fmChannel)->minimum())*0.5); - } - - m_config->saveRCInputToRAM->setEnabled(true); - m_config->saveRCInputToSD->setEnabled(true); - } - firstUpdate = true; - } - - //Update the Flight mode channel slider - ManualControlSettings * manualSettings = ManualControlSettings::GetInstance(getObjectManager()); - ManualControlSettings::DataFields manualSettingsData = manualSettings->getData(); - uint chIndex = manualSettingsData.FlightMode; - if (chIndex < manualSettings->FLIGHTMODE_NONE) { - float valueScaled; - - int chMin = manualSettingsData.ChannelMin[chIndex]; - int chMax = manualSettingsData.ChannelMax[chIndex]; - int chNeutral = manualSettingsData.ChannelNeutral[chIndex]; - - int value = controlCommand->getField("Channel")->getValue(chIndex).toInt(); - if ((chMax > chMin && value >= chNeutral) || (chMin > chMax && value <= chNeutral)) - { - if (chMax != chNeutral) - valueScaled = (float)(value - chNeutral) / (float)(chMax - chNeutral); - else - valueScaled = 0; - } - else - { - if (chMin != chNeutral) - valueScaled = (float)(value - chNeutral) / (float)(chNeutral - chMin); - else - valueScaled = 0; - } - - if(valueScaled < -(1.0 / 3.0)) - m_config->fmsSlider->setValue(-100); - else if (valueScaled > (1.0/3.0)) - m_config->fmsSlider->setValue(100); - else - m_config->fmsSlider->setValue(0); - - } -} - -void ConfigInputWidget::updateChannelInSlider(QSlider *slider, QLabel *min, QLabel *max, int value, bool reversed) -{ - if (!slider || !min || !max) - return; - - if (firstUpdate) { - // Reset all the min/max values of the progress bar since we are starting the calibration. - slider->setMaximum(value); - slider->setMinimum(value); - slider->setValue(value); - max->setText(QString::number(value)); - min->setText(QString::number(value)); - return; - } - - if (value > 0) { - // avoids glitches... - if (value > slider->maximum()) { - slider->setMaximum(value); - if (reversed) - min->setText(QString::number(value)); - else - max->setText(QString::number(value)); - } - if (value < slider->minimum()) { - slider->setMinimum(value); - if (reversed) - max->setText(QString::number(value)); - else - min->setText(QString::number(value)); - } - slider->setValue(value); - } + // Do nothing } void ConfigInputWidget::openHelp() @@ -653,77 +100,5 @@ void ConfigInputWidget::openHelp() QDesktopServices::openUrl( QUrl("http://wiki.openpilot.org/display/Doc/Input+Configuration", QUrl::StrictMode) ); } -void ConfigInputWidget::receiverHelp() -{ - QString unassigned; - ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - UAVObjectManager *objManager = pm->getObject(); - UAVDataObject* controlCommand = dynamic_cast(objManager->getObject(QString("ManualControlSettings"))); - UAVObjectField *field; - field= controlCommand->getField("Roll"); - if(field->getValue().toString()=="None") - unassigned.append("Roll"); - - field =controlCommand->getField("Pitch"); - if(field->getValue().toString()=="None") - { - if(unassigned.length()>0) - unassigned.append(", "); - unassigned.append("Pitch"); - } - - field =controlCommand->getField("Yaw"); - if(field->getValue().toString()=="None") - { - if(unassigned.length()>0) - unassigned.append(", "); - unassigned.append("Yaw"); - } - - field =controlCommand->getField("Throttle"); - if(field->getValue().toString()=="None") - { - if(unassigned.length()>0) - unassigned.append(", "); - unassigned.append("Throttle"); - } - - field =controlCommand->getField("FlightMode"); - if(field->getValue().toString()=="None") - { - if(unassigned.length()>0) - unassigned.append(", "); - unassigned.append("FlightMode"); - } - if(unassigned.length()>0) - m_config->lblMissingInputs->setText(QString("Channels left to assign: ")+unassigned); - else - m_config->lblMissingInputs->setText(""); -} -void ConfigInputWidget::updateTips(int value) -{ - if(value==Qt::Checked) - { - m_config->ch0Cur->setToolTip("Current channel value"); - m_config->ch1Cur->setToolTip("Current channel value"); - m_config->ch2Cur->setToolTip("Current channel value"); - m_config->ch3Cur->setToolTip("Current channel value"); - m_config->ch4Cur->setToolTip("Current channel value"); - m_config->ch5Cur->setToolTip("Current channel value"); - m_config->ch6Cur->setToolTip("Current channel value"); - m_config->ch7Cur->setToolTip("Current channel value"); - } - else - { - m_config->ch0Cur->setToolTip("Channel neutral point"); - m_config->ch1Cur->setToolTip("Channel neutral point"); - m_config->ch2Cur->setToolTip("Channel neutral point"); - m_config->ch3Cur->setToolTip("Channel neutral point"); - m_config->ch4Cur->setToolTip("Channel neutral point"); - m_config->ch5Cur->setToolTip("Channel neutral point"); - m_config->ch6Cur->setToolTip("Channel neutral point"); - m_config->ch7Cur->setToolTip("Channel neutral point"); - } -} diff --git a/ground/openpilotgcs/src/plugins/config/configinputwidget.h b/ground/openpilotgcs/src/plugins/config/configinputwidget.h index 75f47cc8d..5897a6f05 100644 --- a/ground/openpilotgcs/src/plugins/config/configinputwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configinputwidget.h @@ -34,7 +34,8 @@ #include "uavobject.h" #include #include - +#include "inputchannelform.h" +#include "ui_inputchannelform.h" class Ui_InputWidget; class ConfigInputWidget: public ConfigTaskWidget @@ -50,36 +51,9 @@ public slots: private: Ui_InputWidget *m_config; - QList sliders; - - void updateChannelInSlider(QSlider *slider, QLabel *min, QLabel *max, int value, bool reversed); - - void assignChannel(UAVDataObject *obj, QString str); - void assignOutputChannel(UAVDataObject *obj, QString str); - - int mccDataRate; - - UAVObject::Metadata accInitialData; - - QList inSliders; - QList inMaxLabels; - QList inMinLabels; - QList inNeuLabels; - QList inRevCheckboxes; - QList inChannelAssign; - - bool firstUpdate; - - virtual void enableControls(bool enable); - void receiverHelp(); private slots: - void updateChannels(UAVObject* obj); - virtual void refreshValues(); - void sendRCInputUpdate(); - void saveRCInputObject(); - void reverseCheckboxClicked(bool state); + void openHelp(); - void updateTips(int); }; #endif diff --git a/ground/openpilotgcs/src/plugins/config/configtaskwidget.cpp b/ground/openpilotgcs/src/plugins/config/configtaskwidget.cpp index 87309ca63..90d21e494 100644 --- a/ground/openpilotgcs/src/plugins/config/configtaskwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configtaskwidget.cpp @@ -43,13 +43,24 @@ void ConfigTaskWidget::addUAVObject(QString objectName) { addUAVObjectToWidgetRelation(objectName,"",NULL); } -void ConfigTaskWidget::addUAVObjectToWidgetRelation(QString object, QString field, QWidget * widget) +void ConfigTaskWidget::addUAVObjectToWidgetRelation(QString object, QString field, QWidget * widget, QString index) +{ + UAVObject *obj=NULL; + UAVObjectField *_field=NULL; + obj = objManager->getObject(QString(object)); + _field = obj->getField(QString(field)); + addUAVObjectToWidgetRelation(object,field,widget,_field->getElementNames().indexOf(index)); +} + +void ConfigTaskWidget::addUAVObjectToWidgetRelation(QString object, QString field, QWidget * widget, int index,int scale) { UAVObject *obj=NULL; UAVObjectField *_field=NULL; if(!object.isEmpty()) + { obj = objManager->getObject(QString(object)); - connect(obj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(refreshWidgetsValues())); + connect(obj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(refreshWidgetsValues())); + } //smartsave->addObject(obj); if(!field.isEmpty() && obj) _field = obj->getField(QString(field)); @@ -57,6 +68,8 @@ void ConfigTaskWidget::addUAVObjectToWidgetRelation(QString object, QString fiel ow->field=_field; ow->object=obj; ow->widget=widget; + ow->index=index; + ow->scale=scale; objOfInterest.append(ow); if(obj) smartsave->addObject(obj); @@ -156,11 +169,15 @@ void ConfigTaskWidget::populateWidgets() else if(QComboBox * cb=qobject_cast(ow->widget)) { cb->addItems(ow->field->getOptions()); - cb->setCurrentIndex(cb->findText(ow->field->getValue().toString())); + cb->setCurrentIndex(cb->findText(ow->field->getValue(ow->index).toString())); } else if(QLabel * cb=qobject_cast(ow->widget)) { - cb->setText(ow->field->getValue().toString()); + cb->setText(ow->field->getValue(ow->index).toString()); + } + else if(QSpinBox * cb=qobject_cast(ow->widget)) + { + cb->setValue(ow->field->getValue(ow->index).toInt()/ow->scale); } } setDirty(dirtyBack); @@ -177,11 +194,15 @@ void ConfigTaskWidget::refreshWidgetsValues() } else if(QComboBox * cb=qobject_cast(ow->widget)) { - cb->setCurrentIndex(cb->findText(ow->field->getValue().toString())); + cb->setCurrentIndex(cb->findText(ow->field->getValue(ow->index).toString())); } else if(QLabel * cb=qobject_cast(ow->widget)) { - cb->setText(ow->field->getValue().toString()); + cb->setText(ow->field->getValue(ow->index).toString()); + } + else if(QSpinBox * cb=qobject_cast(ow->widget)) + { + cb->setValue(ow->field->getValue(ow->index).toInt()/ow->scale); } } setDirty(dirtyBack); @@ -197,11 +218,15 @@ void ConfigTaskWidget::updateObjectsFromWidgets() } else if(QComboBox * cb=qobject_cast(ow->widget)) { - ow->field->setValue(cb->currentText()); + ow->field->setValue(cb->currentText(),ow->index); } else if(QLabel * cb=qobject_cast(ow->widget)) { - ow->field->setValue(cb->text()); + ow->field->setValue(cb->text(),ow->index); + } + else if(QSpinBox * cb=qobject_cast(ow->widget)) + { + ow->field->setValue(cb->value()* ow->scale,ow->index); } } } diff --git a/ground/openpilotgcs/src/plugins/config/configtaskwidget.h b/ground/openpilotgcs/src/plugins/config/configtaskwidget.h index f5de26ed2..1db0dde9c 100644 --- a/ground/openpilotgcs/src/plugins/config/configtaskwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configtaskwidget.h @@ -54,6 +54,8 @@ public: UAVObject * object; UAVObjectField * field; QWidget * widget; + int index; + int scale; }; ConfigTaskWidget(QWidget *parent = 0); @@ -63,10 +65,12 @@ public: static double listMean(QList list); void addUAVObject(QString objectName); void addWidget(QWidget * widget); - void addUAVObjectToWidgetRelation(QString object,QString field,QWidget * widget); + void addUAVObjectToWidgetRelation(QString object,QString field,QWidget * widget,int index=0,int scale=1); + void setupButtons(QPushButton * update,QPushButton * save); bool isDirty(); void setDirty(bool value); + void addUAVObjectToWidgetRelation(QString object, QString field, QWidget *widget, QString index); public slots: void onAutopilotDisconnect(); void onAutopilotConnect(); @@ -75,6 +79,7 @@ private slots: virtual void refreshValues(); virtual void updateObjectsFromWidgets(); private: + QStringList objectsList; QList objOfInterest; ExtensionSystem::PluginManager *pm; UAVObjectManager *objManager; diff --git a/ground/openpilotgcs/src/plugins/config/input.ui b/ground/openpilotgcs/src/plugins/config/input.ui index e1eb0b435..065967c26 100644 --- a/ground/openpilotgcs/src/plugins/config/input.ui +++ b/ground/openpilotgcs/src/plugins/config/input.ui @@ -25,770 +25,11 @@ - - - - 75 - true - - - - Receiver Type: - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - - - 234 - 54 - - - - - 75 - true - - - - Indicates whether OpenPilot is getting a signal from the RC receiver. - - - RC Receiver not connected or invalid input configuration (missing channels) - - - true - - - - - - - - - Rev. - - - Qt::AlignBottom|Qt::AlignLeading|Qt::AlignLeft - - - - - - - - 16 - 16 - - - - true - - - - - - - true - - - 1000 - - - 2000 - - - 1500 - - - Qt::Horizontal - - - - - - - <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> -<html><head><meta name="qrichtext" content="1" /><style type="text/css"> -p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'FreeSans'; font-size:8pt; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Maximum channel pulse width</p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">(microseconds)</p></body></html> - - - 2000 - - - - - - - Check this to reverse the channel. -(Useful for transmitters without channel -reversal capabilities). - - - - - - - - - - - - - true - - - 1000 - - - 2000 - - - 1500 - - - Qt::Horizontal - - - - - - - <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> -<html><head><meta name="qrichtext" content="1" /><style type="text/css"> -p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'FreeSans'; font-size:8pt; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Maximum channel pulse width</p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">(microseconds)</p></body></html> - - - 2000 - - - - - - - Check this to reverse the channel. -(Useful for transmitters without channel -reversal capabilities). - - - - - - - - - - - - - true - - - 1000 - - - 2000 - - - 1500 - - - Qt::Horizontal - - - - - - - <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> -<html><head><meta name="qrichtext" content="1" /><style type="text/css"> -p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'FreeSans'; font-size:8pt; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Maximum channel pulse width</p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">(microseconds)</p></body></html> - - - 2000 - - - - - - - Check this to reverse the channel. -(Useful for transmitters without channel -reversal capabilities). - - - - - - - - - - - - - true - - - 1000 - - - 2000 - - - 1500 - - - Qt::Horizontal - - - - - - - <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> -<html><head><meta name="qrichtext" content="1" /><style type="text/css"> -p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'FreeSans'; font-size:8pt; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Maximum channel pulse width</p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">(microseconds)</p></body></html> - - - 2000 - - - - - - - Check this to reverse the channel. -(Useful for transmitters without channel -reversal capabilities). - - - - - - - - - - - - - true - - - 1000 - - - 2000 - - - 1500 - - - Qt::Horizontal - - - - - - - <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> -<html><head><meta name="qrichtext" content="1" /><style type="text/css"> -p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'FreeSans'; font-size:8pt; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Maximum channel pulse width</p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">(microseconds)</p></body></html> - - - 2000 - - - - - - - Check this to reverse the channel. -(Useful for transmitters without channel -reversal capabilities). - - - - - - - - - - - - - true - - - 1000 - - - 2000 - - - 1500 - - - Qt::Horizontal - - - - - - - <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> -<html><head><meta name="qrichtext" content="1" /><style type="text/css"> -p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'FreeSans'; font-size:8pt; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Maximum channel pulse width</p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">(microseconds)</p></body></html> - - - 2000 - - - - - - - Check this to reverse the channel. -(Useful for transmitters without channel -reversal capabilities). - - - - - - - - - - - - - true - - - 1000 - - - 2000 - - - 1500 - - - Qt::Horizontal - - - - - - - <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> -<html><head><meta name="qrichtext" content="1" /><style type="text/css"> -p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'FreeSans'; font-size:8pt; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Maximum channel pulse width</p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">(microseconds)</p></body></html> - - - 2000 - - - - - - - Check this to reverse the channel. -(Useful for transmitters without channel -reversal capabilities). - - - - - - - - - - - - - true - - - 1000 - - - 2000 - - - 1500 - - - Qt::Horizontal - - - - - - - <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> -<html><head><meta name="qrichtext" content="1" /><style type="text/css"> -p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'FreeSans'; font-size:8pt; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Maximum channel pulse width</p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">(microseconds)</p></body></html> - - - 2000 - - - - - - - Check this to reverse the channel. -(Useful for transmitters without channel -reversal capabilities). - - - - - - - - - - - 75 - true - - - - BEWARE: make sure your engines are not connected when running calibration! - - - - - - - - - 75 - true - - - - - - - - - - - - 50 - false - - - - Start calibrating the RC Inputs. -Uncheck/Check to restart calibration. -During calibration: move your RC controls over their whole range, -then leave them on Neutral, uncheck calibration and save. -Neutral should be put at the bottom of the slider for the throttle. - - - Run Calibration - - - - - - - - - - - - - - <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> -<html><head><meta name="qrichtext" content="1" /><style type="text/css"> -p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'FreeSans'; font-size:8pt; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Minimum channel pulse width</p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">(microseconds)</p></body></html> - - - 1000 - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> -<html><head><meta name="qrichtext" content="1" /><style type="text/css"> -p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'FreeSans'; font-size:8pt; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Minimum channel pulse width</p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">(microseconds)</p></body></html> - - - 1000 - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> -<html><head><meta name="qrichtext" content="1" /><style type="text/css"> -p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'FreeSans'; font-size:8pt; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Minimum channel pulse width</p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">(microseconds)</p></body></html> - - - 1000 - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> -<html><head><meta name="qrichtext" content="1" /><style type="text/css"> -p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'FreeSans'; font-size:8pt; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Minimum channel pulse width</p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">(microseconds)</p></body></html> - - - 1000 - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> -<html><head><meta name="qrichtext" content="1" /><style type="text/css"> -p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'FreeSans'; font-size:8pt; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Minimum channel pulse width</p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">(microseconds)</p></body></html> - - - 1000 - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> -<html><head><meta name="qrichtext" content="1" /><style type="text/css"> -p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'FreeSans'; font-size:8pt; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Minimum channel pulse width</p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">(microseconds)</p></body></html> - - - 1000 - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> -<html><head><meta name="qrichtext" content="1" /><style type="text/css"> -p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'FreeSans'; font-size:8pt; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Minimum channel pulse width</p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">(microseconds)</p></body></html> - - - 1000 - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> -<html><head><meta name="qrichtext" content="1" /><style type="text/css"> -p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'FreeSans'; font-size:8pt; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Minimum channel pulse width</p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">(microseconds)</p></body></html> - - - 1000 - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> -<html><head><meta name="qrichtext" content="1" /><style type="text/css"> -p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'FreeSans'; font-size:10pt; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:9pt;">Current channel value.</span></p></body></html> - - - 1500 - - - - - - - <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> -<html><head><meta name="qrichtext" content="1" /><style type="text/css"> -p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'FreeSans'; font-size:10pt; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:9pt;">Current channel value.</span></p></body></html> - - - 1500 - - - - - - - <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> -<html><head><meta name="qrichtext" content="1" /><style type="text/css"> -p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'FreeSans'; font-size:10pt; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:9pt;">Current channel value.</span></p></body></html> - - - 1500 - - - - - - - <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> -<html><head><meta name="qrichtext" content="1" /><style type="text/css"> -p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'FreeSans'; font-size:10pt; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:9pt;">Current channel value.</span></p></body></html> - - - 1500 - - - - - - - <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> -<html><head><meta name="qrichtext" content="1" /><style type="text/css"> -p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'FreeSans'; font-size:10pt; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:9pt;">Current channel value.</span></p></body></html> - - - 1500 - - - - - - - <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> -<html><head><meta name="qrichtext" content="1" /><style type="text/css"> -p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'FreeSans'; font-size:10pt; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:9pt;">Current channel value.</span></p></body></html> - - - 1500 - - - - - - - <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> -<html><head><meta name="qrichtext" content="1" /><style type="text/css"> -p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'FreeSans'; font-size:10pt; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:9pt;">Current channel value.</span></p></body></html> - - - 1500 - - - - - - - <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> -<html><head><meta name="qrichtext" content="1" /><style type="text/css"> -p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'FreeSans'; font-size:10pt; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:9pt;">Current channel value.</span></p></body></html> - - - 1500 - + + + + + @@ -1243,14 +484,6 @@ Applies and Saves all settings to SD - ch0Assign - ch1Assign - ch2Assign - ch3Assign - ch4Assign - ch5Assign - ch6Assign - ch7Assign fmsSlider fmsModePos3 fmsSsPos3Roll @@ -1268,134 +501,5 @@ Applies and Saves all settings to SD - - - inSlider0 - valueChanged(int) - ch0Cur - setNum(int) - - - 291 - 93 - - - 150 - 104 - - - - - inSlider1 - valueChanged(int) - ch1Cur - setNum(int) - - - 283 - 137 - - - 160 - 138 - - - - - inSlider2 - valueChanged(int) - ch2Cur - setNum(int) - - - 341 - 163 - - - 156 - 167 - - - - - inSlider3 - valueChanged(int) - ch3Cur - setNum(int) - - - 283 - 211 - - - 159 - 210 - - - - - inSlider4 - valueChanged(int) - ch4Cur - setNum(int) - - - 287 - 239 - - - 156 - 242 - - - - - inSlider5 - valueChanged(int) - ch5Cur - setNum(int) - - - 309 - 272 - - - 164 - 276 - - - - - inSlider6 - valueChanged(int) - ch6Cur - setNum(int) - - - 282 - 300 - - - 144 - 311 - - - - - inSlider7 - valueChanged(int) - ch7Cur - setNum(int) - - - 278 - 339 - - - 168 - 340 - - - - + diff --git a/ground/openpilotgcs/src/plugins/config/inputchannelform.cpp b/ground/openpilotgcs/src/plugins/config/inputchannelform.cpp new file mode 100644 index 000000000..904918a8e --- /dev/null +++ b/ground/openpilotgcs/src/plugins/config/inputchannelform.cpp @@ -0,0 +1,29 @@ +#include "inputchannelform.h" +#include "ui_inputchannelform.h" + +inputChannelForm::inputChannelForm(QWidget *parent,bool showlegend) : + QWidget(parent), + ui(new Ui::inputChannelForm) +{ + ui->setupUi(this); + if(!showlegend) + { + layout()->removeWidget(ui->legend0); + layout()->removeWidget(ui->legend1); + layout()->removeWidget(ui->legend2); + layout()->removeWidget(ui->legend3); + layout()->removeWidget(ui->legend4); + layout()->removeWidget(ui->legend5); + delete ui->legend0; + delete ui->legend1; + delete ui->legend2; + delete ui->legend3; + delete ui->legend4; + delete ui->legend5; + } +} + +inputChannelForm::~inputChannelForm() +{ + delete ui; +} diff --git a/ground/openpilotgcs/src/plugins/config/inputchannelform.h b/ground/openpilotgcs/src/plugins/config/inputchannelform.h new file mode 100644 index 000000000..50615cb2c --- /dev/null +++ b/ground/openpilotgcs/src/plugins/config/inputchannelform.h @@ -0,0 +1,23 @@ +#ifndef INPUTCHANNELFORM_H +#define INPUTCHANNELFORM_H + +#include +#include "configinputwidget.h" +namespace Ui { + class inputChannelForm; +} + +class inputChannelForm : public QWidget +{ + Q_OBJECT + +public: + explicit inputChannelForm(QWidget *parent = 0,bool showlegend=false); + ~inputChannelForm(); + friend class ConfigInputWidget; + +private: + Ui::inputChannelForm *ui; +}; + +#endif // INPUTCHANNELFORM_H diff --git a/ground/openpilotgcs/src/plugins/config/inputchannelform.ui b/ground/openpilotgcs/src/plugins/config/inputchannelform.ui new file mode 100644 index 000000000..0c602a21b --- /dev/null +++ b/ground/openpilotgcs/src/plugins/config/inputchannelform.ui @@ -0,0 +1,119 @@ + + + inputChannelForm + + + + 0 + 0 + 389 + 57 + + + + Form + + + + + + true + + + Number + + + + + + + true + + + Min + + + + + + + true + + + Function + + + + + + + true + + + Group + + + + + + + TextLabel + + + + + + + + + + 255 + + + + + + + true + + + Neutral + + + + + + + 9999 + + + + + + + 9999 + + + + + + + true + + + Max + + + + + + + 9999 + + + + + + + + diff --git a/ground/openpilotgcs/src/plugins/config/smartsavebutton.cpp b/ground/openpilotgcs/src/plugins/config/smartsavebutton.cpp index 708c1b5c1..dff4e409e 100644 --- a/ground/openpilotgcs/src/plugins/config/smartsavebutton.cpp +++ b/ground/openpilotgcs/src/plugins/config/smartsavebutton.cpp @@ -35,7 +35,9 @@ void smartSaveButton::processClick() connect(&timer,SIGNAL(timeout()),&loop,SLOT(quit())); obj->updated(); timer.start(1000); + qDebug()<<"begin loop"; loop.exec(); + qDebug()<<"end loop"; timer.stop(); disconnect(obj,SIGNAL(transactionCompleted(UAVObject*,bool)),this,SLOT(transaction_finished(UAVObject*, bool))); disconnect(&timer,SIGNAL(timeout()),&loop,SLOT(quit())); From a454cf844bfb8f39f9ad65e105b9ac42ff29da96 Mon Sep 17 00:00:00 2001 From: sambas Date: Thu, 4 Aug 2011 19:20:05 +0300 Subject: [PATCH 030/145] DX8 support, BIND3 only --- flight/PiOS/STM32F10x/pios_spektrum.c | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/flight/PiOS/STM32F10x/pios_spektrum.c b/flight/PiOS/STM32F10x/pios_spektrum.c index 8b0f38c6b..4206d80d7 100644 --- a/flight/PiOS/STM32F10x/pios_spektrum.c +++ b/flight/PiOS/STM32F10x/pios_spektrum.c @@ -8,7 +8,6 @@ * * @file pios_spektrum.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) * @brief USART commands. Inits USARTs, controls USARTs & Interrupt handlers. (STM32 dependent) * @see The GNU Public License (GPL) Version 3 * @@ -172,9 +171,10 @@ static int32_t PIOS_SPEKTRUM_Decode(uint8_t b) CaptureValue[7]=b; } #endif - /* Known sync bytes, 0x01, 0x02, 0x12 */ + /* Known sync bytes, 0x01, 0x02, 0x12, 0xb2 */ + /* 0xb2 DX8 3bind pulses only */ if (bytecount == 2) { - if (b == 0x01) { + if ((b == 0x01) || (b == 0xb2)) { datalength=0; // 10bit //frames=1; sync = 1; @@ -234,17 +234,17 @@ static int32_t PIOS_SPEKTRUM_Decode(uint8_t b) *@brief clears the channel values */ static void PIOS_SPEKTRUM_Supervisor(uint32_t spektrum_id) { - /* 125hz */ + /* 625hz */ supv_timer++; - if(supv_timer > 5) { + if(supv_timer > 4) { /* sync between frames */ sync = 0; bytecount = 0; prev_byte = 0xFF; frame_error = 0; sync_of++; - /* watchdog activated after 100ms silence */ - if (sync_of > 12) { + /* watchdog activated after 200ms silence */ + if (sync_of > 30) { /* signal lost */ sync_of = 0; for (int i = 0; i < PIOS_SPEKTRUM_NUM_INPUTS; i++) { From fd86c25b49de402c3b94b20f1203a0a340b4e359 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Thu, 4 Aug 2011 15:01:54 -0500 Subject: [PATCH 031/145] Make sure OP compiles with new object init structure. --- flight/Modules/AHRSComms/ahrs_comms.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/flight/Modules/AHRSComms/ahrs_comms.c b/flight/Modules/AHRSComms/ahrs_comms.c index daa3b443a..b2afc17cc 100644 --- a/flight/Modules/AHRSComms/ahrs_comms.c +++ b/flight/Modules/AHRSComms/ahrs_comms.c @@ -52,6 +52,8 @@ #include "ahrs_comms.h" #include "ahrs_spi_comm.h" +#include "ahrsstatus.h" +#include "ahrscalibration.h" // Private constants #define STACK_SIZE configMINIMAL_STACK_SIZE-128 @@ -72,7 +74,7 @@ static void ahrscommsTask(void *parameters); int32_t AHRSCommsStart(void) { // Start main task - AHRSStatusInitialize(); + AhrsStatusInitialize(); AHRSCalibrationInitialize(); AttitudeRawInitialize(); From 1e67b5e3b5c2d7b07f40a8f74aceb3f8ea16477c Mon Sep 17 00:00:00 2001 From: sambas Date: Sat, 6 Aug 2011 22:16:47 +0300 Subject: [PATCH 032/145] DSMx_binder, hw setting 0 disabled, 1-10 bind pulses. Manual disable after successful bind. Only for flexiport. --- flight/CopterControl/System/pios_board.c | 6 ++++-- flight/PiOS/STM32F10x/pios_spektrum.c | 13 +++++++------ flight/PiOS/inc/pios_spektrum_priv.h | 2 +- shared/uavobjectdefinition/hwsettings.xml | 5 +++-- 4 files changed, 15 insertions(+), 11 deletions(-) diff --git a/flight/CopterControl/System/pios_board.c b/flight/CopterControl/System/pios_board.c index ee3deaf5c..d125e1cf6 100644 --- a/flight/CopterControl/System/pios_board.c +++ b/flight/CopterControl/System/pios_board.c @@ -921,6 +921,8 @@ void PIOS_Board_Init(void) { TaskMonitorInitialize(); /* Configure the main IO port */ + uint8_t hwsettings_DSMxBind; + HwSettingsDSMxBindGet(&hwsettings_DSMxBind); uint8_t hwsettings_cc_mainport; HwSettingsCC_MainPortGet(&hwsettings_cc_mainport); @@ -989,7 +991,7 @@ void PIOS_Board_Init(void) { } uint32_t pios_spektrum_id; - if (PIOS_SPEKTRUM_Init(&pios_spektrum_id, &pios_spektrum_main_cfg, &pios_usart_com_driver, pios_usart_spektrum_id, false)) { + if (PIOS_SPEKTRUM_Init(&pios_spektrum_id, &pios_spektrum_main_cfg, &pios_usart_com_driver, pios_usart_spektrum_id, 0)) { PIOS_Assert(0); } } @@ -1051,7 +1053,7 @@ void PIOS_Board_Init(void) { } uint32_t pios_spektrum_id; - if (PIOS_SPEKTRUM_Init(&pios_spektrum_id, &pios_spektrum_flexi_cfg, &pios_usart_com_driver, pios_usart_spektrum_id, false)) { + if (PIOS_SPEKTRUM_Init(&pios_spektrum_id, &pios_spektrum_flexi_cfg, &pios_usart_com_driver, pios_usart_spektrum_id, hwsettings_DSMxBind)) { PIOS_Assert(0); } } diff --git a/flight/PiOS/STM32F10x/pios_spektrum.c b/flight/PiOS/STM32F10x/pios_spektrum.c index 4206d80d7..9c81b6131 100644 --- a/flight/PiOS/STM32F10x/pios_spektrum.c +++ b/flight/PiOS/STM32F10x/pios_spektrum.c @@ -58,7 +58,7 @@ uint8_t sync_of = 0; uint16_t supv_timer=0; static void PIOS_SPEKTRUM_Supervisor(uint32_t spektrum_id); -static bool PIOS_SPEKTRUM_Bind(const struct pios_spektrum_cfg * cfg); +static bool PIOS_SPEKTRUM_Bind(const struct pios_spektrum_cfg * cfg, uint8_t bind); static int32_t PIOS_SPEKTRUM_Decode(uint8_t b); static uint16_t PIOS_SPEKTRUM_RxInCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield) @@ -84,11 +84,11 @@ static uint16_t PIOS_SPEKTRUM_RxInCallback(uint32_t context, uint8_t * buf, uint /** * Bind and Initialise Spektrum satellite receiver */ -int32_t PIOS_SPEKTRUM_Init(uint32_t * spektrum_id, const struct pios_spektrum_cfg *cfg, const struct pios_com_driver * driver, uint32_t lower_id, bool bind) +int32_t PIOS_SPEKTRUM_Init(uint32_t * spektrum_id, const struct pios_spektrum_cfg *cfg, const struct pios_com_driver * driver, uint32_t lower_id, uint8_t bind) { // TODO: need setting flag for bind on next powerup if (bind) { - PIOS_SPEKTRUM_Bind(cfg); + PIOS_SPEKTRUM_Bind(cfg,bind); } (driver->bind_rx_cb)(lower_id, PIOS_SPEKTRUM_RxInCallback, 0); @@ -120,9 +120,10 @@ static int32_t PIOS_SPEKTRUM_Get(uint32_t rcvr_id, uint8_t channel) * \output true Successful bind * \output false Bind failed */ -static bool PIOS_SPEKTRUM_Bind(const struct pios_spektrum_cfg * cfg) +static bool PIOS_SPEKTRUM_Bind(const struct pios_spektrum_cfg * cfg, uint8_t bind) { -#define BIND_PULSES 5 + /* just to limit bind pulses */ + bind=(bind<=10)?bind:10; GPIO_Init(cfg->bind.gpio, &cfg->bind.init); /* RX line, set high */ @@ -131,7 +132,7 @@ static bool PIOS_SPEKTRUM_Bind(const struct pios_spektrum_cfg * cfg) /* on CC works upto 140ms, I guess bind window is around 20-140ms after powerup */ PIOS_DELAY_WaitmS(60); - for (int i = 0; i < BIND_PULSES ; i++) { + for (int i = 0; i < bind ; i++) { /* RX line, drive low for 120us */ GPIO_ResetBits(cfg->bind.gpio, cfg->bind.init.GPIO_Pin); PIOS_DELAY_WaituS(120); diff --git a/flight/PiOS/inc/pios_spektrum_priv.h b/flight/PiOS/inc/pios_spektrum_priv.h index 2d31a8027..ffd224e40 100644 --- a/flight/PiOS/inc/pios_spektrum_priv.h +++ b/flight/PiOS/inc/pios_spektrum_priv.h @@ -42,7 +42,7 @@ struct pios_spektrum_cfg { extern const struct pios_rcvr_driver pios_spektrum_rcvr_driver; -extern int32_t PIOS_SPEKTRUM_Init(uint32_t * spektrum_id, const struct pios_spektrum_cfg *cfg, const struct pios_com_driver * driver, uint32_t lower_id, bool bind); +extern int32_t PIOS_SPEKTRUM_Init(uint32_t * spektrum_id, const struct pios_spektrum_cfg *cfg, const struct pios_com_driver * driver, uint32_t lower_id, uint8_t bind); #endif /* PIOS_PWM_PRIV_H */ diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index a3494f820..d8facecd9 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -1,8 +1,9 @@ Selection of optional hardware configurations. - - + + + From 9f35b6939c86a7df2906afe70ba6d0429180cec7 Mon Sep 17 00:00:00 2001 From: sambas Date: Sun, 7 Aug 2011 10:10:52 +0300 Subject: [PATCH 033/145] Bind IO fix --- flight/PiOS/STM32F10x/pios_spektrum.c | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/flight/PiOS/STM32F10x/pios_spektrum.c b/flight/PiOS/STM32F10x/pios_spektrum.c index 9c81b6131..2d57d9900 100644 --- a/flight/PiOS/STM32F10x/pios_spektrum.c +++ b/flight/PiOS/STM32F10x/pios_spektrum.c @@ -122,6 +122,11 @@ static int32_t PIOS_SPEKTRUM_Get(uint32_t rcvr_id, uint8_t channel) */ static bool PIOS_SPEKTRUM_Bind(const struct pios_spektrum_cfg * cfg, uint8_t bind) { + GPIO_InitTypeDef GPIO_InitStructure; + GPIO_InitStructure.GPIO_Pin = cfg->bind.init.GPIO_Pin; + GPIO_InitStructure.GPIO_Speed = cfg->bind.init.GPIO_Speed; + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU; + /* just to limit bind pulses */ bind=(bind<=10)?bind:10; @@ -141,6 +146,7 @@ static bool PIOS_SPEKTRUM_Bind(const struct pios_spektrum_cfg * cfg, uint8_t bin PIOS_DELAY_WaituS(120); } /* RX line, set input and wait for data, PIOS_SPEKTRUM_Init */ + GPIO_Init(cfg->bind.gpio, &GPIO_InitStructure); return true; } From f11aa8044421cd47e54b7311af9874606dcf30a6 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Tue, 9 Aug 2011 20:43:48 -0500 Subject: [PATCH 034/145] CameraStabilization: Make the CameraStabilization module put the desired position into another object (will help with configuration). --- flight/CopterControl/Makefile | 1 + flight/Modules/CameraStab/camerastab.c | 31 ++++++++----------- .../OpenPilotOSX.xcodeproj/project.pbxproj | 2 ++ shared/uavobjectdefinition/cameradesired.xml | 12 +++++++ .../camerastabsettings.xml | 3 +- 5 files changed, 29 insertions(+), 20 deletions(-) create mode 100644 shared/uavobjectdefinition/cameradesired.xml diff --git a/flight/CopterControl/Makefile b/flight/CopterControl/Makefile index 814173731..fccc5afd0 100644 --- a/flight/CopterControl/Makefile +++ b/flight/CopterControl/Makefile @@ -166,6 +166,7 @@ SRC += $(OPUAVSYNTHDIR)/mixersettings.c SRC += $(OPUAVSYNTHDIR)/firmwareiapobj.c SRC += $(OPUAVSYNTHDIR)/attitudesettings.c SRC += $(OPUAVSYNTHDIR)/camerastabsettings.c +SRC += $(OPUAVSYNTHDIR)/cameradesired.c SRC += $(OPUAVSYNTHDIR)/hwsettings.c SRC += $(OPUAVSYNTHDIR)/taskinfo.c diff --git a/flight/Modules/CameraStab/camerastab.c b/flight/Modules/CameraStab/camerastab.c index 710bc17c7..1f675ef59 100644 --- a/flight/Modules/CameraStab/camerastab.c +++ b/flight/Modules/CameraStab/camerastab.c @@ -50,6 +50,7 @@ #include "accessorydesired.h" #include "attitudeactual.h" #include "camerastabsettings.h" +#include "cameradesired.h" // // Configuration @@ -75,6 +76,7 @@ int32_t CameraStabInitialize(void) ev.event = 0; CameraStabSettingsInitialize(); + CameraDesiredInitialize(); EventPeriodicCallbackCreate(&ev, attitudeUpdated, SAMPLE_PERIOD_MS / portTICK_RATE_MS); @@ -87,6 +89,7 @@ static void attitudeUpdated(UAVObjEvent* ev) return; float attitude; + float output; AccessoryDesiredData accessory; CameraStabSettingsData cameraStab; @@ -108,26 +111,18 @@ static void attitudeUpdated(UAVObjEvent* ev) } // Set output channels - if(cameraStab.Outputs[CAMERASTABSETTINGS_OUTPUTS_ROLL] != CAMERASTABSETTINGS_OUTPUTS_NONE) { - AttitudeActualRollGet(&attitude); - accessory.AccessoryVal = (attitude + inputs[0]) / cameraStab.OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_ROLL]; - if(AccessoryDesiredInstSet(cameraStab.Outputs[CAMERASTABSETTINGS_OUTPUTS_ROLL] - CAMERASTABSETTINGS_OUTPUTS_ACCESSORY0, &accessory) != 0) - AccessoryDesiredCreateInstance(); - } + AttitudeActualRollGet(&attitude); + output = (attitude + inputs[0]) / cameraStab.OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_ROLL]; + CameraDesiredRollSet(&output); - if(cameraStab.Outputs[CAMERASTABSETTINGS_OUTPUTS_PITCH] != CAMERASTABSETTINGS_OUTPUTS_NONE) { - AttitudeActualPitchGet(&attitude); - accessory.AccessoryVal = (attitude + inputs[1]) / cameraStab.OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_PITCH]; - if(AccessoryDesiredInstSet(cameraStab.Outputs[CAMERASTABSETTINGS_OUTPUTS_PITCH] - CAMERASTABSETTINGS_OUTPUTS_ACCESSORY0, &accessory) != 0) - AccessoryDesiredCreateInstance(); - } + AttitudeActualPitchGet(&attitude); + output = (attitude + inputs[1]) / cameraStab.OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_PITCH]; + CameraDesiredPitchSet(&output); + + AttitudeActualYawGet(&attitude); + output = (attitude + inputs[2]) / cameraStab.OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_YAW]; + CameraDesiredYawSet(&output); - if(cameraStab.Outputs[CAMERASTABSETTINGS_OUTPUTS_YAW] != CAMERASTABSETTINGS_OUTPUTS_NONE) { - AttitudeActualYawGet(&attitude); - accessory.AccessoryVal = (attitude + inputs[2]) / cameraStab.OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_YAW]; - if(AccessoryDesiredInstSet(cameraStab.Outputs[CAMERASTABSETTINGS_OUTPUTS_YAW] - CAMERASTABSETTINGS_OUTPUTS_ACCESSORY0, &accessory) != 0) - AccessoryDesiredCreateInstance(); - } } /** diff --git a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj index 894b64da9..147923766 100644 --- a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj +++ b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj @@ -2695,6 +2695,7 @@ 65C9908E13AC5D8D0082BD60 /* uavobjectsinit_linker.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = uavobjectsinit_linker.c; sourceTree = ""; }; 65D2CA841248F9A400B1E7D6 /* mixersettings.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = mixersettings.xml; sourceTree = ""; }; 65D2CA851248F9A400B1E7D6 /* mixerstatus.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = mixerstatus.xml; sourceTree = ""; }; + 65DEA79113F2143B00095B06 /* cameradesired.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = cameradesired.xml; sourceTree = ""; }; 65E410AE12F65AEA00725888 /* attitudesettings.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = attitudesettings.xml; sourceTree = ""; }; 65E6D80713E3A4D0002A557A /* hwsettings.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = hwsettings.xml; sourceTree = ""; }; 65E6DF7112E02E8E00058553 /* Makefile */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.make; path = Makefile; sourceTree = ""; }; @@ -7475,6 +7476,7 @@ 65C35E7712EFB2F3004811C2 /* velocityactual.xml */, 65C35E7812EFB2F3004811C2 /* velocitydesired.xml */, 65C35E7912EFB2F3004811C2 /* watchdogstatus.xml */, + 65DEA79113F2143B00095B06 /* cameradesired.xml */, ); path = uavobjectdefinition; sourceTree = ""; diff --git a/shared/uavobjectdefinition/cameradesired.xml b/shared/uavobjectdefinition/cameradesired.xml new file mode 100644 index 000000000..9492fb4db --- /dev/null +++ b/shared/uavobjectdefinition/cameradesired.xml @@ -0,0 +1,12 @@ + + + Desired camera outputs. Comes from @ref CameraStabilization module. + + + + + + + + + diff --git a/shared/uavobjectdefinition/camerastabsettings.xml b/shared/uavobjectdefinition/camerastabsettings.xml index 8d7b68cd2..09199b77a 100644 --- a/shared/uavobjectdefinition/camerastabsettings.xml +++ b/shared/uavobjectdefinition/camerastabsettings.xml @@ -1,8 +1,7 @@ Settings for the @ref CameraStab mmodule - - + From f23eaa82733ecc10be528d539af3a6c4b4f90fe5 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Tue, 9 Aug 2011 21:24:27 -0500 Subject: [PATCH 035/145] Make the Actuator pull from the CameraDesired fields --- flight/Modules/Actuator/actuator.c | 23 +++++++++++++++++++ .../src/plugins/uavobjects/uavobjects.pro | 6 +++-- shared/uavobjectdefinition/mixersettings.xml | 18 +++++++-------- 3 files changed, 36 insertions(+), 11 deletions(-) diff --git a/flight/Modules/Actuator/actuator.c b/flight/Modules/Actuator/actuator.c index 3c44f44fb..6b353d20f 100644 --- a/flight/Modules/Actuator/actuator.c +++ b/flight/Modules/Actuator/actuator.c @@ -41,6 +41,7 @@ #include "flightstatus.h" #include "mixersettings.h" #include "mixerstatus.h" +#include "cameradesired.h" // Private constants @@ -295,6 +296,28 @@ static void actuatorTask(void* parameters) else status[ct] = -1; } + if( (mixers[ct].type >= MIXERSETTINGS_MIXER1TYPE_CAMERAROLL) && + (mixers[ct].type <= MIXERSETTINGS_MIXER1TYPE_CAMERAYAW)) + { + CameraDesiredData cameraDesired; + if( CameraDesiredGet(&cameraDesired) == 0 ) { + switch(mixers[ct].type) { + case MIXERSETTINGS_MIXER1TYPE_CAMERAROLL: + status[ct] = cameraDesired.Roll; + break; + case MIXERSETTINGS_MIXER1TYPE_CAMERAPITCH: + status[ct] = cameraDesired.Pitch; + break; + case MIXERSETTINGS_MIXER1TYPE_CAMERAYAW: + status[ct] = cameraDesired.Yaw; + break; + default: + break; + } + } + else + status[ct] = -1; + } command.Channel[ct] = scaleChannel(status[ct], ChannelMax[ct], diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index 29ecc9d12..a7d010661 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -70,7 +70,8 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/sonaraltitude.h \ $$UAVOBJECT_SYNTHETICS/flightstatus.h \ $$UAVOBJECT_SYNTHETICS/hwsettings.h \ - $$UAVOBJECT_SYNTHETICS/attitudesettings.h + $$UAVOBJECT_SYNTHETICS/attitudesettings.h \ + $$UAVOBJECT_SYNTHETICS/cameradesired.h SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/ahrsstatus.cpp \ @@ -120,4 +121,5 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/uavobjectsinit.cpp \ $$UAVOBJECT_SYNTHETICS/flightstatus.cpp \ $$UAVOBJECT_SYNTHETICS/hwsettings.cpp \ - $$UAVOBJECT_SYNTHETICS/attitudesettings.cpp + $$UAVOBJECT_SYNTHETICS/attitudesettings.cpp \ + $$UAVOBJECT_SYNTHETICS/cameradesired.cpp diff --git a/shared/uavobjectdefinition/mixersettings.xml b/shared/uavobjectdefinition/mixersettings.xml index 02ff69013..e070814c5 100644 --- a/shared/uavobjectdefinition/mixersettings.xml +++ b/shared/uavobjectdefinition/mixersettings.xml @@ -5,24 +5,24 @@ - + - + - + - + - + - + - + - + - + From 11a98bb9e0da0cb0ea1efe1cb465ccbe606e49b8 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Tue, 9 Aug 2011 22:52:02 -0500 Subject: [PATCH 036/145] CameraStabilization: Create blank configuration panel --- .../src/plugins/config/camerastabilization.ui | 112 ++++++++++++++++++ .../src/plugins/config/config.pro | 13 +- .../configcamerastabilizationwidget.cpp | 68 +++++++++++ .../config/configcamerastabilizationwidget.h | 64 ++++++++++ .../src/plugins/config/configgadget.qrc | 1 + .../src/plugins/config/configgadgetwidget.cpp | 3 + .../src/plugins/config/configgadgetwidget.h | 2 +- .../src/plugins/config/images/camera.png | Bin 0 -> 2606 bytes 8 files changed, 259 insertions(+), 4 deletions(-) create mode 100644 ground/openpilotgcs/src/plugins/config/camerastabilization.ui create mode 100644 ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.cpp create mode 100644 ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.h create mode 100644 ground/openpilotgcs/src/plugins/config/images/camera.png diff --git a/ground/openpilotgcs/src/plugins/config/camerastabilization.ui b/ground/openpilotgcs/src/plugins/config/camerastabilization.ui new file mode 100644 index 000000000..7cbd8b7c1 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/config/camerastabilization.ui @@ -0,0 +1,112 @@ + + + CameraStabilizationWidget + + + + 0 + 0 + 720 + 567 + + + + Form + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + 0 + 0 + + + + + 32 + 32 + + + + + true + + + + + + + + :/core/images/helpicon.svg:/core/images/helpicon.svg + + + + 32 + 32 + + + + Ctrl+S + + + false + + + true + + + + + + + Save settings to the board (RAM only). + +This does not save the calibration settings, this is done using the +specific calibration button on top of the screen. + + + Apply + + + + + + + Send settings to the board, and save to the non-volatile memory. + + + Save + + + false + + + + + + + + + + + + + + + diff --git a/ground/openpilotgcs/src/plugins/config/config.pro b/ground/openpilotgcs/src/plugins/config/config.pro index 79ddea046..c1c4b7e0e 100644 --- a/ground/openpilotgcs/src/plugins/config/config.pro +++ b/ground/openpilotgcs/src/plugins/config/config.pro @@ -37,7 +37,8 @@ HEADERS += configplugin.h \ calibration.h \ defaultattitudewidget.h \ smartsavebutton.h \ - defaulthwsettingswidget.h + defaulthwsettingswidget.h \ + configcamerastabilizationwidget.h SOURCES += configplugin.cpp \ configgadgetconfiguration.cpp \ @@ -65,7 +66,8 @@ SOURCES += configplugin.cpp \ alignment-calibration.cpp \ defaultattitudewidget.cpp \ smartsavebutton.cpp \ - defaulthwsettingswidget.cpp + defaulthwsettingswidget.cpp \ + configcamerastabilizationwidget.cpp FORMS += \ airframe.ui \ @@ -78,6 +80,11 @@ FORMS += \ output.ui \ ccattitude.ui \ defaultattitude.ui \ - defaulthwsettings.ui + defaulthwsettings.ui \ + camerastabilization.ui RESOURCES += configgadget.qrc + + + + diff --git a/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.cpp b/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.cpp new file mode 100644 index 000000000..d932d526c --- /dev/null +++ b/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.cpp @@ -0,0 +1,68 @@ +/** + ****************************************************************************** + * + * @file configahrswidget.h + * @author E. Lafargue & The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup ConfigPlugin Config Plugin + * @{ + * @brief The Configuration Gadget used to update settings in the firmware + *****************************************************************************/ +/* + * 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 "configcamerastabilizationwidget.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +ConfigCameraStabilizationWidget::ConfigCameraStabilizationWidget(QWidget *parent) : ConfigTaskWidget(parent) +{ + m_camerastabilization = new Ui_CameraStabilizationWidget(); + m_camerastabilization->setupUi(this); + + // Connect the help button + connect(m_camerastabilization->camerastabilizationHelp, SIGNAL(clicked()), this, SLOT(openHelp())); +} + +ConfigCameraStabilizationWidget::~ConfigCameraStabilizationWidget() +{ + // Do nothing +} + +void ConfigCameraStabilizationWidget::enableControls(bool enable) +{ + m_camerastabilization->camerastabilizationSettingsSaveSD->setEnabled(enable); +} + +void ConfigCameraStabilizationWidget::openHelp() +{ + QDesktopServices::openUrl( QUrl("http://wiki.openpilot.org/display/Doc/Camera+Configuration", QUrl::StrictMode) ); +} + +/** + @} + @} + */ diff --git a/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.h b/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.h new file mode 100644 index 000000000..198f3139b --- /dev/null +++ b/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.h @@ -0,0 +1,64 @@ +/** + ****************************************************************************** + * + * @file configahrstwidget.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @addtogroup GCSPlugins GCS Plugins + * @{ + * @addtogroup ConfigPlugin Config Plugin + * @{ + * @brief Telemetry configuration panel + *****************************************************************************/ +/* + * 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 CONFIGCAMERASTABILIZATIONWIDGET_H +#define CONFIGCAMERASTABILIZATIONWIDGET_H + +#include "ui_camerastabilization.h" +#include "configtaskwidget.h" +#include "extensionsystem/pluginmanager.h" +#include "uavobjectmanager.h" +#include "uavobject.h" +#include +#include +#include +#include +#include +#include + +#include "camerastabsettings.h" + +class ConfigCameraStabilizationWidget: public ConfigTaskWidget +{ + Q_OBJECT + +public: + ConfigCameraStabilizationWidget(QWidget *parent = 0); + ~ConfigCameraStabilizationWidget(); + +private: + virtual void enableControls(bool enable); + + Ui_CameraStabilizationWidget *m_camerastabilization; + +private slots: + void openHelp(); + +protected: + +}; + +#endif // ConfigCameraStabilization_H diff --git a/ground/openpilotgcs/src/plugins/config/configgadget.qrc b/ground/openpilotgcs/src/plugins/config/configgadget.qrc index a91cd499a..640828eeb 100644 --- a/ground/openpilotgcs/src/plugins/config/configgadget.qrc +++ b/ground/openpilotgcs/src/plugins/config/configgadget.qrc @@ -18,5 +18,6 @@ images/hw_config.svg images/hw_config.png images/gyroscope.png + images/camera.png diff --git a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp index 8430f00cf..1e7ce3238 100644 --- a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp @@ -33,6 +33,7 @@ #include "configinputwidget.h" #include "configoutputwidget.h" #include "configstabilizationwidget.h" +#include "configcamerastabilizationwidget.h" #include "config_pro_hw_widget.h" #include "config_cc_hw_widget.h" #include "defaultattitudewidget.h" @@ -81,6 +82,8 @@ ConfigGadgetWidget::ConfigGadgetWidget(QWidget *parent) : QWidget(parent) qwd = new ConfigStabilizationWidget(this); ftw->insertTab(ConfigGadgetWidget::stabilization, qwd, QIcon(":/configgadget/images/gyroscope.png"), QString("Stabilization")); + qwd = new ConfigCameraStabilizationWidget(this); + ftw->insertTab(ConfigGadgetWidget::camerastabilization, qwd, QIcon(":/configgadget/images/camera.png"), QString("Camera Stab")); // qwd = new ConfigPipXtremeWidget(this); diff --git a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.h b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.h index df2f553a3..6cfa6e20d 100644 --- a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.h @@ -50,7 +50,7 @@ class ConfigGadgetWidget: public QWidget public: ConfigGadgetWidget(QWidget *parent = 0); ~ConfigGadgetWidget(); - enum widgetTabs {hardware=0, aircraft, input, output, ins, stabilization}; + enum widgetTabs {hardware=0, aircraft, input, output, ins, stabilization, camerastabilization}; public slots: void onAutopilotConnect(); diff --git a/ground/openpilotgcs/src/plugins/config/images/camera.png b/ground/openpilotgcs/src/plugins/config/images/camera.png new file mode 100644 index 0000000000000000000000000000000000000000..0d8242e004b6604820bd5196de8623541507dc64 GIT binary patch literal 2606 zcmV+}3eok6P)RK0kw+C;I779JTb<1gQDlk zm*iyk#!cLK;~w@z(Os`hO!QtaR6KgP@a2y%#-M2BMvV!{aeYZhDMmmRDBZTu?o46k zITsuzog!?(mggC_pM=oonYQzt_B&7K_q_auP)Y#+gbB$&*wYIT2z_EMHI!lii(P1o73qmpZR}LoKEL{yC4YPuohBk zD5X#o1rrk!n3$MAHk&mb1Ar)sray)DI^jJ zBoYZ|nl`W33xWW@-;c`5O8EVL*le~EDP>BHowb5ulSm{Gi^VWDHU?eS7yLe&rXe1W zBOZ@~5CWgihhQ*>%F4<|e!4GYHYlY?CX+}elW@D; z2!%rMdcB{dmlrH=07X$SFff2bA~EZ8YHDiG*w~0wt5%_^stUni(D+XXK|CHuEEdD) z=qN@9U~G&MD0{rdIuzM!L&;_chFc=F^4!r?G_dU`(kyroN*A`}WONIz>- zRUNaW0W?iRZ*MP>$>fY}_4V~=X=%ZZ9XpD;&(6+HT)TD+(P(tWHoxDGHEY&D5QL)c zv(#A90NHF7kw^r8{PBmWzPh>^&CSg?c<^A!uGn>5$Gv;^(AL(5!NEb(Hc65o%Q7TM zD#^Y}fvT!wLUncZS-aitWG$u;rhk94*{rGl@ZrN~ZEeMd4I4@tyAwk2<(FULyYIe3 zB9Xw`w{MMYlarH($K!Cj-EcZB@o_@aw5(+epePEuySq*Cyv=6AFTebP6DLl z1W^=?y0Nh_ynp|mp{l~*+5o9k3d6(0rn<9d&!VBBfsvxw*|>2d&Ye4Fs*A;97#|;J zq^K~sHb8%Wzp4EF_uu2tp+k(+{5b~>9Kf+-$4qs7eSK`i6bQElh{xlmSby!>wK#qH zG($y&@zYN~Va=K~MmdwoV03hpp`y9r(g2iFeE9IeRM*ndVu>MHK@esbbzorNlSU>> z374N}EEa>Rsz!OsmMz%6eLF)%^JC}Eou&w7VqyYALqiM|%?_6a`0cmfjK_o!oH})i zp_+y9^UptYtk&@!E zZrwUlIsX`0!<7N17ivsXU2XjZ%FCB8HbRlE+KEQFDf5#w?0G4ep!0EfsxXK2YgZ#KZx9PMh!^6X-*+9JYZE$UX7cX8gQc^tfkD+XUIq>`a zrgAtOW+SACJbCiORL(y})^KG2Ns>&fYo9-VZdws!HL}?(oi-U_L`s zRmIb%PZ_HDH1ZA1+u09rXn_14z>ba%MoK=N2M->Y$~^XNa%TXK$72eW+S}XF+uO@f z(ZYE5?j7#mzi*U9Q8aZ4^8$AU0L=3MbX`YlYb!%F3*-9r>!$Fps;Y{in%Uvj0J*I| zXJ;q6y1E!Dnjf!Uzs93SkBqV?im0jKGao0H2C&&|GiK6UzI>UhQMB2hl;X;jE2g?- z%a(Dwtc_1qn3@oh8#29l^9F5gZ44C^#*G^{Os$n9NeBjm3>D1{w+0}D%*elY^X5&i zqk@MIAL7=nTc)~DD8$w9Z$8`_0GJA!a?c)0DbAlikM8bnhN?bJcXu}~T)1GW3j_jO zhJLwlZ2({@<7aO8q$mo`oH;Wia|la@hlg?I%o$UskK66Ws#U8Pswxcb4L}HiEX$@r zl)=G496fpzFJHc7sBDVYuV3SbAAT?;jub@^vMh7m^iRXR0RX^mw?mfY8OyDko11a> z?p}Dq9789%vfSoTU(2hCr@JU-n}K= ze@90Lu3ft}Bl(ll>4YrHsHmtY>Hdp}C6h%1Q)gf_8pYVy*oR8>`B<;sf!?iNy2Rb_;i z0RJN_1>pDl;q&>7`~MV@h+#fv@pwE41Of;I0+wv@4 z5exP)uohEFY&IL0)3I2i zD9U3Ykx0Do`~5o{4u{NIP?^OeolZZ9L?SIVN-0eylRu`@>2}tl$}A3gt-EPUqhRL1+MA{R~ZI3@D|;x~~74&1O4#dV2m%|Nq7R4=h37JaFD# QrvLx|07*qoM6N<$g4^@hdjJ3c literal 0 HcmV?d00001 From 44cc9245e4c5b37322c225e7bb46cd28e6d6560a Mon Sep 17 00:00:00 2001 From: James Cotton Date: Wed, 10 Aug 2011 03:27:41 -0500 Subject: [PATCH 037/145] CameraStabilization: UI Seems to be working now. --- .../src/plugins/config/camerastabilization.ui | 322 +++++++++++++++++- .../configcamerastabilizationwidget.cpp | 148 +++++++- .../config/configcamerastabilizationwidget.h | 3 + .../src/plugins/config/images/camera.png | Bin 2606 -> 2854 bytes 4 files changed, 466 insertions(+), 7 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/camerastabilization.ui b/ground/openpilotgcs/src/plugins/config/camerastabilization.ui index 7cbd8b7c1..2a1102f3d 100644 --- a/ground/openpilotgcs/src/plugins/config/camerastabilization.ui +++ b/ground/openpilotgcs/src/plugins/config/camerastabilization.ui @@ -13,9 +13,327 @@ Form - + + + + + Enable CameraStabilization module + + + + + + + Channel Ranges (number of degrees full range) + + + + + + + + Pitch + + + + + + + Yaw + + + + + + + Roll + + + + + + + 180 + + + + + + + 180 + + + + + + + 180 + + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + Qt::Horizontal + + + + 212 + 20 + + + + + + + + + + + Channel Mapping (select output channel or none to disable) + + + + + + + + Roll + + + + + + + + None + + + + + Channel 1 + + + + + Channel 2 + + + + + Channel 3 + + + + + Channel 4 + + + + + Channel 5 + + + + + Channel 6 + + + + + Channel 7 + + + + + Channel 8 + + + + + + + + + None + + + + + Channel 1 + + + + + Channel 2 + + + + + Channel 3 + + + + + Channel 4 + + + + + Channel 5 + + + + + Channel 6 + + + + + Channel 7 + + + + + Channel 8 + + + + + + + + Pitch + + + + + + + + None + + + + + Channel 1 + + + + + Channel 2 + + + + + Channel 3 + + + + + Channel 4 + + + + + Channel 5 + + + + + Channel 6 + + + + + Channel 7 + + + + + Channel 8 + + + + + + + + Yaw + + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + Qt::Horizontal + + + + 212 + 20 + + + + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + + QFrame::StyledPanel + + + QFrame::Raised + + + @@ -86,7 +404,7 @@ specific calibration button on top of the screen. - + Send settings to the board, and save to the non-volatile memory. diff --git a/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.cpp b/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.cpp index d932d526c..a258bba8c 100644 --- a/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.cpp @@ -1,8 +1,8 @@ /** ****************************************************************************** * - * @file configahrswidget.h - * @author E. Lafargue & The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @file configcamerastabilizationwidget.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @addtogroup GCSPlugins GCS Plugins * @{ * @addtogroup ConfigPlugin Config Plugin @@ -38,12 +38,24 @@ #include #include +#include "camerastabsettings.h" +#include "hwsettings.h" +#include "mixersettings.h" + ConfigCameraStabilizationWidget::ConfigCameraStabilizationWidget(QWidget *parent) : ConfigTaskWidget(parent) { m_camerastabilization = new Ui_CameraStabilizationWidget(); m_camerastabilization->setupUi(this); - // Connect the help button + // Now connect the widget to the StabilizationSettings object + connect(MixerSettings::GetInstance(getObjectManager()),SIGNAL(objectUpdated(UAVObject*)),this,SLOT(refreshValues())); + connect(CameraStabSettings::GetInstance(getObjectManager()),SIGNAL(objectUpdated(UAVObject*)),this,SLOT(refreshValues())); + // TODO: This will need to support both CC and OP later + connect(HwSettings::GetInstance(getObjectManager()),SIGNAL(objectUpdated(UAVObject*)),this,SLOT(refreshValues())); + + // Connect buttons + connect(m_camerastabilization->camerastabilizationSaveRAM,SIGNAL(clicked()),this,SLOT(applySettings())); + connect(m_camerastabilization->camerastabilizationSaveSD,SIGNAL(clicked()),this,SLOT(saveSettings())); connect(m_camerastabilization->camerastabilizationHelp, SIGNAL(clicked()), this, SLOT(openHelp())); } @@ -52,9 +64,135 @@ ConfigCameraStabilizationWidget::~ConfigCameraStabilizationWidget() // Do nothing } -void ConfigCameraStabilizationWidget::enableControls(bool enable) +/** + * @brief Populate the gui settings into the appropriate + * UAV structures + */ +void ConfigCameraStabilizationWidget::applySettings() { - m_camerastabilization->camerastabilizationSettingsSaveSD->setEnabled(enable); + // Enable or disable the settings + HwSettings * hwSettings = HwSettings::GetInstance(getObjectManager()); + HwSettings::DataFields hwSettingsData = hwSettings->getData(); + hwSettingsData.OptionalModules[HwSettings::OPTIONALMODULES_CAMERASTABILIZATION] = + m_camerastabilization->enableCameraStabilization->isChecked() ? + HwSettings::OPTIONALMODULES_ENABLED : + HwSettings::OPTIONALMODULES_DISABLED; + + // Update the mixer settings + MixerSettings * mixerSettings = MixerSettings::GetInstance(getObjectManager()); + MixerSettings::DataFields mixerSettingsData = mixerSettings->getData(); + const int NUM_MIXERS = 8; + + QComboBox * selectors[3] = { + m_camerastabilization->rollChannel, + m_camerastabilization->pitchChannel, + m_camerastabilization->yawChannel + }; + + // TODO: Need to reformat object so types are an + // array themselves. This gets really awkward + quint8 * mixerTypes[NUM_MIXERS] = { + &mixerSettingsData.Mixer1Type, + &mixerSettingsData.Mixer2Type, + &mixerSettingsData.Mixer3Type, + &mixerSettingsData.Mixer4Type, + &mixerSettingsData.Mixer5Type, + &mixerSettingsData.Mixer6Type, + &mixerSettingsData.Mixer7Type, + &mixerSettingsData.Mixer8Type, + }; + + for (int i = 0; i < 3; i++) + { + // Channel 1 is second entry, so becomes zero + int mixerNum = selectors[i]->currentIndex() - 1; + + if ( *mixerTypes[mixerNum] != MixerSettings::MIXER1TYPE_DISABLED && + (*mixerTypes[mixerNum] != MixerSettings::MIXER1TYPE_CAMERAROLL + i) ) { + // If the mixer channel already to something that isn't what we are + // about to set it to reset to none + selectors[i]->setCurrentIndex(0); + } else { + // Make sure no other channels have this output set + for (int j = 0; j < NUM_MIXERS; j++) + if (*mixerTypes[j] == (MixerSettings::MIXER1TYPE_CAMERAROLL + i)) + *mixerTypes[j] = MixerSettings::MIXER1TYPE_DISABLED; + + // If this channel is assigned to one of the outputs that is not disabled + // set it + if(mixerNum >= 0 && mixerNum < NUM_MIXERS) + *mixerTypes[mixerNum] = MixerSettings::MIXER1TYPE_CAMERAROLL + i; + } + } + + // Update the ranges + CameraStabSettings * cameraStabSettings = CameraStabSettings::GetInstance(getObjectManager()); + CameraStabSettings::DataFields cameraStab = cameraStabSettings->getData(); + cameraStab.OutputRange[CameraStabSettings::OUTPUTRANGE_ROLL] = m_camerastabilization->rollOutputRange->value(); + cameraStab.OutputRange[CameraStabSettings::OUTPUTRANGE_PITCH] = m_camerastabilization->pitchOutputRange->value(); + cameraStab.OutputRange[CameraStabSettings::OUTPUTRANGE_YAW] = m_camerastabilization->yawOutputRange->value(); +} + +/** + * Push settings into UAV objects then save them + */ +void ConfigCameraStabilizationWidget::saveSettings() +{ + applySettings(); + UAVObject * obj = HwSettings::GetInstance(getObjectManager()); + saveObjectToSD(obj); + obj = MixerSettings::GetInstance(getObjectManager()); + saveObjectToSD(obj); + obj = CameraStabSettings::GetInstance(getObjectManager()); + saveObjectToSD(obj); +} + +void ConfigCameraStabilizationWidget::refreshValues() +{ + HwSettings * hwSettings = HwSettings::GetInstance(getObjectManager()); + HwSettings::DataFields hwSettingsData = hwSettings->getData(); + m_camerastabilization->enableCameraStabilization->setChecked( + hwSettingsData.OptionalModules[HwSettings::OPTIONALMODULES_CAMERASTABILIZATION] == + HwSettings::OPTIONALMODULES_ENABLED); + + CameraStabSettings * cameraStabSettings = CameraStabSettings::GetInstance(getObjectManager()); + CameraStabSettings::DataFields cameraStab = cameraStabSettings->getData(); + m_camerastabilization->rollOutputRange->setValue(cameraStab.OutputRange[CameraStabSettings::OUTPUTRANGE_ROLL]); + m_camerastabilization->pitchOutputRange->setValue(cameraStab.OutputRange[CameraStabSettings::OUTPUTRANGE_PITCH]); + m_camerastabilization->yawOutputRange->setValue(cameraStab.OutputRange[CameraStabSettings::OUTPUTRANGE_YAW]); + + MixerSettings * mixerSettings = MixerSettings::GetInstance(getObjectManager()); + MixerSettings::DataFields mixerSettingsData = mixerSettings->getData(); + const int NUM_MIXERS = 8; + QComboBox * selectors[3] = { + m_camerastabilization->rollChannel, + m_camerastabilization->pitchChannel, + m_camerastabilization->yawChannel + }; + + // TODO: Need to reformat object so types are an + // array themselves. This gets really awkward + quint8 * mixerTypes[NUM_MIXERS] = { + &mixerSettingsData.Mixer1Type, + &mixerSettingsData.Mixer2Type, + &mixerSettingsData.Mixer3Type, + &mixerSettingsData.Mixer4Type, + &mixerSettingsData.Mixer5Type, + &mixerSettingsData.Mixer6Type, + &mixerSettingsData.Mixer7Type, + &mixerSettingsData.Mixer8Type, + }; + + for (int i = 0; i < 3; i++) + { + // Default to none if not found. Then search for any mixer channels set to + // this + selectors[i]->setCurrentIndex(0); + for (int j = 0; j < NUM_MIXERS; j++) + if (*mixerTypes[j] == (MixerSettings::MIXER1TYPE_CAMERAROLL + i) && + selectors[i]->currentIndex() != (j + 1)) + selectors[i]->setCurrentIndex(j + 1); + } } void ConfigCameraStabilizationWidget::openHelp() diff --git a/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.h b/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.h index 198f3139b..ca0f039ce 100644 --- a/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.h @@ -56,6 +56,9 @@ private: private slots: void openHelp(); + void applySettings(); + void saveSettings(); + void refreshValues(); protected: diff --git a/ground/openpilotgcs/src/plugins/config/images/camera.png b/ground/openpilotgcs/src/plugins/config/images/camera.png index 0d8242e004b6604820bd5196de8623541507dc64..50ee4790404dbe833a968246a4e72ad4638a115f 100644 GIT binary patch literal 2854 zcmcImi9gh97ypfh>;^-Yu|?vBq%28xAwx1|?E7*PCcD8T%e9R%vc13Kkxe|yzlcl&vQP{c0SKJ-*e7$lFZEvIaq~Q0RZ4IHqy5MdDOq1 znF$$^dLx~9tHfG`&B~-09Z?n^>r+9d9?f|4^R^f{hcjzY18zT3g6Mm$gz_-@bFM$db)xQY* zuy6|k;_DS5G6^i&H<7>x7Y1`q-l%|VAfpFbgpdZ7|94xGANTIq*-b0t4mG1;zgbzL z_b}JtZZYfm3Cz%ODo)MJAoxHu{Q*YK_b;#%GX)1Y|HZw--tK`Sj zC@<|Ertp7IhdlK54#jBklzz6B%j)z;^q;%*OuJy!>6)~~@^cSC3Wbs?#gZna12-^A zgW63lfPG z&o}Gbru(W0Ylw1v!K!X-ZeB_KOU*~cPI5bMP@7y*{Y%<_ltmT1VQTBd3CG!L&$)v` zfD|L%wYpneRmH-e?2$7n^ohVTH;{fgvDY;_Cntwa#? zFh^!Inmb896)8P98>ohuZ1TR)*4DOtv{_eL?&;q#JMSl;r>7@?K**&RzsgTZ;iTmA z^CZ2a_RMdGQCP(XmF^fC4hn_xCo|5>%;0{W(&u+@S-ug6c+u72e7z@6w0M&Ar&MmK ztIzqVf$N!le>1p~ZN9XJa+SzD3`X?3fXCGQ<^W8cqeH!Ka!F!zH00!9k=Taf?Dv1* z<)xz+dsE>hD?>JkBXz*tkN(v`hEOsFw)y0%Z@czs(^uX;t{EfTj@Wk|%#uG?O1}q7 z(g!qdiJTs;7J{I6JHJ}EnZ<_UI1kDm8M!|*%*7O|7QA7J4vD-*qtVm?SJ-DGk9{2; z%klRd9N^GoIYtU|kqj%SmxOT|rp2X)Deq<=)8=^Xb*Xg9`a3%H3GwupzPTZ$fr~;) zyBU>V=)J-4^x3oZ;a1S3Ynz*~6W&|3tjfyDYc!fcDBa2J0i#Xww9HFaR>;DRnzXcZ zW5YD9FO>S&(2~AYQ5Z=W%2oCDFiulcQSo#xiPL!lq)E-s_axZbz@Ug*A?(HG@4GCQ zw>5~RhQyMxGJ_hE3JUJ~Qc`r)N&*hzTAxiC30?b3!spBDkuLqf#US;ay$Pqyw#3qr zN(C)1n7p>ai=M@PV$=8EmAzj@(6Q%uhwd2~CXjB66E2G04tIAhS=DOMbI4` z9Wdzkc7^cp@E|u1&~+N*gtH%D6Zynn46X}B)^+ULj@9j5NFH3)y?2j@!NB7tn}K=; z_&5CAR4Wzv=J;#7U#q=s4$F27oNbPNywfNANFaNqmm3OkaB#4_nLYrUx~lyW)Nnm~ zwApvs^TRc|$gPmI4^koAyt3Ns^POZerAW$Tv9@J)l*2e3CnRnWH85aIYwDTrDmTq# zN0M>4f{B6LAy1vprQu2Nq67O@5F*NqFA5nE(Y|ksM$b6&v%qAvcf@32%mUf&>lw3o z4XMJI8Ww*B(04X(*fc%NjP}Yh3nZU0T^S`o6M2h%z`U~T{#c0-fjFjSN^3uU+(FVy zY>bvzfXet@va7|!rpbx*T3m;$6gT!dX$U>IGrHc0B?x)N+90D?QR-fCjV87W(77;2 z)#S^WcLlRO*A*fBIy+(mTMQX1mupS+-Te_SY1L z7G(KCo0VCf#v4I^)4Jv1{97KVhM}%9lNXH>G2$@g=Vei{P;)e2#l1omg^w2xPU%$t z&2jl!N=m36A6BxsT@4I@yjT4S%BxXRI8;t^oc1ZN-1%HHo0# zfcVebzrk_$^z_(Ht?qVXTmIt_0IvBpYNz2!=h;!s*H?EhHpne54W!%VEWRd?tQ#y7 z`2m^E;)VCmC2>bTs&u?oRwwJ-K37Wse@#vr{UDUSW19<759X)@ZSYC!XWnZUG;jch z>@_vD%FM8W%aN@D3yeV2_&CNv>}YRuVrNK|Dv({7C#;Ii@Bp*DB=(>Mj$$^qj<3aK zY;R4G57sLR*^zq}S{_mRel2_)kJqdlQ&v^wjaavODhmaG_L-C>?}?3#qobp3F#idv z@9zH@$96Cm%cZESTsFox4p%u>&JEx~(B_{@L=lL0-LdC`(~Ya6tE<)g7oePCnt!xY zCa^7~O-)VJHd!%MP@q7b%_`i(Z+FFXYr2KYs?M?YZoH9H3{mrlxxy@ebgbQ8;mwax zlVr!Napw2f;*UBg6t9npa9>|vV@Ie-EgdWFX56vT1DMr|R9#?|+?122FA`tU`*1B~R;#C61ay_r|6{0_f+uyb*JQm-PD zNSyNW^7<*?hx!TJMO{CB+$b5TBq=)*dgird;JSKx=PlxA75ov)yQr2VNY^5EhKkC0>);L zwY;^owDcs32jXt?mSs*X{$;f|rY3k?Sc#QQWm}{3qA1X0E@gZqnQwLva!EvIadDAK zC{fcmp1_DA`nw-CSv2kP89Xo0e0=0l{0Fc2b0egOQjVx`+Qnrjk8BZx`vL1gM}=U) zqFYSb+SvhKh3{=dP1DX^h%-;i@tYVv%Q;dJRJTOO$qt4TybRCt{2 zo!w6p=^n?wPp8ukeZ_47wTjB%uEt$FF~)a;qUXw&B$&*wYI zT2z_EMHI!lii(P1o73qmpZR}LoKEL{yC4YPuohBkD5X#o1rvW06PTEoKsK8-9s__V zil#q@!vRT>KnP*1ytD{{@Qocn4QnB#K-YDoQYj=72_zB;Xqq;!*9(FGzu%9_%1ZeC ze%Nfb5-DX$jh(fEVv|TD5R1hyHZ}%b*BAUgnx-Khk0TzBgAf9r&xc?zh|0>!BC2K2 zXJdfz@p1I`_alFuP8a-|=_Y=vL*qYS7r&h*hgrp{lA1!C=t%PY6Lg9!D$|!|3QJMn^~S=FJ;)c6K5bi_Lhi zbUKah?r!*eK7>LcxLmGM9$v{IUw{2|%IS1k(#`_|0~miA8v5uNTw7a5$Gv;^(AL(5!NEb(Hc65o%Q7TMD#^Y}fvT!wLUncZS-ait zWG$u;rhk94*{rGl@ZrN~ZEeMd4I4@tyAwk2<(FULyYIe3B9Xw`w{MMYlarH($K!Cj z-EcZB@o_@aw5(+epePEuySq*Cyv=6AFTebP6DNO8z;0i>>sdh%uzUAz1Ofp(d-lu- zHz}n^Boe5stXxz*3q{kktR;FkG)+TKPmk%#_xXHiZEeMoBS)C&`u5vzasB#rELpO| zsM9nJkw^rSlatJJ%?IucKq*Bu8ik@LMqMBfz@0mHuyyNJhRUYcv}qG=-@a`!fvT#A zMx%d!+P}a9+#8_3zaObo$|yS=4qUu=5w*3o3{{y~v0?=-UAhEO6pgyEu`#@V|DK_$ z!rlx(A0J)(b0BqZ~jggY#v2NWuQ#t<_S;Lh9 zrWa~VQ(bNS1Ac#Ow{5Uyqlabo~M5KMnFdUIz1 zS)KfWAf8qob7KGirYTjt4xTK8k&zMOaqcnlK-mC`K<*mDvH|8G_rpo0QkI!oJr|m$ znFhGZ2AG5VxIee)v?{~H!=~9ly!CBxZGaaqUNBNpJo1mBY=AlN`~9YJI2>jpq=-Cu z^2AimKStJYWdKQ%Osi|3KYxF2S`lP5ve_)2KYwnN^N)`QxH14>K0{Ph#nY!x8LIg- z@(s+}*$;4Nfczf7j*bpSNF4UhQMB2hl;X;jE2g?-%a(Dwtc_1q zn3@oh8#29l^9F5gZ44C^#*G^{Os$n9NeBjm3>D1{w+0}D%*elY^X5&iqk@MIAL7=n zTc)~DD8$w9Z$8`_0GNLYn{v+{N-55tKacM2ZicEpPIq@VE?l@^stW`HT!wzRaBTo! zD&uEv_@pQb&YU?jBXbB#hKGl7=FAyWr;pq1#;R4T7^*4^?hQZ)fh^0WL6pJ4K^#4L z6fa-CWT#GMO~OK~2*T4u_FWr?F+rmPHG-CMPG+ z*4BngmoAxd7Yc%ay1F{cq>|S(Eo-@4!HS|F5{b-MVpUsPi<2i$V(;F)CEb5VM+dH5 zyEY^Flhf&hEX#kWsHiCE{)>qvlSKnlXJ9lM#n{-`jBO1K4LE)JbkS4Eb#-;&>eZ`w z_3G7(Z61#YvMgI7J!~$PH2^Rb6881=%{pyt!-frLYHGsXy?Ym&R5BWkA{-9m@#Dv) z#L&4^RaIf-%9WP5;LFF-2AC$3$)LZ#|D&zJG_owCv9W&f!?iNy2Rb_;i0RJN_1>pDl;q&>7`~MV@h+#fv@pwE41Of;I0+wvQ>pC)-OhI=JbvPUlMG-ESi`yaGVxjB$uXaUI9=Tkuf3Ox)N^CY8m(#IWqbSN_ zA(2SD@caEc91e%fT2PtABb`n^h(sbSHcBZ?CX-Y@rqk(m)}qQR4w+1*J(*1YNGYXc zx?i0TLh9=3nw?JP-vvQv0AT$LO=S!yrNg?e|C-HaJ9>J0{!Rb?#s3d1LEk)Z-d?8w O0000 Date: Wed, 10 Aug 2011 15:24:35 +0100 Subject: [PATCH 038/145] Finished coding part of new input calibration wizard. Waiting for some artwork to send it for review. --- .../src/plugins/config/configinputwidget.cpp | 291 +++++++++++++++++- .../src/plugins/config/configinputwidget.h | 49 ++- .../src/plugins/config/configtaskwidget.cpp | 1 - .../openpilotgcs/src/plugins/config/input.ui | 60 +++- .../src/plugins/config/inputchannelform.ui | 28 +- 5 files changed, 411 insertions(+), 18 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp index 18841b9df..8f9934ddd 100644 --- a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp @@ -2,7 +2,7 @@ ****************************************************************************** * * @file configservowidget.cpp - * @author E. Lafargue & The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @addtogroup GCSPlugins GCS Plugins * @{ * @addtogroup ConfigPlugin Config Plugin @@ -39,21 +39,19 @@ #include #include -#include "manualcontrolsettings.h" -ConfigInputWidget::ConfigInputWidget(QWidget *parent) : ConfigTaskWidget(parent) +ConfigInputWidget::ConfigInputWidget(QWidget *parent) : ConfigTaskWidget(parent),wizardStep(wizardWelcome),skipflag(false),loop(NULL) { + manualCommandObj = ManualControlCommand::GetInstance(getObjectManager()); + manualSettingsObj = ManualControlSettings::GetInstance(getObjectManager()); + receiverActivityObj=ReceiverActivity::GetInstance(getObjectManager()); m_config = new Ui_InputWidget(); m_config->setupUi(this); setupButtons(m_config->saveRCInputToRAM,m_config->saveRCInputToSD); - ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - UAVObjectManager *objManager = pm->getObject(); - UAVDataObject * obj = dynamic_cast(objManager->getObject(QString("ManualControlSettings"))); - UAVObjectField * field = obj->getField(QString("ChannelGroups")); int index=0; - foreach(QString name,field->getElementNames()) + foreach(QString name,manualSettingsObj->getFields().at(0)->getElementNames()) { inputChannelForm * inp=new inputChannelForm(this,index==0); m_config->advancedPage->layout()->addWidget(inp); @@ -65,7 +63,14 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) : ConfigTaskWidget(parent) addUAVObjectToWidgetRelation("ManualControlSettings","ChannelMax",inp->ui->channelMax,index); ++index; } + QPushButton * goWizard=new QPushButton(tr("Start Wizard"),this); + m_config->advancedPage->layout()->addWidget(goWizard); + connect(goWizard,SIGNAL(clicked()),this,SLOT(goToWizard())); + connect(m_config->wzNext,SIGNAL(clicked()),this,SLOT(wzNext())); + connect(m_config->wzCancel,SIGNAL(clicked()),this,SLOT(wzCancel())); + connect(m_config->wzBack,SIGNAL(clicked()),this,SLOT(wzBack())); + m_config->stackedWidget->setCurrentIndex(0); addUAVObjectToWidgetRelation("ManualControlSettings","FlightModePosition",m_config->fmsModePos1,0); addUAVObjectToWidgetRelation("ManualControlSettings","FlightModePosition",m_config->fmsModePos2,1); addUAVObjectToWidgetRelation("ManualControlSettings","FlightModePosition",m_config->fmsModePos3,2); @@ -82,17 +87,16 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) : ConfigTaskWidget(parent) addUAVObjectToWidgetRelation("ManualControlSettings","Arming",m_config->armControl); addUAVObjectToWidgetRelation("ManualControlSettings","armTimeout",m_config->armTimeout,0,1000); - + enableControls(false); populateWidgets(); refreshWidgetsValues(); // Connect the help button connect(m_config->inputHelp, SIGNAL(clicked()), this, SLOT(openHelp())); - } ConfigInputWidget::~ConfigInputWidget() { - // Do nothing + } void ConfigInputWidget::openHelp() @@ -101,4 +105,269 @@ void ConfigInputWidget::openHelp() QDesktopServices::openUrl( QUrl("http://wiki.openpilot.org/display/Doc/Input+Configuration", QUrl::StrictMode) ); } +void ConfigInputWidget::goToWizard() +{ + setupWizardWidget(wizardWelcome); +} + +void ConfigInputWidget::wzCancel() +{ + m_config->stackedWidget->setCurrentIndex(0); + foreach (QWidget * wd, extraWidgets) + { + if(wd) + delete wd; + } + extraWidgets.clear(); + switch(wizardStep) + { + case wizardWelcome: + break; + case wizardChooseMode: + break; + case wizardIdentifySticks: + disconnect(receiverActivityObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(identifyControls())); + break; + case wizardIdentifyCenter: + break; + case wizardIdentifyLimits: + disconnect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(identifyLimits())); + manualSettingsObj->setData(manualSettingsData); + manualCommandObj->setMetadata(manualCommandObj->getDefaultMetadata()); + break; + case wizardIdentifyInverted: + break; + case wizardFinish: + break; + default: + break; + } + wizardStep=wizardWelcome; +} + +void ConfigInputWidget::wzNext() +{ + setupWizardWidget(wizardStep+1); +} + +void ConfigInputWidget::wzBack() +{ + setupWizardWidget(wizardStep-1); +} + +void ConfigInputWidget::setupWizardWidget(int step) +{ + if(step==wizardWelcome) + { + if(wizardStep==wizardChooseMode) + { + delete extraWidgets.at(0); + delete extraWidgets.at(1); + extraWidgets.clear(); + } + manualSettingsData=manualSettingsObj->getData(); + manualSettingsData.Arming=ManualControlSettings::ARMING_ALWAYSDISARMED; + manualSettingsObj->setData(manualSettingsData); + m_config->wzText->setText(tr("Welcome to the inputs configuration wizard.\n" + "Please follow the instruction on the screen and only move your controls when asked to.\n" + "At any time you can press 'back' to return to the previous screeen or 'Cancel' to cancel the wizard" + "For your safety your arming setting is now 'Always Disarmed' please reenable it after this wizard.")); + m_config->stackedWidget->setCurrentIndex(1); + m_config->wzBack->setEnabled(false); + wizardStep=wizardWelcome; + } + else if(step==wizardChooseMode) + { + if(wizardStep==wizardIdentifySticks) + { + disconnect(receiverActivityObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(identifyControls())); + m_config->wzNext->setEnabled(true); + } + m_config->wzText->setText(tr("Please choose your transmiter type.\n" + "Mode 1 means your throttle stick is on the right\n" + "Mode 2 means your throttle stick is on the left\n")); + m_config->wzBack->setEnabled(true); + QRadioButton * mode1=new QRadioButton(tr("Mode 1"),this); + QRadioButton * mode2=new QRadioButton(tr("Mode 2"),this); + mode2->setChecked(true); + extraWidgets.clear(); + extraWidgets.append(mode1); + extraWidgets.append(mode2); + m_config->checkBoxesLayout->layout()->addWidget(mode1); + m_config->checkBoxesLayout->layout()->addWidget(mode2); + wizardStep=wizardChooseMode; + } + else if(step==wizardIdentifySticks) + { + usedChannels.clear(); + if(wizardStep==wizardChooseMode) + { + QRadioButton * mode=qobject_cast(extraWidgets.at(0)); + if(mode->isChecked()) + transmitterMode=mode1; + else + transmitterMode=mode2; + delete extraWidgets.at(0); + delete extraWidgets.at(1); + extraWidgets.clear(); + } + wizardStep=wizardIdentifySticks; + currentCommand=0; + m_config->wzText->setText(QString(tr("Please move each control once at a time according to the instructions and picture below.\n\n" + "Move the %1 stick")).arg(manualSettingsObj->getField("ChannelGroups")->getElementNames().at(currentCommand))); + manualSettingsData=manualSettingsObj->getData(); + connect(receiverActivityObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(identifyControls())); + m_config->wzNext->setEnabled(false); + } + else if(step==wizardIdentifyCenter) + { + if(wizardStep==wizardIdentifySticks) + disconnect(receiverActivityObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(identifyControls())); + else + { + disconnect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(identifyLimits())); + manualSettingsObj->setData(manualSettingsData); + manualCommandObj->setMetadata(manualCommandObj->getDefaultMetadata()); + } + wizardStep=wizardIdentifyCenter; + m_config->wzText->setText(QString(tr("Please center all control controls and press next when ready"))); + } + else if(step==wizardIdentifyLimits) + { + if(wizardStep==wizardIdentifyCenter) + { + wizardStep=wizardIdentifyLimits; + manualCommandData=manualCommandObj->getData(); + manualSettingsData=manualSettingsObj->getData(); + for(int i=0;isetData(manualSettingsData); + } + wizardStep=wizardIdentifyLimits; + m_config->wzText->setText(QString(tr("Please move all controls to their maximum extends on both directions and press next when ready"))); + UAVObject::Metadata mdata= manualCommandObj->getMetadata(); + mdata.flightTelemetryUpdateMode = UAVObject::UPDATEMODE_PERIODIC; + mdata.flightTelemetryUpdatePeriod = 150; + manualCommandObj->setMetadata(mdata); + manualSettingsData=manualSettingsObj->getData(); + for(int i=0;isetData(manualSettingsData); + manualCommandObj->setMetadata(manualCommandObj->getDefaultMetadata()); + } + extraWidgets.clear(); + foreach(QString name,manualSettingsObj->getFields().at(0)->getElementNames()) + { + QCheckBox * cb=new QCheckBox(name,this); + extraWidgets.append(cb); + m_config->checkBoxesLayout->layout()->addWidget(cb); + } + wizardStep=wizardIdentifyInverted; + m_config->wzText->setText(QString(tr("Please check the picture below and check all controls which show an inverted movement and press next when ready"))); + } + else if(step==wizardFinish) + { + manualSettingsData=manualSettingsObj->getData(); + foreach(QWidget * wd,extraWidgets) + { + QCheckBox * cb=qobject_cast(wd); + if(cb) + { + if(cb->isChecked()) + { + int index=manualSettingsObj->getFields().at(0)->getElementNames().indexOf(cb->text()); + qint16 aux; + aux=manualSettingsData.ChannelMax[index]; + manualSettingsData.ChannelMax[index]=manualSettingsData.ChannelMin[index]; + manualSettingsData.ChannelMin[index]=aux; + } + } + delete cb; + } + wizardStep=wizardFinish; + manualSettingsObj->setData(manualSettingsData); + extraWidgets.clear(); + m_config->wzText->setText(QString(tr("You have completed this wizard, please check below if the picture below mimics your controls movement.\n" + "This new settings aren't saved to the board yet, after pressing next you will go to the initial screen where you can do that."))); + + } + + else if(step==wizardFinish+1) + { + manualSettingsData=manualSettingsObj->getData(); + manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_THROTTLE]= + manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_THROTTLE]+ + ((manualSettingsData.ChannelMax[ManualControlSettings::CHANNELMAX_THROTTLE]- + manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_THROTTLE])*0.02); + manualSettingsObj->setData(manualSettingsData); + m_config->stackedWidget->setCurrentIndex(0); + wizardStep=wizardWelcome; + } + +} + +void ConfigInputWidget::identifyControls() +{ + static int debounce=0; + receiverActivityData=receiverActivityObj->getData(); + if(receiverActivityData.ActiveChannel==255) + return; + else + { + receiverActivityData=receiverActivityObj->getData(); + currentChannel.group=receiverActivityData.ActiveGroup; + currentChannel.number=receiverActivityData.ActiveChannel; + if(currentChannel==lastChannel) + ++debounce; + lastChannel.group= currentChannel.group; + lastChannel.number=currentChannel.number; + if(!usedChannels.contains(lastChannel) && debounce>5) + { + debounce=0; + usedChannels.append(lastChannel); + manualSettingsData=manualSettingsObj->getData(); + manualSettingsData.ChannelGroups[currentCommand]=currentChannel.group; + manualSettingsData.ChannelNumber[currentCommand]=currentChannel.number; + manualSettingsObj->setData(manualSettingsData); + } + else + return; + } + ++currentCommand; + if(currentCommand>ManualControlSettings::CHANNELGROUPS_NUMELEM-1) + { + disconnect(receiverActivityObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(identifyControls())); + m_config->wzNext->setEnabled(true); + } + m_config->wzText->setText(QString(tr("Please move each control once at a time according to the instructions and picture below.\n\n" + "Move the %1 stick")).arg(manualSettingsObj->getFields().at(0)->getElementNames().at(currentCommand))); + if(manualSettingsObj->getField("ChannelGroups")->getElementNames().at(currentCommand).contains("Accessory")) + { + m_config->wzNext->setEnabled(true); + } +} + +void ConfigInputWidget::identifyLimits() +{ + manualCommandData=manualCommandObj->getData(); + for(int i=0;imanualCommandData.Channel[i]) + manualSettingsData.ChannelMin[i]=manualCommandData.Channel[i]; + if(manualSettingsData.ChannelMax[i] #include "inputchannelform.h" #include "ui_inputchannelform.h" +#include +#include "manualcontrolcommand.h" +#include "manualcontrolsettings.h" +#include "receiveractivity.h" class Ui_InputWidget; class ConfigInputWidget: public ConfigTaskWidget { Q_OBJECT - public: ConfigInputWidget(QWidget *parent = 0); ~ConfigInputWidget(); - + enum wizardSteps{wizardWelcome,wizardChooseMode,wizardIdentifySticks,wizardIdentifyCenter,wizardIdentifyLimits,wizardIdentifyInverted,wizardFinish}; + enum txMode{mode1,mode2}; public slots: private: Ui_InputWidget *m_config; + wizardSteps wizardStep; + void setupWizardWidget(int step); + QList extraWidgets; + txMode transmitterMode; + struct channelsStruct + { + bool operator ==(const channelsStruct& rhs) const + { + return((group==rhs.group) &&(number==rhs.number)); + } + int group; + int number; + }lastChannel; + channelsStruct currentChannel; + QList usedChannels; + QEventLoop * loop; + bool skipflag; + int currentCommand; + + ManualControlCommand * manualCommandObj; + ManualControlCommand::DataFields manualCommandData; + ManualControlSettings * manualSettingsObj; + ManualControlSettings::DataFields manualSettingsData; + ReceiverActivity * receiverActivityObj; + ReceiverActivity::DataFields receiverActivityData; + + /* + ManualControlCommand * manualCommandObj = ManualControlCommand::GetInstance(getObjectManager()); + ManualControlCommand::DataFields manualCommandData = manualCommandObj->getData(); + ManualControlSettings * manualSettingsObj = ManualControlSettings::GetInstance(getObjectManager()); + ManualControlSettings::DataFields manualSettingsData; + ReceiverActivity * receiverActivityObj=ReceiverActivity::GetInstance(getObjectManager()); + ReceiverActivity::DataFields receiverActivityData =receiverActivityObj->getData(); +*/ private slots: - + void wzNext(); + void wzBack(); + void wzCancel(); + void goToWizard(); void openHelp(); + void identifyControls(); + void identifyLimits(); }; #endif diff --git a/ground/openpilotgcs/src/plugins/config/configtaskwidget.cpp b/ground/openpilotgcs/src/plugins/config/configtaskwidget.cpp index 90d21e494..8aa6a3f33 100644 --- a/ground/openpilotgcs/src/plugins/config/configtaskwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configtaskwidget.cpp @@ -257,7 +257,6 @@ void ConfigTaskWidget::clearDirty() } void ConfigTaskWidget::setDirty(bool value) { - qDebug()<<"dirty="< + + 1 + - + + + + + + + 10 + 75 + true + + + + TextLabel + + + true + + + + + + + + + + + + + + + + + + + Back + + + + + + + Next + + + + + + + Cancel + + + + + + + diff --git a/ground/openpilotgcs/src/plugins/config/inputchannelform.ui b/ground/openpilotgcs/src/plugins/config/inputchannelform.ui index 0c602a21b..4d7f07d9d 100644 --- a/ground/openpilotgcs/src/plugins/config/inputchannelform.ui +++ b/ground/openpilotgcs/src/plugins/config/inputchannelform.ui @@ -6,14 +6,26 @@ 0 0 - 389 - 57 + 404 + 43 Form + + 1 + + + 1 + + + 2 + + + 0 + @@ -56,6 +68,18 @@ + + + 0 + 0 + + + + + 66 + 0 + + TextLabel From a387532a945466c4099b3fcfbd04e546640f45b9 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Wed, 10 Aug 2011 21:57:17 -0500 Subject: [PATCH 039/145] Fixed some little race conditions that messed with the configuration UI. Added message to indicate power cycle is required after enabling this module. --- .../src/plugins/config/camerastabilization.ui | 17 +++++-- .../configcamerastabilizationwidget.cpp | 50 +++++++++++++++---- .../config/configcamerastabilizationwidget.h | 3 +- 3 files changed, 56 insertions(+), 14 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/camerastabilization.ui b/ground/openpilotgcs/src/plugins/config/camerastabilization.ui index 2a1102f3d..28e7740c1 100644 --- a/ground/openpilotgcs/src/plugins/config/camerastabilization.ui +++ b/ground/openpilotgcs/src/plugins/config/camerastabilization.ui @@ -21,6 +21,20 @@ + + + + After enabling the module, you must power cycle before using and configuring. + + + + + + + Qt::Horizontal + + + @@ -424,7 +438,4 @@ specific calibration button on top of the screen. - - - diff --git a/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.cpp b/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.cpp index a258bba8c..43eb5b752 100644 --- a/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.cpp @@ -47,11 +47,7 @@ ConfigCameraStabilizationWidget::ConfigCameraStabilizationWidget(QWidget *parent m_camerastabilization = new Ui_CameraStabilizationWidget(); m_camerastabilization->setupUi(this); - // Now connect the widget to the StabilizationSettings object - connect(MixerSettings::GetInstance(getObjectManager()),SIGNAL(objectUpdated(UAVObject*)),this,SLOT(refreshValues())); - connect(CameraStabSettings::GetInstance(getObjectManager()),SIGNAL(objectUpdated(UAVObject*)),this,SLOT(refreshValues())); - // TODO: This will need to support both CC and OP later - connect(HwSettings::GetInstance(getObjectManager()),SIGNAL(objectUpdated(UAVObject*)),this,SLOT(refreshValues())); + connectUpdates(); // Connect buttons connect(m_camerastabilization->camerastabilizationSaveRAM,SIGNAL(clicked()),this,SLOT(applySettings())); @@ -64,6 +60,24 @@ ConfigCameraStabilizationWidget::~ConfigCameraStabilizationWidget() // Do nothing } +void ConfigCameraStabilizationWidget::connectUpdates() +{ + // Now connect the widget to the StabilizationSettings object + connect(MixerSettings::GetInstance(getObjectManager()),SIGNAL(objectUpdated(UAVObject*)),this,SLOT(refreshValues())); + connect(CameraStabSettings::GetInstance(getObjectManager()),SIGNAL(objectUpdated(UAVObject*)),this,SLOT(refreshValues())); + // TODO: This will need to support both CC and OP later + connect(HwSettings::GetInstance(getObjectManager()),SIGNAL(objectUpdated(UAVObject*)),this,SLOT(refreshValues())); +} + +void ConfigCameraStabilizationWidget::disconnectUpdates() +{ + // Now connect the widget to the StabilizationSettings object + disconnect(MixerSettings::GetInstance(getObjectManager()),SIGNAL(objectUpdated(UAVObject*)),this,SLOT(refreshValues())); + disconnect(CameraStabSettings::GetInstance(getObjectManager()),SIGNAL(objectUpdated(UAVObject*)),this,SLOT(refreshValues())); + // TODO: This will need to support both CC and OP later + disconnect(HwSettings::GetInstance(getObjectManager()),SIGNAL(objectUpdated(UAVObject*)),this,SLOT(refreshValues())); +} + /** * @brief Populate the gui settings into the appropriate * UAV structures @@ -126,11 +140,21 @@ void ConfigCameraStabilizationWidget::applySettings() } // Update the ranges - CameraStabSettings * cameraStabSettings = CameraStabSettings::GetInstance(getObjectManager()); - CameraStabSettings::DataFields cameraStab = cameraStabSettings->getData(); - cameraStab.OutputRange[CameraStabSettings::OUTPUTRANGE_ROLL] = m_camerastabilization->rollOutputRange->value(); - cameraStab.OutputRange[CameraStabSettings::OUTPUTRANGE_PITCH] = m_camerastabilization->pitchOutputRange->value(); - cameraStab.OutputRange[CameraStabSettings::OUTPUTRANGE_YAW] = m_camerastabilization->yawOutputRange->value(); + CameraStabSettings * cameraStab = CameraStabSettings::GetInstance(getObjectManager()); + CameraStabSettings::DataFields cameraStabData = cameraStab->getData(); + cameraStabData.OutputRange[CameraStabSettings::OUTPUTRANGE_ROLL] = m_camerastabilization->rollOutputRange->value(); + cameraStabData.OutputRange[CameraStabSettings::OUTPUTRANGE_PITCH] = m_camerastabilization->pitchOutputRange->value(); + cameraStabData.OutputRange[CameraStabSettings::OUTPUTRANGE_YAW] = m_camerastabilization->yawOutputRange->value(); + + // Because multiple objects are updated, and all of them trigger the callback + // they must be done together (if update one then load settings from second + // the first update would wipe the UI controls). However to be extra cautious + // I'm also disabling updates during the setting to the UAVObjects + disconnectUpdates(); + hwSettings->setData(hwSettingsData); + mixerSettings->setData(mixerSettingsData); + cameraStab->setData(cameraStabData); + connectUpdates(); } /** @@ -200,6 +224,12 @@ void ConfigCameraStabilizationWidget::openHelp() QDesktopServices::openUrl( QUrl("http://wiki.openpilot.org/display/Doc/Camera+Configuration", QUrl::StrictMode) ); } +void ConfigCameraStabilizationWidget::enableControls(bool enable) +{ + m_camerastabilization->camerastabilizationSaveSD->setEnabled(enable); + m_camerastabilization->camerastabilizationSaveRAM->setEnabled(enable); +} + /** @} @} diff --git a/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.h b/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.h index ca0f039ce..f5440e360 100644 --- a/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.h @@ -61,7 +61,8 @@ private slots: void refreshValues(); protected: - + void connectUpdates(); + void disconnectUpdates(); }; #endif // ConfigCameraStabilization_H From c8313d7b3ec504a49fe75c75dfa96cd22e197c51 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Wed, 10 Aug 2011 22:31:38 -0500 Subject: [PATCH 040/145] Add the camera stabilization files to the Pro build. Not functional though. --- flight/OpenPilot/UAVObjects.inc | 2 ++ 1 file changed, 2 insertions(+) diff --git a/flight/OpenPilot/UAVObjects.inc b/flight/OpenPilot/UAVObjects.inc index 3eb2fa571..67dd71b97 100644 --- a/flight/OpenPilot/UAVObjects.inc +++ b/flight/OpenPilot/UAVObjects.inc @@ -69,6 +69,8 @@ UAVOBJSRCFILENAMES += velocityactual UAVOBJSRCFILENAMES += velocitydesired UAVOBJSRCFILENAMES += watchdogstatus UAVOBJSRCFILENAMES += flightstatus +UAVOBJSRCFILENAMES += cameradesired +UAVOBJSRCFILENAMES += camerastabsettings UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(UAVOBJSYNTHDIR)/$(UAVOBJSRCFILE).c ) UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) ) From 9c7799dfe672f07960d8de258e735d7a992d7c1f Mon Sep 17 00:00:00 2001 From: James Cotton Date: Wed, 10 Aug 2011 22:44:31 -0500 Subject: [PATCH 041/145] UAVObject Init: Get rid of deprecated linker init code --- flight/CopterControl/Makefile | 1 - flight/OpenPilot/Makefile | 1 - flight/PiOS.posix/inc/pios_initcall.h | 1 - flight/PiOS.win32/inc/pios_initcall.h | 1 - .../link_STM32103CB_CC_Rev1_BL_sections.ld | 10 ----- .../link_STM32103CB_CC_Rev1_sections.ld | 10 ----- .../link_STM3210E_INS_BL_sections.ld | 10 ----- .../STM32F10x/link_STM3210E_INS_sections.ld | 10 ----- .../STM32F10x/link_STM3210E_OP_BL_sections.ld | 11 ----- .../STM32F10x/link_STM3210E_OP_sections.ld | 11 ----- flight/PiOS/inc/pios_initcall.h | 1 - .../OpenPilotOSX.xcodeproj/project.pbxproj | 2 - flight/UAVObjects/uavobjectsinit_linker.c | 45 ------------------- flight/UAVObjects/uavobjectsinittemplate.c | 2 + flight/UAVObjects/uavobjecttemplate.c | 2 - 15 files changed, 2 insertions(+), 116 deletions(-) delete mode 100644 flight/UAVObjects/uavobjectsinit_linker.c diff --git a/flight/CopterControl/Makefile b/flight/CopterControl/Makefile index fccc5afd0..2d7663853 100644 --- a/flight/CopterControl/Makefile +++ b/flight/CopterControl/Makefile @@ -130,7 +130,6 @@ SRC += $(OPSYSTEM)/taskmonitor.c SRC += $(OPUAVTALK)/uavtalk.c SRC += $(OPUAVOBJ)/uavobjectmanager.c SRC += $(OPUAVOBJ)/eventdispatcher.c -SRC += $(OPUAVOBJ)/uavobjectsinit_linker.c SRC += $(OPSYSTEM)/pios_usb_hid_desc.c else ## TESTCODE diff --git a/flight/OpenPilot/Makefile b/flight/OpenPilot/Makefile index 85c56a3b3..7d3cdf514 100644 --- a/flight/OpenPilot/Makefile +++ b/flight/OpenPilot/Makefile @@ -135,7 +135,6 @@ SRC += $(OPSYSTEM)/taskmonitor.c SRC += $(OPUAVTALK)/uavtalk.c SRC += $(OPUAVOBJ)/uavobjectmanager.c SRC += $(OPUAVOBJ)/eventdispatcher.c -SRC += $(OPUAVOBJ)/uavobjectsinit_linker.c else ## TESTCODE SRC += $(OPTESTS)/test_common.c diff --git a/flight/PiOS.posix/inc/pios_initcall.h b/flight/PiOS.posix/inc/pios_initcall.h index deb6e0eb6..f88081c33 100644 --- a/flight/PiOS.posix/inc/pios_initcall.h +++ b/flight/PiOS.posix/inc/pios_initcall.h @@ -51,7 +51,6 @@ extern initmodule_t __module_initcall_start[], __module_initcall_end[]; extern void InitModules(); extern void StartModules(); -#define UAVOBJ_INITCALL(fn) #define MODULE_INITCALL(ifn, sfn) #define MODULE_TASKCREATE_ALL { \ diff --git a/flight/PiOS.win32/inc/pios_initcall.h b/flight/PiOS.win32/inc/pios_initcall.h index f370f27ee..edd9ddc08 100644 --- a/flight/PiOS.win32/inc/pios_initcall.h +++ b/flight/PiOS.win32/inc/pios_initcall.h @@ -38,7 +38,6 @@ * and we cannot define a linker script for each of them atm */ -#define UAVOBJ_INITCALL(fn) #define MODULE_INITCALL(ifn, iparam, sfn, sparam, flags) #define MODULE_TASKCREATE_ALL diff --git a/flight/PiOS/STM32F10x/link_STM32103CB_CC_Rev1_BL_sections.ld b/flight/PiOS/STM32F10x/link_STM32103CB_CC_Rev1_BL_sections.ld index f818fff42..777e09aa6 100644 --- a/flight/PiOS/STM32F10x/link_STM32103CB_CC_Rev1_BL_sections.ld +++ b/flight/PiOS/STM32F10x/link_STM32103CB_CC_Rev1_BL_sections.ld @@ -18,16 +18,6 @@ SECTIONS *(.rodata .rodata* .gnu.linkonce.r.*) } > BL_FLASH - /* init sections */ - .initcalluavobj.init : - { - . = ALIGN(4); - __uavobj_initcall_start = .; - KEEP(*(.initcalluavobj.init)) - . = ALIGN(4); - __uavobj_initcall_end = .; - } > BL_FLASH - .ARM.extab : { *(.ARM.extab* .gnu.linkonce.armextab.*) diff --git a/flight/PiOS/STM32F10x/link_STM32103CB_CC_Rev1_sections.ld b/flight/PiOS/STM32F10x/link_STM32103CB_CC_Rev1_sections.ld index d2c4abb17..57530d320 100644 --- a/flight/PiOS/STM32F10x/link_STM32103CB_CC_Rev1_sections.ld +++ b/flight/PiOS/STM32F10x/link_STM32103CB_CC_Rev1_sections.ld @@ -22,16 +22,6 @@ SECTIONS *(.rodata .rodata* .gnu.linkonce.r.*) } > FLASH - /* init sections */ - .initcalluavobj.init : - { - . = ALIGN(4); - __uavobj_initcall_start = .; - KEEP(*(.initcalluavobj.init)) - . = ALIGN(4); - __uavobj_initcall_end = .; - } >FLASH - /* module sections */ .initcallmodule.init : { diff --git a/flight/PiOS/STM32F10x/link_STM3210E_INS_BL_sections.ld b/flight/PiOS/STM32F10x/link_STM3210E_INS_BL_sections.ld index 1b3c11e34..3420c385e 100644 --- a/flight/PiOS/STM32F10x/link_STM3210E_INS_BL_sections.ld +++ b/flight/PiOS/STM32F10x/link_STM3210E_INS_BL_sections.ld @@ -207,16 +207,6 @@ SECTIONS } > BL_FLASH - /* init sections */ - .initcalluavobj.init : - { - . = ALIGN(4); - __uavobj_initcall_start = .; - KEEP(*(.initcalluavobj.init)) - . = ALIGN(4); - __uavobj_initcall_end = .; - } > BL_FLASH - /* the program code is stored in the .text section, which goes to Flash */ .text : { diff --git a/flight/PiOS/STM32F10x/link_STM3210E_INS_sections.ld b/flight/PiOS/STM32F10x/link_STM3210E_INS_sections.ld index ef25c9344..bcf498dd0 100644 --- a/flight/PiOS/STM32F10x/link_STM3210E_INS_sections.ld +++ b/flight/PiOS/STM32F10x/link_STM3210E_INS_sections.ld @@ -206,16 +206,6 @@ SECTIONS . = ALIGN(4); } > FLASH - - /* init sections */ - .initcalluavobj.init : - { - . = ALIGN(4); - __uavobj_initcall_start = .; - KEEP(*(.initcalluavobj.init)) - . = ALIGN(4); - __uavobj_initcall_end = .; - } > FLASH /* the program code is stored in the .text section, which goes to Flash */ .text : diff --git a/flight/PiOS/STM32F10x/link_STM3210E_OP_BL_sections.ld b/flight/PiOS/STM32F10x/link_STM3210E_OP_BL_sections.ld index 7716ccbf2..9cd7c8b4b 100644 --- a/flight/PiOS/STM32F10x/link_STM3210E_OP_BL_sections.ld +++ b/flight/PiOS/STM32F10x/link_STM3210E_OP_BL_sections.ld @@ -179,17 +179,6 @@ SECTIONS . = ALIGN(4); } > BL_FLASH - - /* init sections */ - .initcalluavobj.init : - { - . = ALIGN(4); - __uavobj_initcall_start = .; - KEEP(*(.initcalluavobj.init)) - . = ALIGN(4); - __uavobj_initcall_end = .; - } > BL_FLASH - /* the program code is stored in the .text section, which goes to Flash */ .text : { diff --git a/flight/PiOS/STM32F10x/link_STM3210E_OP_sections.ld b/flight/PiOS/STM32F10x/link_STM3210E_OP_sections.ld index 87a65d7c1..5d504bfdd 100644 --- a/flight/PiOS/STM32F10x/link_STM3210E_OP_sections.ld +++ b/flight/PiOS/STM32F10x/link_STM3210E_OP_sections.ld @@ -181,17 +181,6 @@ SECTIONS *(.flashtext) /* Startup code */ . = ALIGN(4); } >FLASH - - - /* init sections */ - .initcalluavobj.init : - { - . = ALIGN(4); - __uavobj_initcall_start = .; - KEEP(*(.initcalluavobj.init)) - . = ALIGN(4); - __uavobj_initcall_end = .; - } >FLASH /* module sections */ .initcallmodule.init : diff --git a/flight/PiOS/inc/pios_initcall.h b/flight/PiOS/inc/pios_initcall.h index 68b9ef077..e242989c0 100644 --- a/flight/PiOS/inc/pios_initcall.h +++ b/flight/PiOS/inc/pios_initcall.h @@ -67,7 +67,6 @@ extern initmodule_t __module_initcall_start[], __module_initcall_end[]; static initmodule_t __initcall_##fn __attribute__((__used__)) \ __attribute__((__section__(".initcall" level ".init"))) = { .fn_minit = ifn, .fn_tinit = sfn }; -#define UAVOBJ_INITCALL(fn) __define_initcall("uavobj",fn,1) #define MODULE_INITCALL(ifn, sfn) __define_module_initcall("module", ifn, sfn) #define MODULE_INITIALISE_ALL { for (initmodule_t *fn = __module_initcall_start; fn < __module_initcall_end; fn++) \ diff --git a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj index 147923766..ceca7eafb 100644 --- a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj +++ b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj @@ -2692,7 +2692,6 @@ 65C35F6812F0DC2D004811C2 /* attitude.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = attitude.h; sourceTree = ""; }; 65C9903C13A871B90082BD60 /* camerastab.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = camerastab.c; sourceTree = ""; }; 65C9903E13A871B90082BD60 /* camerastab.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = camerastab.h; sourceTree = ""; }; - 65C9908E13AC5D8D0082BD60 /* uavobjectsinit_linker.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = uavobjectsinit_linker.c; sourceTree = ""; }; 65D2CA841248F9A400B1E7D6 /* mixersettings.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = mixersettings.xml; sourceTree = ""; }; 65D2CA851248F9A400B1E7D6 /* mixerstatus.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = mixerstatus.xml; sourceTree = ""; }; 65DEA79113F2143B00095B06 /* cameradesired.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = cameradesired.xml; sourceTree = ""; }; @@ -3521,7 +3520,6 @@ 650D8E6A12DFE17500D05CC9 /* UAVObjects */ = { isa = PBXGroup; children = ( - 65C9908E13AC5D8D0082BD60 /* uavobjectsinit_linker.c */, 65C35E9E12F0A834004811C2 /* uavobjecttemplate.c */, 65C35E9F12F0A834004811C2 /* uavobjectsinittemplate.c */, 65C35EA112F0A834004811C2 /* uavobjectmanager.c */, diff --git a/flight/UAVObjects/uavobjectsinit_linker.c b/flight/UAVObjects/uavobjectsinit_linker.c deleted file mode 100644 index 3d4abd7eb..000000000 --- a/flight/UAVObjects/uavobjectsinit_linker.c +++ /dev/null @@ -1,45 +0,0 @@ -/** - ****************************************************************************** - * - * @file uavobjectsinit.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Initialize all objects. - * Automatically generated by the UAVObjectGenerator. - * - * @note This is an automatically generated file. - * DO NOT modify manually. - * @see The GNU Public License (GPL) Version 3 - * - *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License - * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -#include "openpilot.h" - -/** - * Function used to initialize the first instance of each object. - * This file is automatically updated by the UAVObjectGenerator. - */ -extern initcall_t __uavobj_initcall_start[], __uavobj_initcall_end[]; - -void UAVObjectsInitializeAll() -{ - initcall_t *fn; - int32_t ret; - - for (fn = __uavobj_initcall_start; fn < __uavobj_initcall_end; fn++) - ret = (*fn)(); -} diff --git a/flight/UAVObjects/uavobjectsinittemplate.c b/flight/UAVObjects/uavobjectsinittemplate.c index 75a104d18..97a1525a4 100644 --- a/flight/UAVObjects/uavobjectsinittemplate.c +++ b/flight/UAVObjects/uavobjectsinittemplate.c @@ -36,5 +36,7 @@ $(OBJINC) */ void UAVObjectsInitializeAll() { +return; +// This function is no longer used anyway $(OBJINIT) } diff --git a/flight/UAVObjects/uavobjecttemplate.c b/flight/UAVObjects/uavobjecttemplate.c index c997b46fc..019b282fe 100644 --- a/flight/UAVObjects/uavobjecttemplate.c +++ b/flight/UAVObjects/uavobjecttemplate.c @@ -68,8 +68,6 @@ int32_t $(NAME)Initialize(void) } } -UAVOBJ_INITCALL($(NAME)Initialize); - /** * Initialize object fields and metadata with the default values. * If a default value is not specified the object fields From 0f1fd8f8c10d1d359068cf465fcf31899578002e Mon Sep 17 00:00:00 2001 From: James Cotton Date: Thu, 11 Aug 2011 00:00:04 -0500 Subject: [PATCH 042/145] Update HISTORY.txt and correct a help URL. --- HISTORY.txt | 13 +++++++++++++ .../config/configcamerastabilizationwidget.cpp | 2 +- 2 files changed, 14 insertions(+), 1 deletion(-) diff --git a/HISTORY.txt b/HISTORY.txt index ac278b889..b0e1ce8b0 100644 --- a/HISTORY.txt +++ b/HISTORY.txt @@ -1,5 +1,18 @@ Short summary of changes. For a complete list see the git log. +2011-08-10 +Added Camera Stabilization and a gui to configure this. This is a software +selectable module from the GUI. However, a restart is required to make it +active. The GUI does not currently expose the configuration for using the +transmitter to change the view angle but this is supported by the hardware. + +2011-08-10 +By default a lot of diagnostic objects that were enabled by default are now +disabled in the build. This include TaskInfo (and all the FreeRTOS options +that provide that debugging information). Also MixerStatus, I2CStatus, +WatchdogStatus and RateDesired. These can be reenabled for debugging with +-DDIAGNOSTICS. + 2011-08-04 Fixed packaging aesthetic issues. Also avoid runtime issues on OSX Lion by disabling the ModelView and Notify plugins for now (sorry). diff --git a/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.cpp b/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.cpp index 43eb5b752..3d7ffb082 100644 --- a/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.cpp @@ -221,7 +221,7 @@ void ConfigCameraStabilizationWidget::refreshValues() void ConfigCameraStabilizationWidget::openHelp() { - QDesktopServices::openUrl( QUrl("http://wiki.openpilot.org/display/Doc/Camera+Configuration", QUrl::StrictMode) ); + QDesktopServices::openUrl( QUrl("http://wiki.openpilot.org/display/Doc/Camera+Stabilization", QUrl::StrictMode) ); } void ConfigCameraStabilizationWidget::enableControls(bool enable) From e56dbfdb2424612a3d2ed485b4779f4ace91d9e8 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Thu, 11 Aug 2011 11:02:12 -0500 Subject: [PATCH 043/145] CameraStabilization: Bad memory error that Sambas caught --- .../src/plugins/config/configcamerastabilizationwidget.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.cpp b/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.cpp index 3d7ffb082..afbdae760 100644 --- a/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.cpp @@ -121,7 +121,8 @@ void ConfigCameraStabilizationWidget::applySettings() // Channel 1 is second entry, so becomes zero int mixerNum = selectors[i]->currentIndex() - 1; - if ( *mixerTypes[mixerNum] != MixerSettings::MIXER1TYPE_DISABLED && + if ( mixerNum >= 0 && // Short circuit in case of none + *mixerTypes[mixerNum] != MixerSettings::MIXER1TYPE_DISABLED && (*mixerTypes[mixerNum] != MixerSettings::MIXER1TYPE_CAMERAROLL + i) ) { // If the mixer channel already to something that isn't what we are // about to set it to reset to none From 98346186e3253e72bba362e8b31195e7c26bffa0 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Thu, 11 Aug 2011 14:07:24 -0500 Subject: [PATCH 044/145] The camera desired object can now be a single instance. I think it will be a while before we have multiple cameras :) although it's easy to extend when we do. --- shared/uavobjectdefinition/cameradesired.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/shared/uavobjectdefinition/cameradesired.xml b/shared/uavobjectdefinition/cameradesired.xml index 9492fb4db..022728f7e 100644 --- a/shared/uavobjectdefinition/cameradesired.xml +++ b/shared/uavobjectdefinition/cameradesired.xml @@ -1,5 +1,5 @@ - + Desired camera outputs. Comes from @ref CameraStabilization module. From ec4e23aff93fca4e1e2172f7f1f8486fa6a20475 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Thu, 11 Aug 2011 14:34:38 -0500 Subject: [PATCH 045/145] Camera Stabilization: Bound outputs to [-1,1] although this should be redundant with checks in actuator. --- flight/Modules/CameraStab/camerastab.c | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/flight/Modules/CameraStab/camerastab.c b/flight/Modules/CameraStab/camerastab.c index 1f675ef59..2e21b8e92 100644 --- a/flight/Modules/CameraStab/camerastab.c +++ b/flight/Modules/CameraStab/camerastab.c @@ -63,6 +63,7 @@ // Private functions static void attitudeUpdated(UAVObjEvent* ev); +static float bound(float val); /** * Initialise the module, called on startup @@ -112,19 +113,25 @@ static void attitudeUpdated(UAVObjEvent* ev) // Set output channels AttitudeActualRollGet(&attitude); - output = (attitude + inputs[0]) / cameraStab.OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_ROLL]; + output = bound((attitude + inputs[0]) / cameraStab.OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_ROLL]); CameraDesiredRollSet(&output); AttitudeActualPitchGet(&attitude); - output = (attitude + inputs[1]) / cameraStab.OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_PITCH]; + output = bound((attitude + inputs[1]) / cameraStab.OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_PITCH]); CameraDesiredPitchSet(&output); AttitudeActualYawGet(&attitude); - output = (attitude + inputs[2]) / cameraStab.OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_YAW]; + output = bound((attitude + inputs[2]) / cameraStab.OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_YAW]); CameraDesiredYawSet(&output); } +float bound(float val) +{ + return (val > 1) ? 1 : + (val < -1) ? -1 : + val; +} /** * @} */ From ca413415a0f69078da620a33785aa9d891f253a9 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sat, 13 Aug 2011 18:03:49 -0500 Subject: [PATCH 046/145] Code to make openocd 0.5.0 work. I'll keep this branch rebased on next. --- flight/Project/OpenOCD/stm32f1x.cfg | 75 +++++++++++++++++++++++++++++ make/firmware-defs.mk | 50 ++++++++++++++++++- 2 files changed, 123 insertions(+), 2 deletions(-) create mode 100644 flight/Project/OpenOCD/stm32f1x.cfg diff --git a/flight/Project/OpenOCD/stm32f1x.cfg b/flight/Project/OpenOCD/stm32f1x.cfg new file mode 100644 index 000000000..8007ff57a --- /dev/null +++ b/flight/Project/OpenOCD/stm32f1x.cfg @@ -0,0 +1,75 @@ +# script for stm32 + +if { [info exists CHIPNAME] } { + set _CHIPNAME $CHIPNAME +} else { + set _CHIPNAME stm32 +} + +if { [info exists ENDIAN] } { + set _ENDIAN $ENDIAN +} else { + set _ENDIAN little +} + +# Work-area is a space in RAM used for flash programming +# By default use 16kB +if { [info exists WORKAREASIZE] } { + set _WORKAREASIZE $WORKAREASIZE +} else { + set _WORKAREASIZE 0x4000 +} + +# JTAG speed should be <= F_CPU/6. F_CPU after reset is 8MHz, so use F_JTAG = 1MHz +adapter_khz 1000 + +adapter_nsrst_delay 100 +jtag_ntrst_delay 100 + +#jtag scan chain +if { [info exists CPUTAPID ] } { + set _CPUTAPID $CPUTAPID +} else { + # See STM Document RM0008 + # Section 26.6.3 + set _CPUTAPID 0x3ba00477 +} +jtag newtap $_CHIPNAME cpu -irlen 4 -ircapture 0x1 -irmask 0xf -expected-id $_CPUTAPID + +if { [info exists BSTAPID ] } { + # FIXME this never gets used to override defaults... + set _BSTAPID $BSTAPID +} else { + # See STM Document RM0008 + # Section 29.6.2 + # Low density devices, Rev A + set _BSTAPID1 0x06412041 + # Medium density devices, Rev A + set _BSTAPID2 0x06410041 + # Medium density devices, Rev B and Rev Z + set _BSTAPID3 0x16410041 + set _BSTAPID4 0x06420041 + # High density devices, Rev A + set _BSTAPID5 0x06414041 + # Connectivity line devices, Rev A and Rev Z + set _BSTAPID6 0x06418041 + # XL line devices, Rev A + set _BSTAPID7 0x06430041 +} +jtag newtap $_CHIPNAME bs -irlen 5 -expected-id $_BSTAPID1 \ + -expected-id $_BSTAPID2 -expected-id $_BSTAPID3 \ + -expected-id $_BSTAPID4 -expected-id $_BSTAPID5 \ + -expected-id $_BSTAPID6 -expected-id $_BSTAPID7 + +set _TARGETNAME $_CHIPNAME.cpu +target create $_TARGETNAME cortex_m3 -endian $_ENDIAN -chain-position $_TARGETNAME + +$_TARGETNAME configure -work-area-phys 0x20000000 -work-area-size $_WORKAREASIZE -work-area-backup 0 + +# flash size will be probed +set _FLASHNAME $_CHIPNAME.flash +flash bank $_FLASHNAME stm32f1x 0x08000000 0 0 0 $_TARGETNAME + +# if srst is not fitted use SYSRESETREQ to +# perform a soft reset +cortex_m3 reset_config sysresetreq diff --git a/make/firmware-defs.mk b/make/firmware-defs.mk index 901a6c454..0bbbb54b8 100644 --- a/make/firmware-defs.mk +++ b/make/firmware-defs.mk @@ -213,7 +213,7 @@ OOCD_EXE ?= openocd OOCD_JTAG_SETUP = -d0 # interface and board/target settings (using the OOCD target-library here) OOCD_JTAG_SETUP += -s $(TOP)/flight/Project/OpenOCD -OOCD_JTAG_SETUP += -f foss-jtag.revb.cfg -f stm32.cfg +OOCD_JTAG_SETUP += -f foss-jtag.revb.cfg -f stm32f1x.cfg # initialize OOCD_BOARD_RESET = -c init @@ -242,4 +242,50 @@ wipe: -c "flash erase_address pad $(2) $(3)" \ -c "reset run" \ -c "shutdown" -endef \ No newline at end of file +endef + +# $(1) = Name of binary image to write +# $(2) = Base of flash region to write/wipe +# $(3) = Size of flash region to write/wipe +define JTAG_TEMPLATE_F2X +# --------------------------------------------------------------------------- +# Options for OpenOCD flash-programming +# see openocd.pdf/openocd.texi for further information + +# if OpenOCD is in the $PATH just set OPENOCDEXE=openocd +OOCD_EXE ?= openocd + +# debug level +OOCD_JTAG_SETUP = -d0 +# interface and board/target settings (using the OOCD target-library here) +OOCD_JTAG_SETUP += -s $(TOP)/flight/Project/OpenOCD +OOCD_JTAG_SETUP += -f foss-jtag.revb.cfg -f stm32f2x.cfg + +# initialize +OOCD_BOARD_RESET = -c init +# show the targets +#OOCD_BOARD_RESET += -c targets +# commands to prepare flash-write +OOCD_BOARD_RESET += -c "reset halt" + +.PHONY: program +program: $(1) + @echo $(MSG_JTAG_PROGRAM) $$(call toprel, $$<) + $(V1) $(OOCD_EXE) \ + $$(OOCD_JTAG_SETUP) \ + $$(OOCD_BOARD_RESET) \ + -c "flash write_image erase $$< $(2) bin" \ + -c "verify_image $$< $(2) bin" \ + -c "reset run" \ + -c "shutdown" + +.PHONY: wipe +wipe: + @echo $(MSG_JTAG_WIPE) wiping $(3) bytes starting from $(2) + $(V1) $(OOCD_EXE) \ + $$(OOCD_JTAG_SETUP) \ + $$(OOCD_BOARD_RESET) \ + -c "flash erase_address pad $(2) $(3)" \ + -c "reset run" \ + -c "shutdown" +endef From 98f84e472bb6b7c53248ef20e37c9572feb09604 Mon Sep 17 00:00:00 2001 From: zedamota Date: Mon, 15 Aug 2011 14:12:54 +0100 Subject: [PATCH 047/145] Trying to get out of merge hell --- .../src/libs/utils/mytabbedstackwidget.cpp | 18 ++++++++--- .../src/libs/utils/mytabbedstackwidget.h | 3 +- .../plugins/config/configccattitudewidget.cpp | 31 +++---------------- .../plugins/config/configccattitudewidget.h | 2 -- .../src/plugins/config/configgadgetwidget.cpp | 4 +-- .../src/plugins/config/configgadgetwidget.h | 2 +- .../src/plugins/config/configoutputwidget.cpp | 1 - .../config/configstabilizationwidget.cpp | 14 ++++----- .../config/configstabilizationwidget.h | 1 - 9 files changed, 31 insertions(+), 45 deletions(-) diff --git a/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.cpp b/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.cpp index faf2c911e..c412a475c 100644 --- a/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.cpp +++ b/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.cpp @@ -65,7 +65,7 @@ MyTabbedStackWidget::MyTabbedStackWidget(QWidget *parent, bool isVertical, bool m_stackWidget->setContentsMargins(0, 0, 0, 0); setLayout(toplevelLayout); - connect(m_listWidget, SIGNAL(currentRowChanged(int)), this, SLOT(showWidget(int))); + connect(m_listWidget, SIGNAL(currentRowChanged(int)), this, SLOT(showWidget(int)),Qt::QueuedConnection); } void MyTabbedStackWidget::insertTab(const int index, QWidget *tab, const QIcon &icon, const QString &label) @@ -97,9 +97,19 @@ void MyTabbedStackWidget::setCurrentIndex(int index) void MyTabbedStackWidget::showWidget(int index) { - emit currentAboutToShow(index); - m_stackWidget->setCurrentIndex(index); - emit currentChanged(index); + if(m_stackWidget->currentIndex()==index) + return; + bool proceed=false; + emit currentAboutToShow(index,&proceed); + if(proceed) + { + m_stackWidget->setCurrentIndex(index); + emit currentChanged(index); + } + else + { + m_listWidget->setCurrentRow(m_stackWidget->currentIndex(),QItemSelectionModel::ClearAndSelect); + } } void MyTabbedStackWidget::insertCornerWidget(int index, QWidget *widget) diff --git a/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.h b/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.h index 428ed8646..f32524ce5 100644 --- a/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.h +++ b/ground/openpilotgcs/src/libs/utils/mytabbedstackwidget.h @@ -51,9 +51,10 @@ public: void insertCornerWidget(int index, QWidget *widget); int cornerWidgetCount() { return m_cornerWidgetCount; } + QWidget * currentWidget(){return m_stackWidget->currentWidget();} signals: - void currentAboutToShow(int index); + void currentAboutToShow(int index,bool * proceed); void currentChanged(int index); public slots: diff --git a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp index a88bcf2ac..ae9eeeaf0 100644 --- a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp @@ -45,16 +45,16 @@ ConfigCCAttitudeWidget::ConfigCCAttitudeWidget(QWidget *parent) : setupButtons(ui->applyButton,ui->saveButton); addUAVObject("AttitudeSettings"); - refreshWidgetsValues(); // The 1st time this panel is instanciated, the autopilot is already connected. UAVObject * settings = AttitudeSettings::GetInstance(getObjectManager()); // Connect the help button connect(ui->ccAttitudeHelp, SIGNAL(clicked()), this, SLOT(openHelp())); - addWidget(ui->rollBias); - addWidget(ui->pitchBias); - addWidget(ui->yawBias); + addUAVObjectToWidgetRelation("AttitudeSettings","ZeroDuringArming",ui->zeroGyroBiasOnArming); + + addUAVObjectToWidgetRelation("AttitudeSettings","BoardRotation",ui->rollBias,AttitudeSettings::BOARDROTATION_ROLL); + addUAVObjectToWidgetRelation("AttitudeSettings","BoardRotation",ui->pitchBias,AttitudeSettings::BOARDROTATION_PITCH); + addUAVObjectToWidgetRelation("AttitudeSettings","BoardRotation",ui->yawBias,AttitudeSettings::BOARDROTATION_YAW); addWidget(ui->zeroBias); - addWidget(ui->zeroGyroBiasOnArming); } ConfigCCAttitudeWidget::~ConfigCCAttitudeWidget() @@ -123,27 +123,6 @@ void ConfigCCAttitudeWidget::timeout() { } -void ConfigCCAttitudeWidget::updateObjectsFromWidgets() { - AttitudeSettings::DataFields attitudeSettingsData = AttitudeSettings::GetInstance(getObjectManager())->getData(); - attitudeSettingsData.BoardRotation[AttitudeSettings::BOARDROTATION_ROLL] = ui->rollBias->value(); - attitudeSettingsData.BoardRotation[AttitudeSettings::BOARDROTATION_PITCH] = ui->pitchBias->value(); - attitudeSettingsData.BoardRotation[AttitudeSettings::BOARDROTATION_YAW] = ui->yawBias->value(); - attitudeSettingsData.ZeroDuringArming = ui->zeroGyroBiasOnArming->isChecked() ? AttitudeSettings::ZERODURINGARMING_TRUE : - AttitudeSettings::ZERODURINGARMING_FALSE; - AttitudeSettings::GetInstance(getObjectManager())->setData(attitudeSettingsData); -} - -void ConfigCCAttitudeWidget::refreshWidgetsValues() { - bool dirty=isDirty(); - AttitudeSettings::DataFields attitudeSettingsData = AttitudeSettings::GetInstance(getObjectManager())->getData(); - - ui->rollBias->setValue(attitudeSettingsData.BoardRotation[0]); - ui->pitchBias->setValue(attitudeSettingsData.BoardRotation[1]); - ui->yawBias->setValue(attitudeSettingsData.BoardRotation[2]); - ui->zeroGyroBiasOnArming->setChecked(attitudeSettingsData.ZeroDuringArming == AttitudeSettings::ZERODURINGARMING_TRUE); - setDirty(dirty); -} - void ConfigCCAttitudeWidget::startAccelCalibration() { QMutexLocker locker(&startStop); diff --git a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h index 209d7ed28..aa3b6cad7 100644 --- a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h +++ b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h @@ -50,8 +50,6 @@ private slots: void attitudeRawUpdated(UAVObject * obj); void timeout(); void startAccelCalibration(); - void updateObjectsFromWidgets(); - virtual void refreshWidgetsValues(); void openHelp(); private: diff --git a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp index 5662bb3de..0078a1bf2 100644 --- a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp @@ -99,7 +99,7 @@ ConfigGadgetWidget::ConfigGadgetWidget(QWidget *parent) : QWidget(parent) onAutopilotConnect(); help = 0; - connect(ftw->m_tabBar,SIGNAL(aboutToChange(bool*)),this,SLOT(tabAboutToChange(bool*)));//,Qt::BlockingQueuedConnection); + connect(ftw,SIGNAL(currentAboutToShow(int,bool*)),this,SLOT(tabAboutToChange(int,bool*)));//,Qt::BlockingQueuedConnection); } @@ -155,7 +155,7 @@ void ConfigGadgetWidget::onAutopilotConnect() { emit autopilotConnected(); } -void ConfigGadgetWidget::tabAboutToChange(bool * proceed) +void ConfigGadgetWidget::tabAboutToChange(int i,bool * proceed) { *proceed=true; ConfigTaskWidget * wid=qobject_cast(ftw->currentWidget()); diff --git a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.h b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.h index f84694642..356d11a74 100644 --- a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.h @@ -55,7 +55,7 @@ public: public slots: void onAutopilotConnect(); void onAutopilotDisconnect(); - void tabAboutToChange(bool *); + void tabAboutToChange(int i,bool *); signals: void autopilotConnected(); diff --git a/ground/openpilotgcs/src/plugins/config/configoutputwidget.cpp b/ground/openpilotgcs/src/plugins/config/configoutputwidget.cpp index 77138a5a0..0adc41e2c 100644 --- a/ground/openpilotgcs/src/plugins/config/configoutputwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configoutputwidget.cpp @@ -45,7 +45,6 @@ ConfigOutputWidget::ConfigOutputWidget(QWidget *parent) : ConfigTaskWidget(paren m_config->setupUi(this); ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - UAVObjectManager *objManager = pm->getObject(); setupButtons(m_config->saveRCOutputToRAM,m_config->saveRCOutputToSD); addUAVObject("ActuatorSettings"); diff --git a/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.cpp b/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.cpp index 22ad60b0e..cbe554f4e 100644 --- a/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.cpp @@ -100,6 +100,8 @@ ConfigStabilizationWidget::ConfigStabilizationWidget(QWidget *parent) : ConfigTa addWidget(m_stabilization->maximumRoll); addWidget(m_stabilization->maximumPitch); addWidget(m_stabilization->maximumYaw); + addWidget(m_stabilization->lowThrottleZeroIntegral); + } ConfigStabilizationWidget::~ConfigStabilizationWidget() @@ -107,13 +109,6 @@ ConfigStabilizationWidget::~ConfigStabilizationWidget() // Do nothing } - -void ConfigStabilizationWidget::enableControls(bool enable) -{ - //m_stabilization->saveStabilizationToRAM->setEnabled(enable); - m_stabilization->saveStabilizationToSD->setEnabled(enable); -} - void ConfigStabilizationWidget::updateRateRollKP(double val) { if (m_stabilization->linkRateRP->isChecked()) { @@ -251,6 +246,8 @@ void ConfigStabilizationWidget::refreshWidgetsValues() m_stabilization->maximumRoll->setValue(stabData.MaximumRate[StabilizationSettings::MAXIMUMRATE_ROLL]); m_stabilization->maximumPitch->setValue(stabData.MaximumRate[StabilizationSettings::MAXIMUMRATE_PITCH]); m_stabilization->maximumYaw->setValue(stabData.MaximumRate[StabilizationSettings::MAXIMUMRATE_YAW]); + m_stabilization->lowThrottleZeroIntegral->setChecked(stabData.LowThrottleZeroIntegral==StabilizationSettings::LOWTHROTTLEZEROINTEGRAL_TRUE ? true : false); + setDirty(dirty); } @@ -299,6 +296,9 @@ void ConfigStabilizationWidget::updateObjectsFromWidgets() stabData.MaximumRate[StabilizationSettings::MAXIMUMRATE_PITCH] = m_stabilization->maximumPitch->value(); stabData.MaximumRate[StabilizationSettings::MAXIMUMRATE_YAW] = m_stabilization->maximumYaw->value(); + stabData.LowThrottleZeroIntegral = (m_stabilization->lowThrottleZeroIntegral->isChecked() ? StabilizationSettings::LOWTHROTTLEZEROINTEGRAL_TRUE :StabilizationSettings::LOWTHROTTLEZEROINTEGRAL_FALSE); + + stabSettings->setData(stabData); // this is atomic } diff --git a/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.h b/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.h index f17ac593e..e512c5097 100644 --- a/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configstabilizationwidget.h @@ -49,7 +49,6 @@ private: Ui_StabilizationWidget *m_stabilization; StabilizationSettings* stabSettings; QTimer updateTimer; - virtual void enableControls(bool enable); private slots: virtual void refreshWidgetsValues(); From 35eef66bfeb686a463abf40688c5c46889b0ba09 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Mon, 15 Aug 2011 10:24:37 -0500 Subject: [PATCH 048/145] OP-557: Add a UAVO access method to erase the entire flash chip. Normally not needed by users because if too much changes I change the FS magic and trigger a wipe. Possibly the erase should require a particular "magic" object id value to execute? This would make it harder to do manually through UAVOs though. --- flight/Modules/System/systemmod.c | 5 +++++ flight/PiOS/Common/pios_flashfs_objlist.c | 13 +++++++++++++ flight/PiOS/inc/pios_flashfs_objlist.h | 3 ++- shared/uavobjectdefinition/objectpersistence.xml | 2 +- 4 files changed, 21 insertions(+), 2 deletions(-) diff --git a/flight/Modules/System/systemmod.c b/flight/Modules/System/systemmod.c index f555d227f..e985da78d 100644 --- a/flight/Modules/System/systemmod.c +++ b/flight/Modules/System/systemmod.c @@ -230,6 +230,11 @@ static void objectUpdatedCb(UAVObjEvent * ev) || objper.Selection == OBJECTPERSISTENCE_SELECTION_ALLOBJECTS) { retval = UAVObjDeleteMetaobjects(); } + } else if (objper.Operation == OBJECTPERSISTENCE_OPERATION_FULLERASE) { + retval = -1; +#if defined(PIOS_INCLUDE_FLASH_SECTOR_SETTINGS) + retval = PIOS_FLASHFS_Format(); +#endif } if(retval == 0) { objper.Operation = OBJECTPERSISTENCE_OPERATION_COMPLETED; diff --git a/flight/PiOS/Common/pios_flashfs_objlist.c b/flight/PiOS/Common/pios_flashfs_objlist.c index 08fe640c9..2b2fe06fa 100644 --- a/flight/PiOS/Common/pios_flashfs_objlist.c +++ b/flight/PiOS/Common/pios_flashfs_objlist.c @@ -116,6 +116,19 @@ int32_t PIOS_FLASHFS_Init() return 0; } +/** + * @brief Erase the whole flash chip and create the file system + * @return 0 if successful, -1 if not + */ +int32_t PIOS_FLASHFS_Format() +{ + if(PIOS_Flash_W25X_EraseChip() != 0) + return -1; + if(PIOS_FLASHFS_ClearObjectTableHeader() != 0) + return -1; + return 0; +} + /** * @brief Erase the headers for all objects in the flash chip * @return 0 if successful, -1 if not diff --git a/flight/PiOS/inc/pios_flashfs_objlist.h b/flight/PiOS/inc/pios_flashfs_objlist.h index 945df4068..f5c22d973 100644 --- a/flight/PiOS/inc/pios_flashfs_objlist.h +++ b/flight/PiOS/inc/pios_flashfs_objlist.h @@ -32,6 +32,7 @@ #include "uavobjectmanager.h" int32_t PIOS_FLASHFS_Init(); +int32_t PIOS_FLASHFS_Format(); int32_t PIOS_FLASHFS_ObjSave(UAVObjHandle obj, uint16_t instId, uint8_t * data); int32_t PIOS_FLASHFS_ObjLoad(UAVObjHandle obj, uint16_t instId, uint8_t * data); -int32_t PIOS_FLASHFS_ObjDelete(UAVObjHandle obj, uint16_t instId); \ No newline at end of file +int32_t PIOS_FLASHFS_ObjDelete(UAVObjHandle obj, uint16_t instId); diff --git a/shared/uavobjectdefinition/objectpersistence.xml b/shared/uavobjectdefinition/objectpersistence.xml index 94076de9a..aa7a58e8c 100644 --- a/shared/uavobjectdefinition/objectpersistence.xml +++ b/shared/uavobjectdefinition/objectpersistence.xml @@ -1,7 +1,7 @@ Someone who knows please enter this - + From 33e61b4405b7f05fd00119fd16d0608bcc1689f1 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Mon, 15 Aug 2011 10:55:36 -0500 Subject: [PATCH 049/145] OP-557: Make the tools erase menu item attempt to perform a full format on the flash chip first, then fall back to erasing settings via the EraseSettings object persistence. The later only removes UAVO instances that the firmware is current aware of, so can create issues when jumping between versions. --- .../src/plugins/config/configplugin.cpp | 76 ++++++++++++------- .../src/plugins/config/configplugin.h | 1 + 2 files changed, 50 insertions(+), 27 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/configplugin.cpp b/ground/openpilotgcs/src/plugins/config/configplugin.cpp index dced1c231..622b3db9b 100644 --- a/ground/openpilotgcs/src/plugins/config/configplugin.cpp +++ b/ground/openpilotgcs/src/plugins/config/configplugin.cpp @@ -30,7 +30,7 @@ #include #include #include - +#include "objectpersistence.h" ConfigPlugin::ConfigPlugin() { @@ -58,7 +58,7 @@ bool ConfigPlugin::initialize(const QStringList& args, QString *errMsg) "ConfigPlugin.EraseAll", QList() << Core::Constants::C_GLOBAL_ID); - cmd->action()->setText("Erase all settings from board..."); + cmd->action()->setText(tr("Erase all settings from board...")); ac->menu()->addSeparator(); ac->appendGroup("Utilities"); @@ -80,6 +80,17 @@ bool ConfigPlugin::initialize(const QStringList& args, QString *errMsg) return true; } +/** + * @brief Return handle to object manager + */ +UAVObjectManager * ConfigPlugin::getObjectManager() +{ + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + UAVObjectManager * objMngr = pm->getObject(); + Q_ASSERT(objMngr); + return objMngr; +} + void ConfigPlugin::extensionsInitialized() { cmd->action()->setEnabled(false); @@ -122,18 +133,19 @@ void ConfigPlugin::eraseAllSettings() return; settingsErased = false; - ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - UAVObjectManager * objMngr = pm->getObject(); - Q_ASSERT(objMngr); - ObjectPersistence* objper = dynamic_cast( objMngr->getObject(ObjectPersistence::NAME) ); + ObjectPersistence* objper = ObjectPersistence::GetInstance(getObjectManager()); Q_ASSERT(objper); + connect(objper, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(eraseDone(UAVObject *))); - ObjectPersistence::DataFields data; - data.Operation = ObjectPersistence::OPERATION_DELETE; - data.Selection = ObjectPersistence::SELECTION_ALLSETTINGS; + + ObjectPersistence::DataFields data = objper->getData(); + data.Operation = ObjectPersistence::OPERATION_FULLERASE; + + // No need for manual updated event, this is triggered by setData + // based on UAVO meta data objper->setData(data); objper->updated(); - QTimer::singleShot(1500,this,SLOT(eraseFailed())); + QTimer::singleShot(6000,this,SLOT(eraseFailed())); } @@ -141,37 +153,47 @@ void ConfigPlugin::eraseFailed() { if (settingsErased) return; - ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - UAVObjectManager * objMngr = pm->getObject(); - Q_ASSERT(objMngr); - ObjectPersistence* objper = dynamic_cast( objMngr->getObject(ObjectPersistence::NAME)); - disconnect(objper, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(eraseDone(UAVObject *))); - QMessageBox msgBox; - msgBox.setText(tr("Error trying to erase settings.")); - msgBox.setInformativeText(tr("Power-cycle your board. Settings might be inconsistent.")); - msgBox.setStandardButtons(QMessageBox::Ok); - msgBox.setDefaultButton(QMessageBox::Ok); - msgBox.exec(); + + ObjectPersistence* objper = ObjectPersistence::GetInstance(getObjectManager()); + + ObjectPersistence::DataFields data = objper->getData(); + if(data.Operation == ObjectPersistence::OPERATION_FULLERASE) { + // First attempt via flash erase failed. Fall back on erase all settings + data.Operation = ObjectPersistence::OPERATION_DELETE; + data.Selection = ObjectPersistence::SELECTION_ALLSETTINGS; + objper->setData(data); + objper->updated(); + QTimer::singleShot(1500,this,SLOT(eraseFailed())); + } else { + disconnect(objper, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(eraseDone(UAVObject *))); + QMessageBox msgBox; + msgBox.setText(tr("Error trying to erase settings.")); + msgBox.setInformativeText(tr("Power-cycle your board after removing all blades. Settings might be inconsistent.")); + msgBox.setStandardButtons(QMessageBox::Ok); + msgBox.setDefaultButton(QMessageBox::Ok); + msgBox.exec(); + } } void ConfigPlugin::eraseDone(UAVObject * obj) { QMessageBox msgBox; - ObjectPersistence* objper = dynamic_cast(sender()); - Q_ASSERT(obj->getName().compare("ObjectPersistence") == 0); - QString tmp = obj->getField("Operation")->getValue().toString(); - if (obj->getField("Operation")->getValue().toString().compare(QString("Delete")) == 0 ) { + ObjectPersistence* objper = ObjectPersistence::GetInstance(getObjectManager()); + ObjectPersistence::DataFields data = objper->getData(); + Q_ASSERT(obj->getInstID() == objper->getInstID()); + + if(data.Operation != ObjectPersistence::OPERATION_COMPLETED) { return; } disconnect(objper, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(eraseDone(UAVObject *))); - if (obj->getField("Operation")->getValue().toString().compare(QString("Completed")) == 0) { + if (data.Operation == ObjectPersistence::OPERATION_COMPLETED) { settingsErased = true; msgBox.setText(tr("Settings are now erased.")); msgBox.setInformativeText(tr("Please now power-cycle your board to complete reset.")); } else { msgBox.setText(tr("Error trying to erase settings.")); - msgBox.setInformativeText(tr("Power-cycle your board. Settings might be inconsistent.")); + msgBox.setInformativeText(tr("Power-cycle your board after removing all blades. Settings might be inconsistent.")); } msgBox.setStandardButtons(QMessageBox::Ok); msgBox.setDefaultButton(QMessageBox::Ok); diff --git a/ground/openpilotgcs/src/plugins/config/configplugin.h b/ground/openpilotgcs/src/plugins/config/configplugin.h index f4fa677de..7091ad460 100644 --- a/ground/openpilotgcs/src/plugins/config/configplugin.h +++ b/ground/openpilotgcs/src/plugins/config/configplugin.h @@ -48,6 +48,7 @@ public: ConfigPlugin(); ~ConfigPlugin(); + UAVObjectManager * getObjectManager(); void extensionsInitialized(); bool initialize(const QStringList & arguments, QString * errorString); void shutdown(); From 6a103ed332b752d78aeded4b137ba2cf39e083c7 Mon Sep 17 00:00:00 2001 From: zedamota Date: Tue, 16 Aug 2011 16:06:18 +0100 Subject: [PATCH 050/145] Integrated artwork into the input config wizard. Some testing left to do before sending for review. --- .../src/plugins/config/configgadget.qrc | 1 + .../src/plugins/config/configinputwidget.cpp | 500 ++++++++++++++- .../src/plugins/config/configinputwidget.h | 48 +- .../src/plugins/config/images/TX.svg | 571 ++++++++++++++++++ 4 files changed, 1102 insertions(+), 18 deletions(-) create mode 100644 ground/openpilotgcs/src/plugins/config/images/TX.svg diff --git a/ground/openpilotgcs/src/plugins/config/configgadget.qrc b/ground/openpilotgcs/src/plugins/config/configgadget.qrc index 5f982053b..e157ee0ca 100644 --- a/ground/openpilotgcs/src/plugins/config/configgadget.qrc +++ b/ground/openpilotgcs/src/plugins/config/configgadget.qrc @@ -15,5 +15,6 @@ images/coptercontrol.svg images/hw_config.png images/gyroscope.png + images/TX.svg diff --git a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp index 8f9934ddd..d18976086 100644 --- a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp @@ -38,9 +38,14 @@ #include #include #include +#include +#define ACCESS_MIN_MOVE -6 +#define ACCESS_MAX_MOVE 6 +#define STICK_MIN_MOVE -8 +#define STICK_MAX_MOVE 8 -ConfigInputWidget::ConfigInputWidget(QWidget *parent) : ConfigTaskWidget(parent),wizardStep(wizardWelcome),skipflag(false),loop(NULL) +ConfigInputWidget::ConfigInputWidget(QWidget *parent) : ConfigTaskWidget(parent),wizardStep(wizardWelcome),loop(NULL),skipflag(false) { manualCommandObj = ManualControlCommand::GetInstance(getObjectManager()); manualSettingsObj = ManualControlSettings::GetInstance(getObjectManager()); @@ -88,10 +93,144 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) : ConfigTaskWidget(parent) addUAVObjectToWidgetRelation("ManualControlSettings","Arming",m_config->armControl); addUAVObjectToWidgetRelation("ManualControlSettings","armTimeout",m_config->armTimeout,0,1000); enableControls(false); + populateWidgets(); refreshWidgetsValues(); // Connect the help button connect(m_config->inputHelp, SIGNAL(clicked()), this, SLOT(openHelp())); + + m_config->graphicsView->setScene(new QGraphicsScene(this)); + m_config->graphicsView->setViewportUpdateMode(QGraphicsView::FullViewportUpdate); + m_renderer = new QSvgRenderer(); + QGraphicsScene *l_scene = m_config->graphicsView->scene(); + m_config->graphicsView->setBackgroundBrush(QBrush(Utils::StyleHelper::baseColor())); + if (QFile::exists(":/configgadget/images/TX.svg") && m_renderer->load(QString(":/configgadget/images/TX.svg")) && m_renderer->isValid()) + { + l_scene->clear(); // Deletes all items contained in the scene as well. + + m_txBackground = new QGraphicsSvgItem(); + // All other items will be clipped to the shape of the background + m_txBackground->setFlags(QGraphicsItem::ItemClipsChildrenToShape| + QGraphicsItem::ItemClipsToShape); + m_txBackground->setSharedRenderer(m_renderer); + m_txBackground->setElementId("background"); + l_scene->addItem(m_txBackground); + + m_txMainBody = new QGraphicsSvgItem(); + m_txMainBody->setParentItem(m_txBackground); + m_txMainBody->setSharedRenderer(m_renderer); + m_txMainBody->setElementId("body"); + l_scene->addItem(m_txMainBody); + + m_txLeftStick = new QGraphicsSvgItem(); + m_txLeftStick->setParentItem(m_txBackground); + m_txLeftStick->setSharedRenderer(m_renderer); + m_txLeftStick->setElementId("ljoy"); + + m_txRightStick = new QGraphicsSvgItem(); + m_txRightStick->setParentItem(m_txBackground); + m_txRightStick->setSharedRenderer(m_renderer); + m_txRightStick->setElementId("rjoy"); + + m_txAccess0 = new QGraphicsSvgItem(); + m_txAccess0->setParentItem(m_txBackground); + m_txAccess0->setSharedRenderer(m_renderer); + m_txAccess0->setElementId("access0"); + + m_txAccess1 = new QGraphicsSvgItem(); + m_txAccess1->setParentItem(m_txBackground); + m_txAccess1->setSharedRenderer(m_renderer); + m_txAccess1->setElementId("access1"); + + m_txAccess2 = new QGraphicsSvgItem(); + m_txAccess2->setParentItem(m_txBackground); + m_txAccess2->setSharedRenderer(m_renderer); + m_txAccess2->setElementId("access2"); + + m_txFlightMode = new QGraphicsSvgItem(); + m_txFlightMode->setParentItem(m_txBackground); + m_txFlightMode->setSharedRenderer(m_renderer); + m_txFlightMode->setElementId("flightModeCenter"); + m_txFlightMode->setZValue(-10); + + m_txArrows = new QGraphicsSvgItem(); + m_txArrows->setParentItem(m_txBackground); + m_txArrows->setSharedRenderer(m_renderer); + m_txArrows->setElementId("arrows"); + m_txArrows->setVisible(false); + + QRectF orig=m_renderer->boundsOnElement("ljoy"); + QMatrix Matrix = m_renderer->matrixForElement("ljoy"); + orig=Matrix.mapRect(orig); + m_txLeftStickOrig.translate(orig.x(),orig.y()); + m_txLeftStick->setTransform(m_txLeftStickOrig,false); + + orig=m_renderer->boundsOnElement("arrows"); + Matrix = m_renderer->matrixForElement("arrows"); + orig=Matrix.mapRect(orig); + m_txArrowsOrig.translate(orig.x(),orig.y()); + m_txArrows->setTransform(m_txArrowsOrig,false); + + orig=m_renderer->boundsOnElement("body"); + Matrix = m_renderer->matrixForElement("body"); + orig=Matrix.mapRect(orig); + m_txMainBodyOrig.translate(orig.x(),orig.y()); + m_txMainBody->setTransform(m_txMainBodyOrig,false); + + orig=m_renderer->boundsOnElement("flightModeCenter"); + Matrix = m_renderer->matrixForElement("flightModeCenter"); + orig=Matrix.mapRect(orig); + m_txFlightModeCOrig.translate(orig.x(),orig.y()); + m_txFlightMode->setTransform(m_txFlightModeCOrig,false); + + orig=m_renderer->boundsOnElement("flightModeLeft"); + Matrix = m_renderer->matrixForElement("flightModeLeft"); + orig=Matrix.mapRect(orig); + m_txFlightModeLOrig.translate(orig.x(),orig.y()); + orig=m_renderer->boundsOnElement("flightModeRight"); + Matrix = m_renderer->matrixForElement("flightModeRight"); + orig=Matrix.mapRect(orig); + m_txFlightModeROrig.translate(orig.x(),orig.y()); + + orig=m_renderer->boundsOnElement("rjoy"); + Matrix = m_renderer->matrixForElement("rjoy"); + orig=Matrix.mapRect(orig); + m_txRightStickOrig.translate(orig.x(),orig.y()); + m_txRightStick->setTransform(m_txRightStickOrig,false); + + orig=m_renderer->boundsOnElement("access0"); + Matrix = m_renderer->matrixForElement("access0"); + orig=Matrix.mapRect(orig); + m_txAccess0Orig.translate(orig.x(),orig.y()); + m_txAccess0->setTransform(m_txAccess0Orig,false); + + orig=m_renderer->boundsOnElement("access1"); + Matrix = m_renderer->matrixForElement("access1"); + orig=Matrix.mapRect(orig); + m_txAccess1Orig.translate(orig.x(),orig.y()); + m_txAccess1->setTransform(m_txAccess1Orig,false); + + orig=m_renderer->boundsOnElement("access2"); + Matrix = m_renderer->matrixForElement("access2"); + orig=Matrix.mapRect(orig); + m_txAccess2Orig.translate(orig.x(),orig.y()); + m_txAccess2->setTransform(m_txAccess2Orig,true); + } + m_config->graphicsView->fitInView(m_txMainBody, Qt::KeepAspectRatio ); + animate=new QTimer(this); + connect(animate,SIGNAL(timeout()),this,SLOT(moveTxControls())); +} +void ConfigInputWidget::resetTxControls() +{ + + m_txLeftStick->setTransform(m_txLeftStickOrig,false); + m_txRightStick->setTransform(m_txRightStickOrig,false); + m_txAccess0->setTransform(m_txAccess0Orig,false); + m_txAccess1->setTransform(m_txAccess1Orig,false); + m_txAccess2->setTransform(m_txAccess2Orig,false); + m_txFlightMode->setElementId("flightModeCenter"); + m_txFlightMode->setTransform(m_txFlightModeCOrig,false); + m_txArrows->setVisible(false); } ConfigInputWidget::~ConfigInputWidget() @@ -99,6 +238,12 @@ ConfigInputWidget::~ConfigInputWidget() } +void ConfigInputWidget::resizeEvent(QResizeEvent *event) +{ + QWidget::resizeEvent(event); + m_config->graphicsView->fitInView(m_txBackground, Qt::KeepAspectRatio ); +} + void ConfigInputWidget::openHelp() { @@ -108,10 +253,12 @@ void ConfigInputWidget::openHelp() void ConfigInputWidget::goToWizard() { setupWizardWidget(wizardWelcome); + m_config->graphicsView->fitInView(m_txBackground, Qt::KeepAspectRatio ); } void ConfigInputWidget::wzCancel() { + manualCommandObj->setMetadata(manualCommandObj->getDefaultMetadata()); m_config->stackedWidget->setCurrentIndex(0); foreach (QWidget * wd, extraWidgets) { @@ -133,7 +280,6 @@ void ConfigInputWidget::wzCancel() case wizardIdentifyLimits: disconnect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(identifyLimits())); manualSettingsObj->setData(manualSettingsData); - manualCommandObj->setMetadata(manualCommandObj->getDefaultMetadata()); break; case wizardIdentifyInverted: break; @@ -159,6 +305,7 @@ void ConfigInputWidget::setupWizardWidget(int step) { if(step==wizardWelcome) { + setTxMovement(nothing); if(wizardStep==wizardChooseMode) { delete extraWidgets.at(0); @@ -178,6 +325,7 @@ void ConfigInputWidget::setupWizardWidget(int step) } else if(step==wizardChooseMode) { + setTxMovement(nothing); if(wizardStep==wizardIdentifySticks) { disconnect(receiverActivityObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(identifyControls())); @@ -213,6 +361,7 @@ void ConfigInputWidget::setupWizardWidget(int step) } wizardStep=wizardIdentifySticks; currentCommand=0; + setMoveFromCommand(currentCommand); m_config->wzText->setText(QString(tr("Please move each control once at a time according to the instructions and picture below.\n\n" "Move the %1 stick")).arg(manualSettingsObj->getField("ChannelGroups")->getElementNames().at(currentCommand))); manualSettingsData=manualSettingsObj->getData(); @@ -221,6 +370,7 @@ void ConfigInputWidget::setupWizardWidget(int step) } else if(step==wizardIdentifyCenter) { + setTxMovement(centerAll); if(wizardStep==wizardIdentifySticks) disconnect(receiverActivityObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(identifyControls())); else @@ -234,12 +384,13 @@ void ConfigInputWidget::setupWizardWidget(int step) } else if(step==wizardIdentifyLimits) { + setTxMovement(moveAll); if(wizardStep==wizardIdentifyCenter) { wizardStep=wizardIdentifyLimits; manualCommandData=manualCommandObj->getData(); manualSettingsData=manualSettingsObj->getData(); - for(int i=0;isetData(manualSettingsData); - manualCommandObj->setMetadata(manualCommandObj->getDefaultMetadata()); } extraWidgets.clear(); foreach(QString name,manualSettingsObj->getFields().at(0)->getElementNames()) { - QCheckBox * cb=new QCheckBox(name,this); - extraWidgets.append(cb); - m_config->checkBoxesLayout->layout()->addWidget(cb); + if(!name.contains("Access") && !name.contains("Flight")) + { + QCheckBox * cb=new QCheckBox(name,this); + extraWidgets.append(cb); + m_config->checkBoxesLayout->layout()->addWidget(cb); + } } + connect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); wizardStep=wizardIdentifyInverted; - m_config->wzText->setText(QString(tr("Please check the picture below and check all controls which show an inverted movement and press next when ready"))); + m_config->wzText->setText(QString(tr("Please check the picture below and check all the sticks which show an inverted movement and press next when ready"))); } else if(step==wizardFinish) { @@ -299,18 +454,21 @@ void ConfigInputWidget::setupWizardWidget(int step) wizardStep=wizardFinish; manualSettingsObj->setData(manualSettingsData); extraWidgets.clear(); - m_config->wzText->setText(QString(tr("You have completed this wizard, please check below if the picture below mimics your controls movement.\n" + m_config->wzText->setText(QString(tr("You have completed this wizard, please check below if the picture below mimics your sticks movement.\n" "This new settings aren't saved to the board yet, after pressing next you will go to the initial screen where you can do that."))); } else if(step==wizardFinish+1) { + setTxMovement(nothing); + manualCommandObj->setMetadata(manualCommandObj->getDefaultMetadata()); + disconnect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); manualSettingsData=manualSettingsObj->getData(); manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_THROTTLE]= manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_THROTTLE]+ ((manualSettingsData.ChannelMax[ManualControlSettings::CHANNELMAX_THROTTLE]- - manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_THROTTLE])*0.02); + manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_THROTTLE])*0.02); manualSettingsObj->setData(manualSettingsData); m_config->stackedWidget->setCurrentIndex(0); wizardStep=wizardWelcome; @@ -346,6 +504,7 @@ void ConfigInputWidget::identifyControls() return; } ++currentCommand; + setMoveFromCommand(currentCommand); if(currentCommand>ManualControlSettings::CHANNELGROUPS_NUMELEM-1) { disconnect(receiverActivityObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(identifyControls())); @@ -370,4 +529,325 @@ void ConfigInputWidget::identifyLimits() manualSettingsData.ChannelMax[i]=manualCommandData.Channel[i]; } } +void ConfigInputWidget::setMoveFromCommand(int command) +{ + //CHANNELNUMBER_ROLL=0, CHANNELNUMBER_PITCH=1, CHANNELNUMBER_YAW=2, CHANNELNUMBER_THROTTLE=3, CHANNELNUMBER_FLIGHTMODE=4, CHANNELNUMBER_ACCESSORY0=5, CHANNELNUMBER_ACCESSORY1=6, CHANNELNUMBER_ACCESSORY2=7 } ChannelNumberElem; + if(command==ManualControlSettings::CHANNELNUMBER_ROLL) + { + if(transmitterMode==mode2) + setTxMovement(moveRightHorizontalStick); + else + setTxMovement(moveLeftHorizontalStick); + } + else if(command==ManualControlSettings::CHANNELNUMBER_PITCH) + { + if(transmitterMode==mode2) + setTxMovement(moveRightVerticalStick); + else + setTxMovement(moveLeftVerticalStick); + } + else if(command==ManualControlSettings::CHANNELNUMBER_YAW) + { + if(transmitterMode==mode2) + setTxMovement(moveLeftHorizontalStick); + else + setTxMovement(moveRightHorizontalStick); + } + else if(command==ManualControlSettings::CHANNELNUMBER_THROTTLE) + { + if(transmitterMode==mode2) + setTxMovement(moveLeftVerticalStick); + else + setTxMovement(moveRightVerticalStick); + } + else if(command==ManualControlSettings::CHANNELNUMBER_FLIGHTMODE) + { + setTxMovement(moveFlightMode); + } + else if(command==ManualControlSettings::CHANNELNUMBER_ACCESSORY0) + { + setTxMovement(moveAccess0); + } + else if(command==ManualControlSettings::CHANNELNUMBER_ACCESSORY1) + { + setTxMovement(moveAccess1); + } + else if(command==ManualControlSettings::CHANNELNUMBER_ACCESSORY2) + { + setTxMovement(moveAccess2); + } + +} + +void ConfigInputWidget::setTxMovement(txMovements movement) +{ + resetTxControls(); + switch(movement) + { + case moveLeftVerticalStick: + movePos=0; + growing=true; + currentMovement=moveLeftVerticalStick; + animate->start(100); + break; + case moveRightVerticalStick: + movePos=0; + growing=true; + currentMovement=moveRightVerticalStick; + animate->start(100); + break; + case moveLeftHorizontalStick: + movePos=0; + growing=true; + currentMovement=moveLeftHorizontalStick; + animate->start(100); + break; + case moveRightHorizontalStick: + movePos=0; + growing=true; + currentMovement=moveRightHorizontalStick; + animate->start(100); + break; + case moveAccess0: + movePos=0; + growing=true; + currentMovement=moveAccess0; + animate->start(100); + break; + case moveAccess1: + movePos=0; + growing=true; + currentMovement=moveAccess1; + animate->start(100); + break; + case moveAccess2: + movePos=0; + growing=true; + currentMovement=moveAccess2; + animate->start(100); + break; + case moveFlightMode: + movePos=0; + growing=true; + currentMovement=moveFlightMode; + animate->start(1000); + break; + case centerAll: + movePos=0; + currentMovement=centerAll; + animate->start(1000); + break; + case moveAll: + movePos=0; + growing=true; + currentMovement=moveAll; + animate->start(50); + break; + case nothing: + animate->stop(); + break; + default: + break; + } +} + +void ConfigInputWidget::moveTxControls() +{ + QTransform trans; + QGraphicsItem * item; + txMovementType move; + int limitMax; + int limitMin; + static bool auxFlag=false; + switch(currentMovement) + { + case moveLeftVerticalStick: + item=m_txLeftStick; + trans=m_txLeftStickOrig; + limitMax=STICK_MAX_MOVE; + limitMin=STICK_MIN_MOVE; + move=vertical; + break; + case moveRightVerticalStick: + item=m_txRightStick; + trans=m_txRightStickOrig; + limitMax=STICK_MAX_MOVE; + limitMin=STICK_MIN_MOVE; + move=vertical; + break; + case moveLeftHorizontalStick: + item=m_txLeftStick; + trans=m_txLeftStickOrig; + limitMax=STICK_MAX_MOVE; + limitMin=STICK_MIN_MOVE; + move=horizontal; + break; + case moveRightHorizontalStick: + item=m_txRightStick; + trans=m_txRightStickOrig; + limitMax=STICK_MAX_MOVE; + limitMin=STICK_MIN_MOVE; + move=horizontal; + break; + case moveAccess0: + item=m_txAccess0; + trans=m_txAccess0Orig; + limitMax=ACCESS_MAX_MOVE; + limitMin=ACCESS_MIN_MOVE; + move=horizontal; + break; + case moveAccess1: + item=m_txAccess1; + trans=m_txAccess1Orig; + limitMax=ACCESS_MAX_MOVE; + limitMin=ACCESS_MIN_MOVE; + move=horizontal; + break; + case moveAccess2: + item=m_txAccess2; + trans=m_txAccess2Orig; + limitMax=ACCESS_MAX_MOVE; + limitMin=ACCESS_MIN_MOVE; + move=horizontal; + break; + case moveFlightMode: + item=m_txFlightMode; + move=jump; + break; + case centerAll: + item=m_txArrows; + move=jump; + break; + case moveAll: + limitMax=STICK_MAX_MOVE; + limitMin=STICK_MIN_MOVE; + move=mix; + break; + default: + break; + } + if(move==vertical) + item->setTransform(trans.translate(0,movePos*10),false); + else if(move==horizontal) + item->setTransform(trans.translate(movePos*10,0),false); + else if(move==jump) + { + if(item==m_txArrows) + { + m_txArrows->setVisible(!m_txArrows->isVisible()); + } + else if(item==m_txFlightMode) + { + QGraphicsSvgItem * svg; + svg=(QGraphicsSvgItem *)item; + if (svg) + { + if(svg->elementId()=="flightModeCenter") + { + if(growing) + { + svg->setElementId("flightModeRight"); + m_txFlightMode->setTransform(m_txFlightModeROrig,false); + } + else + { + svg->setElementId("flightModeLeft"); + m_txFlightMode->setTransform(m_txFlightModeLOrig,false); + } + } + else if(svg->elementId()=="flightModeRight") + { + growing=false; + svg->setElementId("flightModeCenter"); + m_txFlightMode->setTransform(m_txFlightModeCOrig,false); + } + else if(svg->elementId()=="flightModeLeft") + { + growing=true; + svg->setElementId("flightModeCenter"); + m_txFlightMode->setTransform(m_txFlightModeCOrig,false); + } + } + } + } + else if(move==mix) + { + trans=m_txAccess0Orig; + m_txAccess0->setTransform(trans.translate(movePos*10*ACCESS_MAX_MOVE/STICK_MAX_MOVE,0),false); + trans=m_txAccess1Orig; + m_txAccess1->setTransform(trans.translate(movePos*10*ACCESS_MAX_MOVE/STICK_MAX_MOVE,0),false); + trans=m_txAccess2Orig; + m_txAccess2->setTransform(trans.translate(movePos*10*ACCESS_MAX_MOVE/STICK_MAX_MOVE,0),false); + + if(auxFlag) + { + trans=m_txLeftStickOrig; + m_txLeftStick->setTransform(trans.translate(0,movePos*10),false); + trans=m_txRightStickOrig; + m_txRightStick->setTransform(trans.translate(0,movePos*10),false); + } + else + { + trans=m_txLeftStickOrig; + m_txLeftStick->setTransform(trans.translate(movePos*10,0),false); + trans=m_txRightStickOrig; + m_txRightStick->setTransform(trans.translate(movePos*10,0),false); + } + + if(movePos==0) + { + m_txFlightMode->setElementId("flightModeCenter"); + m_txFlightMode->setTransform(m_txFlightModeCOrig,false); + } + else if(movePos==ACCESS_MAX_MOVE/2) + { + m_txFlightMode->setElementId("flightModeRight"); + m_txFlightMode->setTransform(m_txFlightModeROrig,false); + } + else if(movePos==ACCESS_MIN_MOVE/2) + { + m_txFlightMode->setElementId("flightModeLeft"); + m_txFlightMode->setTransform(m_txFlightModeLOrig,false); + } + } + if(move==horizontal || move==vertical ||move==mix) + { + if(movePos==0 && growing) + auxFlag=!auxFlag; + if(growing) + ++movePos; + else + --movePos; + if(movePos>limitMax) + { + movePos=movePos-2; + growing=false; + } + if(movePosgetData(); + if(transmitterMode==mode2) + { + trans=m_txLeftStickOrig; + m_txLeftStick->setTransform(trans.translate(manualCommandData.Yaw*STICK_MAX_MOVE*10,-manualCommandData.Throttle*STICK_MAX_MOVE*10),false); + trans=m_txRightStickOrig; + m_txRightStick->setTransform(trans.translate(manualCommandData.Roll*STICK_MAX_MOVE*10,-manualCommandData.Pitch*STICK_MAX_MOVE*10),false); + } + else + { + trans=m_txRightStickOrig; + m_txRightStick->setTransform(trans.translate(manualCommandData.Yaw*STICK_MAX_MOVE*10,-manualCommandData.Throttle*STICK_MAX_MOVE*10),false); + trans=m_txLeftStickOrig; + m_txLeftStick->setTransform(trans.translate(manualCommandData.Roll*STICK_MAX_MOVE*10,-manualCommandData.Pitch*STICK_MAX_MOVE*10),false); + } +} diff --git a/ground/openpilotgcs/src/plugins/config/configinputwidget.h b/ground/openpilotgcs/src/plugins/config/configinputwidget.h index d2c4c2976..b1769026b 100644 --- a/ground/openpilotgcs/src/plugins/config/configinputwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configinputwidget.h @@ -40,6 +40,10 @@ #include "manualcontrolcommand.h" #include "manualcontrolsettings.h" #include "receiveractivity.h" +#include +#include +#include + class Ui_InputWidget; class ConfigInputWidget: public ConfigTaskWidget @@ -50,9 +54,15 @@ public: ~ConfigInputWidget(); enum wizardSteps{wizardWelcome,wizardChooseMode,wizardIdentifySticks,wizardIdentifyCenter,wizardIdentifyLimits,wizardIdentifyInverted,wizardFinish}; enum txMode{mode1,mode2}; + enum txMovements{moveLeftVerticalStick,moveRightVerticalStick,moveLeftHorizontalStick,moveRightHorizontalStick,moveAccess0,moveAccess1,moveAccess2,moveFlightMode,centerAll,moveAll,nothing}; + enum txMovementType{vertical,horizontal,jump,mix}; public slots: private: + bool growing; + txMovements currentMovement; + int movePos; + void setTxMovement(txMovements movement); Ui_InputWidget *m_config; wizardSteps wizardStep; void setupWizardWidget(int step); @@ -81,14 +91,31 @@ private: ReceiverActivity * receiverActivityObj; ReceiverActivity::DataFields receiverActivityData; - /* - ManualControlCommand * manualCommandObj = ManualControlCommand::GetInstance(getObjectManager()); - ManualControlCommand::DataFields manualCommandData = manualCommandObj->getData(); - ManualControlSettings * manualSettingsObj = ManualControlSettings::GetInstance(getObjectManager()); - ManualControlSettings::DataFields manualSettingsData; - ReceiverActivity * receiverActivityObj=ReceiverActivity::GetInstance(getObjectManager()); - ReceiverActivity::DataFields receiverActivityData =receiverActivityObj->getData(); -*/ + QSvgRenderer *m_renderer; + + // Background: background + QGraphicsSvgItem *m_txMainBody; + QGraphicsSvgItem *m_txLeftStick; + QGraphicsSvgItem *m_txRightStick; + QGraphicsSvgItem *m_txAccess0; + QGraphicsSvgItem *m_txAccess1; + QGraphicsSvgItem *m_txAccess2; + QGraphicsSvgItem *m_txFlightMode; + QGraphicsSvgItem *m_txBackground; + QGraphicsSvgItem *m_txArrows; + QTransform m_txLeftStickOrig; + QTransform m_txRightStickOrig; + QTransform m_txAccess0Orig; + QTransform m_txAccess1Orig; + QTransform m_txAccess2Orig; + QTransform m_txFlightModeCOrig; + QTransform m_txFlightModeLOrig; + QTransform m_txFlightModeROrig; + QTransform m_txMainBodyOrig; + QTransform m_txArrowsOrig; + QTimer * animate; + void resetTxControls(); + void setMoveFromCommand(int command); private slots: void wzNext(); void wzBack(); @@ -97,6 +124,11 @@ private slots: void openHelp(); void identifyControls(); void identifyLimits(); + void moveTxControls(); + void moveSticks(); +protected: + void resizeEvent(QResizeEvent *event); + }; #endif diff --git a/ground/openpilotgcs/src/plugins/config/images/TX.svg b/ground/openpilotgcs/src/plugins/config/images/TX.svg new file mode 100644 index 000000000..ebfee4463 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/config/images/TX.svg @@ -0,0 +1,571 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + image/svg+xml + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Flight Mode + + Accessory2 + + + + Accessory1 + + + Accessory0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 2020e336d60448e164813e5557a400f299760a1b Mon Sep 17 00:00:00 2001 From: Oleg Semyonov Date: Tue, 16 Aug 2011 20:58:39 +0300 Subject: [PATCH 051/145] OP-290: allow GCS tabs to shrink (by eliding the text), less limit min window width --- ground/openpilotgcs/src/plugins/coreplugin/mainwindow.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ground/openpilotgcs/src/plugins/coreplugin/mainwindow.cpp b/ground/openpilotgcs/src/plugins/coreplugin/mainwindow.cpp index a2b8137f4..1849b4e8b 100644 --- a/ground/openpilotgcs/src/plugins/coreplugin/mainwindow.cpp +++ b/ground/openpilotgcs/src/plugins/coreplugin/mainwindow.cpp @@ -179,6 +179,8 @@ MainWindow::MainWindow() : m_modeStack->setIconSize(QSize(24,24)); m_modeStack->setTabPosition(QTabWidget::South); m_modeStack->setMovable(false); + m_modeStack->setMinimumWidth(512); + m_modeStack->setElideMode(Qt::ElideRight); #ifndef Q_WS_MAC m_modeStack->setDocumentMode(true); #endif From 0a0d405fc1fe52d6c2fb2ea52bba61f4d30d771a Mon Sep 17 00:00:00 2001 From: zedamota Date: Wed, 17 Aug 2011 12:43:08 +0100 Subject: [PATCH 052/145] Finished the new input configuration wizard. Fixed some bugs along the way. Sending it for review now. --- .../plugins/config/configccattitudewidget.cpp | 7 +++ .../plugins/config/configccattitudewidget.h | 2 + .../src/plugins/config/configgadgetwidget.cpp | 9 ++++ .../src/plugins/config/configinputwidget.cpp | 46 +++++++++++++++++-- .../src/plugins/config/configinputwidget.h | 5 +- .../src/plugins/config/configtaskwidget.cpp | 1 + .../openpilotgcs/src/plugins/config/input.ui | 12 +++++ .../src/plugins/config/smartsavebutton.cpp | 3 +- 8 files changed, 78 insertions(+), 7 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp index ae9eeeaf0..c2242f1ef 100644 --- a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp @@ -163,3 +163,10 @@ void ConfigCCAttitudeWidget::openHelp() QDesktopServices::openUrl( QUrl("http://wiki.openpilot.org/display/Doc/CopterControl+Attitude+Configuration", QUrl::StrictMode) ); } +void ConfigCCAttitudeWidget::enableControls(bool enable) +{ + if(ui->zeroBias) + ui->zeroBias->setEnabled(enable); + ConfigTaskWidget::enableControls(enable); + +} diff --git a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h index aa3b6cad7..09dfdb222 100644 --- a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h +++ b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.h @@ -66,6 +66,8 @@ private: static const int NUM_ACCEL_UPDATES = 60; static const float ACCEL_SCALE = 0.004f * 9.81f; +protected: + virtual void enableControls(bool enable); }; diff --git a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp index 0078a1bf2..af8368c1f 100644 --- a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp @@ -117,6 +117,15 @@ void ConfigGadgetWidget::resizeEvent(QResizeEvent *event) } void ConfigGadgetWidget::onAutopilotDisconnect() { + ftw->setCurrentIndex(ConfigGadgetWidget::hardware); + ftw->removeTab(ConfigGadgetWidget::ins); + QWidget *qwd = new DefaultAttitudeWidget(this); + ftw->insertTab(ConfigGadgetWidget::ins, qwd, QIcon(":/configgadget/images/AHRS-v1.3.png"), QString("INS")); + ftw->removeTab(ConfigGadgetWidget::hardware); + qwd = new DefaultHwSettingsWidget(this); + ftw->insertTab(ConfigGadgetWidget::hardware, qwd, QIcon(":/configgadget/images/hw_config.png"), QString("HW Settings")); + ftw->setCurrentIndex(ConfigGadgetWidget::hardware); + emit autopilotDisconnected(); } diff --git a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp index d18976086..3fc4e8378 100644 --- a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp @@ -45,7 +45,7 @@ #define STICK_MIN_MOVE -8 #define STICK_MAX_MOVE 8 -ConfigInputWidget::ConfigInputWidget(QWidget *parent) : ConfigTaskWidget(parent),wizardStep(wizardWelcome),loop(NULL),skipflag(false) +ConfigInputWidget::ConfigInputWidget(QWidget *parent) : ConfigTaskWidget(parent),wizardStep(wizardWelcome),loop(NULL),skipflag(false),goWizard(NULL) { manualCommandObj = ManualControlCommand::GetInstance(getObjectManager()); manualSettingsObj = ManualControlSettings::GetInstance(getObjectManager()); @@ -68,7 +68,7 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) : ConfigTaskWidget(parent) addUAVObjectToWidgetRelation("ManualControlSettings","ChannelMax",inp->ui->channelMax,index); ++index; } - QPushButton * goWizard=new QPushButton(tr("Start Wizard"),this); + goWizard=new QPushButton(tr("Start Wizard"),this); m_config->advancedPage->layout()->addWidget(goWizard); connect(goWizard,SIGNAL(clicked()),this,SLOT(goToWizard())); connect(m_config->wzNext,SIGNAL(clicked()),this,SLOT(wzNext())); @@ -92,6 +92,8 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) : ConfigTaskWidget(parent) addUAVObjectToWidgetRelation("ManualControlSettings","Arming",m_config->armControl); addUAVObjectToWidgetRelation("ManualControlSettings","armTimeout",m_config->armTimeout,0,1000); + + addWidget(goWizard); enableControls(false); populateWidgets(); @@ -258,6 +260,7 @@ void ConfigInputWidget::goToWizard() void ConfigInputWidget::wzCancel() { + dimOtherControls(false); manualCommandObj->setMetadata(manualCommandObj->getDefaultMetadata()); m_config->stackedWidget->setCurrentIndex(0); foreach (QWidget * wd, extraWidgets) @@ -305,6 +308,7 @@ void ConfigInputWidget::setupWizardWidget(int step) { if(step==wizardWelcome) { + m_config->graphicsView->setVisible(false); setTxMovement(nothing); if(wizardStep==wizardChooseMode) { @@ -317,7 +321,8 @@ void ConfigInputWidget::setupWizardWidget(int step) manualSettingsObj->setData(manualSettingsData); m_config->wzText->setText(tr("Welcome to the inputs configuration wizard.\n" "Please follow the instruction on the screen and only move your controls when asked to.\n" - "At any time you can press 'back' to return to the previous screeen or 'Cancel' to cancel the wizard" + "Make sure you already configured your hardware settings on the proper tab and restarted your board.\n" + "At any time you can press 'back' to return to the previous screeen or 'Cancel' to cancel the wizard \n" "For your safety your arming setting is now 'Always Disarmed' please reenable it after this wizard.")); m_config->stackedWidget->setCurrentIndex(1); m_config->wzBack->setEnabled(false); @@ -325,6 +330,7 @@ void ConfigInputWidget::setupWizardWidget(int step) } else if(step==wizardChooseMode) { + m_config->graphicsView->setVisible(true); setTxMovement(nothing); if(wizardStep==wizardIdentifySticks) { @@ -384,6 +390,7 @@ void ConfigInputWidget::setupWizardWidget(int step) } else if(step==wizardIdentifyLimits) { + dimOtherControls(false); setTxMovement(moveAll); if(wizardStep==wizardIdentifyCenter) { @@ -396,6 +403,13 @@ void ConfigInputWidget::setupWizardWidget(int step) } manualSettingsObj->setData(manualSettingsData); } + foreach (QWidget * wd, extraWidgets) + { + if(wd) + delete wd; + } + extraWidgets.clear(); + disconnect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); wizardStep=wizardIdentifyLimits; m_config->wzText->setText(QString(tr("Please move all controls to their maximum extends on both directions and press next when ready"))); UAVObject::Metadata mdata= manualCommandObj->getMetadata(); @@ -412,6 +426,7 @@ void ConfigInputWidget::setupWizardWidget(int step) } else if(step==wizardIdentifyInverted) { + dimOtherControls(true); setTxMovement(nothing); if(wizardStep==wizardIdentifyLimits) { @@ -644,6 +659,7 @@ void ConfigInputWidget::setTxMovement(txMovements movement) animate->start(50); break; case nothing: + movePos=0; animate->stop(); break; default: @@ -840,14 +856,34 @@ void ConfigInputWidget::moveSticks() trans=m_txLeftStickOrig; m_txLeftStick->setTransform(trans.translate(manualCommandData.Yaw*STICK_MAX_MOVE*10,-manualCommandData.Throttle*STICK_MAX_MOVE*10),false); trans=m_txRightStickOrig; - m_txRightStick->setTransform(trans.translate(manualCommandData.Roll*STICK_MAX_MOVE*10,-manualCommandData.Pitch*STICK_MAX_MOVE*10),false); + m_txRightStick->setTransform(trans.translate(manualCommandData.Roll*STICK_MAX_MOVE*10,manualCommandData.Pitch*STICK_MAX_MOVE*10),false); } else { trans=m_txRightStickOrig; m_txRightStick->setTransform(trans.translate(manualCommandData.Yaw*STICK_MAX_MOVE*10,-manualCommandData.Throttle*STICK_MAX_MOVE*10),false); trans=m_txLeftStickOrig; - m_txLeftStick->setTransform(trans.translate(manualCommandData.Roll*STICK_MAX_MOVE*10,-manualCommandData.Pitch*STICK_MAX_MOVE*10),false); + m_txLeftStick->setTransform(trans.translate(manualCommandData.Roll*STICK_MAX_MOVE*10,manualCommandData.Pitch*STICK_MAX_MOVE*10),false); } } +void ConfigInputWidget::dimOtherControls(bool value) +{ + qreal opac; + if(value) + opac=0.1; + else + opac=1; + m_txAccess0->setOpacity(opac); + m_txAccess1->setOpacity(opac); + m_txAccess2->setOpacity(opac); + m_txFlightMode->setOpacity(opac); +} + +void ConfigInputWidget::enableControls(bool enable) +{ + if(goWizard) + goWizard->setEnabled(enable); + ConfigTaskWidget::enableControls(enable); + +} diff --git a/ground/openpilotgcs/src/plugins/config/configinputwidget.h b/ground/openpilotgcs/src/plugins/config/configinputwidget.h index b1769026b..e3917bb85 100644 --- a/ground/openpilotgcs/src/plugins/config/configinputwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configinputwidget.h @@ -116,6 +116,7 @@ private: QTimer * animate; void resetTxControls(); void setMoveFromCommand(int command); + QPushButton * goWizard; private slots: void wzNext(); void wzBack(); @@ -125,9 +126,11 @@ private slots: void identifyControls(); void identifyLimits(); void moveTxControls(); - void moveSticks(); + void moveSticks(); + void dimOtherControls(bool value); protected: void resizeEvent(QResizeEvent *event); + virtual void enableControls(bool enable); }; diff --git a/ground/openpilotgcs/src/plugins/config/configtaskwidget.cpp b/ground/openpilotgcs/src/plugins/config/configtaskwidget.cpp index 8aa6a3f33..03b84804c 100644 --- a/ground/openpilotgcs/src/plugins/config/configtaskwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configtaskwidget.cpp @@ -238,6 +238,7 @@ void ConfigTaskWidget::setupButtons(QPushButton *update, QPushButton *save) connect(smartsave,SIGNAL(saveSuccessfull()),this,SLOT(clearDirty())); connect(smartsave,SIGNAL(beginOp()),this,SLOT(disableObjUpdates())); connect(smartsave,SIGNAL(endOp()),this,SLOT(enableObjUpdates())); + enableControls(false); } void ConfigTaskWidget::enableControls(bool enable) diff --git a/ground/openpilotgcs/src/plugins/config/input.ui b/ground/openpilotgcs/src/plugins/config/input.ui index 9849183d5..0ec15f063 100644 --- a/ground/openpilotgcs/src/plugins/config/input.ui +++ b/ground/openpilotgcs/src/plugins/config/input.ui @@ -36,6 +36,18 @@ + + + 0 + 0 + + + + + 0 + 70 + + 10 diff --git a/ground/openpilotgcs/src/plugins/config/smartsavebutton.cpp b/ground/openpilotgcs/src/plugins/config/smartsavebutton.cpp index dff4e409e..7335570eb 100644 --- a/ground/openpilotgcs/src/plugins/config/smartsavebutton.cpp +++ b/ground/openpilotgcs/src/plugins/config/smartsavebutton.cpp @@ -92,7 +92,8 @@ void smartSaveButton::setObjects(QList list) void smartSaveButton::addObject(UAVObject * obj) { - objects.append(obj); + if(!objects.contains(obj)) + objects.append(obj); } void smartSaveButton::clearObjects() From b8d47381d2dd4012502cbff1c3321d14d15b46c7 Mon Sep 17 00:00:00 2001 From: Oleg Semyonov Date: Wed, 17 Aug 2011 17:47:17 +0300 Subject: [PATCH 053/145] Milestones: add Maximus43's flybared heli inverted flight, funnel, loop As always, there is the rule: no video - no milestone :-) --- MILESTONES.txt | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/MILESTONES.txt b/MILESTONES.txt index 776c4a758..9a99dc8ed 100644 --- a/MILESTONES.txt +++ b/MILESTONES.txt @@ -137,6 +137,11 @@ C: Mat Wellington D: July 2011 V: http://www.youtube.com/watch?v=YE4Fd9vdg1I +M: First CopterControl Heli inverted flight, funnel, loop +C: Maxim Izergin (Maximus43) +D: August 2011 +V: http://www.youtube.com/watch?v=8SrfIS7OkB4 + M: First Altitude Hold using Sonar C: D: From 7791db98204635221739c057a5176e820322a54e Mon Sep 17 00:00:00 2001 From: Oleg Semyonov Date: Wed, 17 Aug 2011 17:58:39 +0300 Subject: [PATCH 054/145] Milestones: add alconaft43's milestones (same video, two pilots) --- MILESTONES.txt | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/MILESTONES.txt b/MILESTONES.txt index 9a99dc8ed..14db545d6 100644 --- a/MILESTONES.txt +++ b/MILESTONES.txt @@ -137,11 +137,16 @@ C: Mat Wellington D: July 2011 V: http://www.youtube.com/watch?v=YE4Fd9vdg1I -M: First CopterControl Heli inverted flight, funnel, loop +M: First CopterControl Flybared Heli inverted flight (2:33) C: Maxim Izergin (Maximus43) D: August 2011 V: http://www.youtube.com/watch?v=8SrfIS7OkB4 +M: First CopterControl Flybared Heli funnel (4:18), loop (5:35) +C: Sergey Solodennikov (alconaft43) +D: August 2011 +V: http://www.youtube.com/watch?v=8SrfIS7OkB4 + M: First Altitude Hold using Sonar C: D: From 25b54a4323f31e25de6b9ab48c735931ed107ec6 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Fri, 19 Aug 2011 01:46:52 -0500 Subject: [PATCH 055/145] Change to default settings at request of Dave. AccelKp = 0.05. MaxAxisLock = 15 degrees. --- shared/uavobjectdefinition/attitudesettings.xml | 2 +- shared/uavobjectdefinition/stabilizationsettings.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/shared/uavobjectdefinition/attitudesettings.xml b/shared/uavobjectdefinition/attitudesettings.xml index 522fd6f3a..677493d68 100644 --- a/shared/uavobjectdefinition/attitudesettings.xml +++ b/shared/uavobjectdefinition/attitudesettings.xml @@ -5,7 +5,7 @@ - + diff --git a/shared/uavobjectdefinition/stabilizationsettings.xml b/shared/uavobjectdefinition/stabilizationsettings.xml index b6668350d..653c7cce8 100644 --- a/shared/uavobjectdefinition/stabilizationsettings.xml +++ b/shared/uavobjectdefinition/stabilizationsettings.xml @@ -16,7 +16,7 @@ - + From 01a844e0da7a45ac6339e59d37048f9bdb33ba22 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Fri, 19 Aug 2011 12:11:05 +0200 Subject: [PATCH 056/145] Fixed redefinition of configCHECK_FOR_STACK_OVERFLOW in OpenPilot/System/inc/FreeRTOSConfig.h --- flight/OpenPilot/System/inc/FreeRTOSConfig.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/OpenPilot/System/inc/FreeRTOSConfig.h b/flight/OpenPilot/System/inc/FreeRTOSConfig.h index 1899e2db1..48b7c1560 100644 --- a/flight/OpenPilot/System/inc/FreeRTOSConfig.h +++ b/flight/OpenPilot/System/inc/FreeRTOSConfig.h @@ -39,7 +39,7 @@ #define configUSE_RECURSIVE_MUTEXES 1 #define configUSE_COUNTING_SEMAPHORES 0 #define configUSE_ALTERNATIVE_API 0 -#define configCHECK_FOR_STACK_OVERFLOW 2 +//#define configCHECK_FOR_STACK_OVERFLOW 2 #define configQUEUE_REGISTRY_SIZE 10 /* Co-routine definitions. */ From 78e42470d47e987633b049224ff26fbec3aa3444 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Fri, 19 Aug 2011 12:56:26 +0200 Subject: [PATCH 057/145] Debugging: Add PIOS_DEBUG_Panic() call wen an UAVObject handle is requested but object is uninitialized, make PiOS.posix bail in case of a Panic with an error message --- flight/PiOS.posix/posix/pios_debug.c | 11 +++++++++-- flight/UAVObjects/uavobjecttemplate.c | 3 ++- 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/flight/PiOS.posix/posix/pios_debug.c b/flight/PiOS.posix/posix/pios_debug.c index f79f2e730..5956a9d04 100644 --- a/flight/PiOS.posix/posix/pios_debug.c +++ b/flight/PiOS.posix/posix/pios_debug.c @@ -79,8 +79,15 @@ void PIOS_DEBUG_Panic(const char *msg) PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_DEBUG, "\r%s @0x%x\r", msg, lr); #endif - // Stay put - while (1) ; + // tell the user whats going on on commandline too + fprintf(stderr,"CRITICAL ERROR: %s\n",msg); + + // this helps debugging: causing a div by zero allows a backtrace + // and/or ends execution + int b = 0; + int a = (2/b); + b=a; + } /** diff --git a/flight/UAVObjects/uavobjecttemplate.c b/flight/UAVObjects/uavobjecttemplate.c index 019b282fe..a773c2756 100644 --- a/flight/UAVObjects/uavobjecttemplate.c +++ b/flight/UAVObjects/uavobjecttemplate.c @@ -40,7 +40,7 @@ #include "$(NAMELC).h" // Private variables -static UAVObjHandle handle; +static UAVObjHandle handle = NULL; /** * Initialize object. @@ -103,6 +103,7 @@ $(INITFIELDS) */ UAVObjHandle $(NAME)Handle() { + if (!handle) PIOS_DEBUG_Panic("$(NAME) used uninitialized"); return handle; } From 9ad85e9b7bc7c23a416c9dbd2f931533d902607a Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Fri, 19 Aug 2011 13:20:15 +0200 Subject: [PATCH 058/145] Fixed modules that didn't initialize used UAVObjects correctly --- flight/Modules/FlightPlan/flightplan.c | 4 ++++ flight/Modules/Guidance/guidance.c | 15 +++++++++++++++ flight/Modules/ManualControl/manualcontrol.c | 8 ++++++-- flight/Modules/Stabilization/stabilization.c | 2 ++ 4 files changed, 27 insertions(+), 2 deletions(-) diff --git a/flight/Modules/FlightPlan/flightplan.c b/flight/Modules/FlightPlan/flightplan.c index 5537c8f15..e983d6445 100644 --- a/flight/Modules/FlightPlan/flightplan.c +++ b/flight/Modules/FlightPlan/flightplan.c @@ -73,7 +73,11 @@ int32_t FlightPlanStart() */ int32_t FlightPlanInitialize() { + // Initialize variables taskHandle = NULL; + FlightPlanStatusInitialize(); + FlightPlanControlInitialize(); + FlightPlanSettingsInitialize(); // Listen for object updates FlightPlanControlConnectCallback(&objectUpdatedCb); diff --git a/flight/Modules/Guidance/guidance.c b/flight/Modules/Guidance/guidance.c index 348210675..14e293579 100644 --- a/flight/Modules/Guidance/guidance.c +++ b/flight/Modules/Guidance/guidance.c @@ -97,6 +97,21 @@ int32_t GuidanceStart() */ int32_t GuidanceInitialize() { + //initialize variables + GuidanceSettingsInitialize(); + AttitudeRawInitialize(); + AttitudeActualInitialize(); + PositionDesiredInitialize(); + PositionActualInitialize(); + ManualControlCommandInitialize(); + FlightStatusInitialize(); + NedAccelInitialize(); + StabilizationDesiredInitialize(); + StabilizationSettingsInitialize(); + SystemSettingsInitialize(); + VelocityDesiredInitialize(); + VelocityActualInitialize(); + // Create object queue queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c index bdd9f6b34..7767d57d8 100644 --- a/flight/Modules/ManualControl/manualcontrol.c +++ b/flight/Modules/ManualControl/manualcontrol.c @@ -112,10 +112,14 @@ int32_t ManualControlInitialize() if(!assumptions) return -1; - AccessoryDesiredInitialize(); + ManualControlSettingsInitialize(); + StabilizationSettingsInitialize(); ManualControlCommandInitialize(); - FlightStatusInitialize(); + ActuatorDesiredInitialize(); StabilizationDesiredInitialize(); + FlightTelemetryStatsInitialize(); + FlightStatusInitialize(); + AccessoryDesiredInitialize(); // ManualControlSettingsInitialize(); // this is initialized in // pios_board.c diff --git a/flight/Modules/Stabilization/stabilization.c b/flight/Modules/Stabilization/stabilization.c index 7142c9770..74175b86c 100644 --- a/flight/Modules/Stabilization/stabilization.c +++ b/flight/Modules/Stabilization/stabilization.c @@ -115,6 +115,8 @@ int32_t StabilizationInitialize() // Initialize variables StabilizationSettingsInitialize(); ActuatorDesiredInitialize(); + AttitudeActualInitialize(); + AttitudeRawInitialize(); #if defined(DIAGNOSTICS) RateDesiredInitialize(); #endif From adfb6eccc2ace32f1857f9400ef5b6437154bc47 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Fri, 19 Aug 2011 13:20:40 +0200 Subject: [PATCH 059/145] fixed usage of GPS buffer in sim_posix --- flight/OpenPilot/System/inc/pios_config_posix.h | 1 + flight/OpenPilot/System/pios_board_posix.c | 7 +++++-- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/flight/OpenPilot/System/inc/pios_config_posix.h b/flight/OpenPilot/System/inc/pios_config_posix.h index 4b779cc49..e8701be90 100644 --- a/flight/OpenPilot/System/inc/pios_config_posix.h +++ b/flight/OpenPilot/System/inc/pios_config_posix.h @@ -36,6 +36,7 @@ #define PIOS_INCLUDE_SDCARD #define PIOS_INCLUDE_FREERTOS #define PIOS_INCLUDE_COM +#define PIOS_INCLUDE_GPS #define PIOS_INCLUDE_IRQ #define PIOS_INCLUDE_TELEMETRY_RF #define PIOS_INCLUDE_UDP diff --git a/flight/OpenPilot/System/pios_board_posix.c b/flight/OpenPilot/System/pios_board_posix.c index 2480d0730..573291025 100644 --- a/flight/OpenPilot/System/pios_board_posix.c +++ b/flight/OpenPilot/System/pios_board_posix.c @@ -66,6 +66,7 @@ const struct pios_udp_cfg pios_udp_aux_cfg = { #define PIOS_COM_TELEM_RF_RX_BUF_LEN 192 #define PIOS_COM_TELEM_RF_TX_BUF_LEN 192 +#define PIOS_COM_GPS_RX_BUF_LEN 192 /* * Board specific number of devices. @@ -154,14 +155,16 @@ void PIOS_Board_Init(void) { #if defined(PIOS_INCLUDE_GPS) { uint32_t pios_udp_gps_id; - if (PIOS_USART_Init(&pios_udp_gps_id, &pios_udp_gps_cfg)) { + if (PIOS_UDP_Init(&pios_udp_gps_id, &pios_udp_gps_cfg)) { PIOS_Assert(0); } uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN); + uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN); PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_gps_id, &pios_udp_com_driver, pios_udp_gps_id, rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, - NULL, 0)) { + rx_buffer, PIOS_COM_GPS_RX_BUF_LEN)) { PIOS_Assert(0); } } From 69f10cd6d05906a2c88354043ff445c02e2eee7d Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Fri, 19 Aug 2011 13:54:16 +0200 Subject: [PATCH 060/145] PiOS.posix: bugfix in COM_UDP --- flight/OpenPilot/System/pios_board_posix.c | 2 +- flight/PiOS.posix/posix/pios_udp.c | 3 +++ 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/flight/OpenPilot/System/pios_board_posix.c b/flight/OpenPilot/System/pios_board_posix.c index 573291025..fa2cec9de 100644 --- a/flight/OpenPilot/System/pios_board_posix.c +++ b/flight/OpenPilot/System/pios_board_posix.c @@ -164,7 +164,7 @@ void PIOS_Board_Init(void) { PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_gps_id, &pios_udp_com_driver, pios_udp_gps_id, rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, - rx_buffer, PIOS_COM_GPS_RX_BUF_LEN)) { + tx_buffer, PIOS_COM_GPS_RX_BUF_LEN)) { PIOS_Assert(0); } } diff --git a/flight/PiOS.posix/posix/pios_udp.c b/flight/PiOS.posix/posix/pios_udp.c index 39077b455..f7f91f620 100644 --- a/flight/PiOS.posix/posix/pios_udp.c +++ b/flight/PiOS.posix/posix/pios_udp.c @@ -64,6 +64,7 @@ static pios_udp_dev * find_udp_dev_by_id (uint8_t udp) { if (udp >= pios_udp_num_devices) { /* Undefined UDP port for this board (see pios_board.c) */ + PIOS_Assert(0); return NULL; } @@ -154,6 +155,8 @@ int32_t PIOS_UDP_Init(uint32_t * udp_id, const struct pios_udp_cfg * cfg) printf("udp dev %i - socket %i opened - result %i\n",pios_udp_num_devices-1,udp_dev->socket,res); + *udp_id = pios_udp_num_devices-1; + return res; } From 482bec497b80c8a4b656ff4ba20158cfca04c62d Mon Sep 17 00:00:00 2001 From: James Cotton Date: Fri, 19 Aug 2011 09:44:38 -0500 Subject: [PATCH 061/145] Revert "Fixed modules that didn't initialize used UAVObjects correctly" Modules should initialize their settings and the objects they source. Not just everything. This reverts commit 9ad85e9b7bc7c23a416c9dbd2f931533d902607a. --- flight/Modules/FlightPlan/flightplan.c | 4 ---- flight/Modules/Guidance/guidance.c | 15 --------------- flight/Modules/ManualControl/manualcontrol.c | 10 +++------- flight/Modules/Stabilization/stabilization.c | 2 -- 4 files changed, 3 insertions(+), 28 deletions(-) diff --git a/flight/Modules/FlightPlan/flightplan.c b/flight/Modules/FlightPlan/flightplan.c index e983d6445..5537c8f15 100644 --- a/flight/Modules/FlightPlan/flightplan.c +++ b/flight/Modules/FlightPlan/flightplan.c @@ -73,11 +73,7 @@ int32_t FlightPlanStart() */ int32_t FlightPlanInitialize() { - // Initialize variables taskHandle = NULL; - FlightPlanStatusInitialize(); - FlightPlanControlInitialize(); - FlightPlanSettingsInitialize(); // Listen for object updates FlightPlanControlConnectCallback(&objectUpdatedCb); diff --git a/flight/Modules/Guidance/guidance.c b/flight/Modules/Guidance/guidance.c index 14e293579..348210675 100644 --- a/flight/Modules/Guidance/guidance.c +++ b/flight/Modules/Guidance/guidance.c @@ -97,21 +97,6 @@ int32_t GuidanceStart() */ int32_t GuidanceInitialize() { - //initialize variables - GuidanceSettingsInitialize(); - AttitudeRawInitialize(); - AttitudeActualInitialize(); - PositionDesiredInitialize(); - PositionActualInitialize(); - ManualControlCommandInitialize(); - FlightStatusInitialize(); - NedAccelInitialize(); - StabilizationDesiredInitialize(); - StabilizationSettingsInitialize(); - SystemSettingsInitialize(); - VelocityDesiredInitialize(); - VelocityActualInitialize(); - // Create object queue queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c index 7767d57d8..bdd9f6b34 100644 --- a/flight/Modules/ManualControl/manualcontrol.c +++ b/flight/Modules/ManualControl/manualcontrol.c @@ -112,14 +112,10 @@ int32_t ManualControlInitialize() if(!assumptions) return -1; - ManualControlSettingsInitialize(); - StabilizationSettingsInitialize(); - ManualControlCommandInitialize(); - ActuatorDesiredInitialize(); - StabilizationDesiredInitialize(); - FlightTelemetryStatsInitialize(); - FlightStatusInitialize(); AccessoryDesiredInitialize(); + ManualControlCommandInitialize(); + FlightStatusInitialize(); + StabilizationDesiredInitialize(); // ManualControlSettingsInitialize(); // this is initialized in // pios_board.c diff --git a/flight/Modules/Stabilization/stabilization.c b/flight/Modules/Stabilization/stabilization.c index 74175b86c..7142c9770 100644 --- a/flight/Modules/Stabilization/stabilization.c +++ b/flight/Modules/Stabilization/stabilization.c @@ -115,8 +115,6 @@ int32_t StabilizationInitialize() // Initialize variables StabilizationSettingsInitialize(); ActuatorDesiredInitialize(); - AttitudeActualInitialize(); - AttitudeRawInitialize(); #if defined(DIAGNOSTICS) RateDesiredInitialize(); #endif From ced29e221ef95279c9b6b1a48b8e1f38df0a84d4 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Fri, 19 Aug 2011 09:45:10 -0500 Subject: [PATCH 062/145] Revert "Debugging: Add PIOS_DEBUG_Panic() call wen an UAVObject handle is requested but object is uninitialized, make PiOS.posix bail in case of a Panic with an error message" We do not want to panic in this case as we want the ability to detect if objects exist at run time This reverts commit 78e42470d47e987633b049224ff26fbec3aa3444. --- flight/PiOS.posix/posix/pios_debug.c | 11 ++--------- flight/UAVObjects/uavobjecttemplate.c | 3 +-- 2 files changed, 3 insertions(+), 11 deletions(-) diff --git a/flight/PiOS.posix/posix/pios_debug.c b/flight/PiOS.posix/posix/pios_debug.c index 5956a9d04..f79f2e730 100644 --- a/flight/PiOS.posix/posix/pios_debug.c +++ b/flight/PiOS.posix/posix/pios_debug.c @@ -79,15 +79,8 @@ void PIOS_DEBUG_Panic(const char *msg) PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_DEBUG, "\r%s @0x%x\r", msg, lr); #endif - // tell the user whats going on on commandline too - fprintf(stderr,"CRITICAL ERROR: %s\n",msg); - - // this helps debugging: causing a div by zero allows a backtrace - // and/or ends execution - int b = 0; - int a = (2/b); - b=a; - + // Stay put + while (1) ; } /** diff --git a/flight/UAVObjects/uavobjecttemplate.c b/flight/UAVObjects/uavobjecttemplate.c index a773c2756..019b282fe 100644 --- a/flight/UAVObjects/uavobjecttemplate.c +++ b/flight/UAVObjects/uavobjecttemplate.c @@ -40,7 +40,7 @@ #include "$(NAMELC).h" // Private variables -static UAVObjHandle handle = NULL; +static UAVObjHandle handle; /** * Initialize object. @@ -103,7 +103,6 @@ $(INITFIELDS) */ UAVObjHandle $(NAME)Handle() { - if (!handle) PIOS_DEBUG_Panic("$(NAME) used uninitialized"); return handle; } From ab7e57ad2b4f65dc125911ffa9572af127113c62 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Fri, 19 Aug 2011 09:52:05 -0500 Subject: [PATCH 063/145] Initialize objects for guidance in the correct place. Init some objects in pios_board_posix since AHRSComms not around. --- flight/Modules/AHRSComms/ahrs_comms.c | 11 ++++++----- flight/Modules/FlightPlan/flightplan.c | 6 +++++- flight/Modules/GPS/GPS.c | 2 +- flight/Modules/Guidance/guidance.c | 8 ++++++++ flight/OpenPilot/System/pios_board_posix.c | 8 ++++++++ 5 files changed, 28 insertions(+), 7 deletions(-) diff --git a/flight/Modules/AHRSComms/ahrs_comms.c b/flight/Modules/AHRSComms/ahrs_comms.c index b2afc17cc..c3fb45fae 100644 --- a/flight/Modules/AHRSComms/ahrs_comms.c +++ b/flight/Modules/AHRSComms/ahrs_comms.c @@ -73,11 +73,7 @@ static void ahrscommsTask(void *parameters); */ int32_t AHRSCommsStart(void) { - // Start main task - AhrsStatusInitialize(); - AHRSCalibrationInitialize(); - AttitudeRawInitialize(); - + // Start main task xTaskCreate(ahrscommsTask, (signed char *)"AHRSComms", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle); TaskMonitorAdd(TASKINFO_RUNNING_AHRSCOMMS, taskHandle); PIOS_WDG_RegisterFlag(PIOS_WDG_AHRS); @@ -91,6 +87,11 @@ int32_t AHRSCommsStart(void) */ int32_t AHRSCommsInitialize(void) { + AhrsStatusInitialize(); + AHRSCalibrationInitialize(); + AttitudeRawInitialize(); + VelocityActualInitialize(); + PositionActualInitialize(); return 0; } diff --git a/flight/Modules/FlightPlan/flightplan.c b/flight/Modules/FlightPlan/flightplan.c index 5537c8f15..e919dd41e 100644 --- a/flight/Modules/FlightPlan/flightplan.c +++ b/flight/Modules/FlightPlan/flightplan.c @@ -74,7 +74,11 @@ int32_t FlightPlanStart() int32_t FlightPlanInitialize() { taskHandle = NULL; - + + FlightPlanStatusInitialize(); + FlightPlanControlInitialize(); + FlightPlanSettingsInitialize(); + // Listen for object updates FlightPlanControlConnectCallback(&objectUpdatedCb); diff --git a/flight/Modules/GPS/GPS.c b/flight/Modules/GPS/GPS.c index c08964342..60ff80184 100644 --- a/flight/Modules/GPS/GPS.c +++ b/flight/Modules/GPS/GPS.c @@ -127,7 +127,7 @@ int32_t GPSInitialize(void) GPSPositionInitialize(); GPSTimeInitialize(); HomeLocationInitialize(); - + // TODO: Get gps settings object gpsPort = PIOS_COM_GPS; diff --git a/flight/Modules/Guidance/guidance.c b/flight/Modules/Guidance/guidance.c index 348210675..d1626923c 100644 --- a/flight/Modules/Guidance/guidance.c +++ b/flight/Modules/Guidance/guidance.c @@ -97,6 +97,14 @@ int32_t GuidanceStart() */ int32_t GuidanceInitialize() { + + GuidanceSettingsInitialize(); + PositionDesiredInitialize(); + ManualControlCommandInitialize(); + FlightStatusInitialize(); + NedAccelInitialize(); + VelocityDesiredInitialize(); + // Create object queue queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); diff --git a/flight/OpenPilot/System/pios_board_posix.c b/flight/OpenPilot/System/pios_board_posix.c index fa2cec9de..2285533aa 100644 --- a/flight/OpenPilot/System/pios_board_posix.c +++ b/flight/OpenPilot/System/pios_board_posix.c @@ -29,6 +29,10 @@ #include #include +#include "attituderaw.h" +#include "positionactual.h" +#include "velocityactual.h" + #include "pios_rcvr_priv.h" struct pios_rcvr_channel_map pios_rcvr_channel_to_id_map[PIOS_RCVR_MAX_CHANNELS]; @@ -171,6 +175,10 @@ void PIOS_Board_Init(void) { #endif /* PIOS_INCLUDE_GPS */ #endif + // Initialize these here as posix has no AHRSComms + AttitudeRawInitialize(); + VelocityActualInitialize(); + PositionActualInitialize(); } From 03a8bd76741ca139be2a62f7509bbde9fcb0e861 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Fri, 19 Aug 2011 10:12:39 -0500 Subject: [PATCH 064/145] Statically initialize object handles to null to make sure the uavobjectmanager can catch it and return -1. Do NOT panic when they are null as this is something that should be caugth at run time in some cases. Ideally as a compromise in a task start it could have a set of PIOS_Assert(ObjectNameHandle()) to make sure the minimal set of objects it needs are there. --- flight/UAVObjects/uavobjecttemplate.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/UAVObjects/uavobjecttemplate.c b/flight/UAVObjects/uavobjecttemplate.c index 019b282fe..6d572dbcb 100644 --- a/flight/UAVObjects/uavobjecttemplate.c +++ b/flight/UAVObjects/uavobjecttemplate.c @@ -40,7 +40,7 @@ #include "$(NAMELC).h" // Private variables -static UAVObjHandle handle; +static UAVObjHandle handle = NULL; /** * Initialize object. From 9bba1f7ae9a6f00779c76de5f246bdd3e4b447d4 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Fri, 19 Aug 2011 18:36:30 +0200 Subject: [PATCH 065/145] Posix: fix object initialization where necessary --- flight/Modules/Guidance/guidance.c | 2 -- flight/Modules/ManualControl/manualcontrol.c | 3 +-- flight/OpenPilot/System/pios_board_posix.c | 2 ++ 3 files changed, 3 insertions(+), 4 deletions(-) diff --git a/flight/Modules/Guidance/guidance.c b/flight/Modules/Guidance/guidance.c index d1626923c..b90303928 100644 --- a/flight/Modules/Guidance/guidance.c +++ b/flight/Modules/Guidance/guidance.c @@ -100,8 +100,6 @@ int32_t GuidanceInitialize() GuidanceSettingsInitialize(); PositionDesiredInitialize(); - ManualControlCommandInitialize(); - FlightStatusInitialize(); NedAccelInitialize(); VelocityDesiredInitialize(); diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c index bdd9f6b34..ba47fd64b 100644 --- a/flight/Modules/ManualControl/manualcontrol.c +++ b/flight/Modules/ManualControl/manualcontrol.c @@ -117,8 +117,7 @@ int32_t ManualControlInitialize() FlightStatusInitialize(); StabilizationDesiredInitialize(); - // ManualControlSettingsInitialize(); // this is initialized in - // pios_board.c + ManualControlSettingsInitialize(); return 0; } diff --git a/flight/OpenPilot/System/pios_board_posix.c b/flight/OpenPilot/System/pios_board_posix.c index 2285533aa..e1e8dcc54 100644 --- a/flight/OpenPilot/System/pios_board_posix.c +++ b/flight/OpenPilot/System/pios_board_posix.c @@ -30,6 +30,7 @@ #include #include "attituderaw.h" +#include "attitudeactual.h" #include "positionactual.h" #include "velocityactual.h" @@ -177,6 +178,7 @@ void PIOS_Board_Init(void) { // Initialize these here as posix has no AHRSComms AttitudeRawInitialize(); + AttitudeActualInitialize(); VelocityActualInitialize(); PositionActualInitialize(); From 5e4941f7cbecb2181f559fd12fa2489120573b1a Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Fri, 19 Aug 2011 18:41:59 +0200 Subject: [PATCH 066/145] PiOS.posix: make PiOS.posix bail in case of a Panic with an error message --- flight/PiOS.posix/posix/pios_debug.c | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/flight/PiOS.posix/posix/pios_debug.c b/flight/PiOS.posix/posix/pios_debug.c index f79f2e730..5956a9d04 100644 --- a/flight/PiOS.posix/posix/pios_debug.c +++ b/flight/PiOS.posix/posix/pios_debug.c @@ -79,8 +79,15 @@ void PIOS_DEBUG_Panic(const char *msg) PIOS_COM_SendFormattedStringNonBlocking(PIOS_COM_DEBUG, "\r%s @0x%x\r", msg, lr); #endif - // Stay put - while (1) ; + // tell the user whats going on on commandline too + fprintf(stderr,"CRITICAL ERROR: %s\n",msg); + + // this helps debugging: causing a div by zero allows a backtrace + // and/or ends execution + int b = 0; + int a = (2/b); + b=a; + } /** From 2062ec0aeb5342cee0c75b848b01cb6493fa275f Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Fri, 19 Aug 2011 19:52:27 +0200 Subject: [PATCH 067/145] AHRS_COM: Initialize transpherred objects before using them --- flight/Libraries/ahrs_comm_objects.c | 1 + 1 file changed, 1 insertion(+) diff --git a/flight/Libraries/ahrs_comm_objects.c b/flight/Libraries/ahrs_comm_objects.c index e43c380e4..5fc4f1b20 100644 --- a/flight/Libraries/ahrs_comm_objects.c +++ b/flight/Libraries/ahrs_comm_objects.c @@ -79,6 +79,7 @@ CREATEHANDLE(10, FirmwareIAPObj); static void ObjectUpdatedCb(UAVObjEvent * ev); #define ADDHANDLE(idx,obj) {\ + obj##Initialize();\ int n = idx;\ objectHandles[n].data = &obj;\ objectHandles[n].uavHandle = obj##Handle();\ From 85aef9c7b46caff6e6a7913fe32117f06b53cc17 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Fri, 19 Aug 2011 20:20:44 +0200 Subject: [PATCH 068/145] OpenPilot: bugfix: Add GPS tx buffer by default (needed until GPS has been refactored to not send or only send when configured so) --- flight/OpenPilot/System/inc/pios_config.h | 1 + flight/OpenPilot/System/pios_board.c | 5 ++++- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/flight/OpenPilot/System/inc/pios_config.h b/flight/OpenPilot/System/inc/pios_config.h index 4d561382e..4e6657cef 100644 --- a/flight/OpenPilot/System/inc/pios_config.h +++ b/flight/OpenPilot/System/inc/pios_config.h @@ -60,6 +60,7 @@ //#define PIOS_INCLUDE_HCSR04 #define PIOS_INCLUDE_OPAHRS #define PIOS_INCLUDE_COM +#define PIOS_INCLUDE_GPS #define PIOS_INCLUDE_SDCARD #define PIOS_INCLUDE_SETTINGS #define PIOS_INCLUDE_FREERTOS diff --git a/flight/OpenPilot/System/pios_board.c b/flight/OpenPilot/System/pios_board.c index fbc4419a1..f4ac5c367 100644 --- a/flight/OpenPilot/System/pios_board.c +++ b/flight/OpenPilot/System/pios_board.c @@ -525,6 +525,7 @@ static const struct pios_spektrum_cfg pios_spektrum_cfg = { #define PIOS_COM_TELEM_RF_TX_BUF_LEN 192 #define PIOS_COM_GPS_RX_BUF_LEN 96 +#define PIOS_COM_GPS_TX_BUF_LEN 96 #define PIOS_COM_TELEM_USB_RX_BUF_LEN 192 #define PIOS_COM_TELEM_USB_TX_BUF_LEN 192 @@ -1074,10 +1075,12 @@ void PIOS_Board_Init(void) { PIOS_Assert(0); } uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN); + uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_TX_BUF_LEN); PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id, rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, - NULL, 0)) { + tx_buffer, PIOS_COM_GPS_TX_BUF_LEN)) { PIOS_Assert(0); } } From 7bf9f4e817c15d819e8b95bc6c39a655486ea471 Mon Sep 17 00:00:00 2001 From: zedamota Date: Sat, 20 Aug 2011 19:10:28 +0100 Subject: [PATCH 069/145] Untested changes, should fix: Spelling mistakes Show inverted movement as soon as checkbox is checked. Bring back flightmode change messagebox. Layout issues (this was not a issue on win so not sure its fixed). Support for flightmode binary switches neutral=min+(max-min)/2. Flightmode slider not changing according to command. Mode 1 roll on the right (still think it doesn't make sense :) ) --- .../plugins/config/config_cc_hw_widget.cpp | 2 +- .../plugins/config/config_pro_hw_widget.cpp | 2 +- .../src/plugins/config/configinputwidget.cpp | 111 +++++++++++++----- .../src/plugins/config/configinputwidget.h | 3 + .../src/plugins/config/inputchannelform.ui | 2 +- 5 files changed, 88 insertions(+), 32 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.cpp b/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.cpp index 382b074d5..1d98874ba 100644 --- a/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.cpp +++ b/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.cpp @@ -2,7 +2,7 @@ ****************************************************************************** * * @file configtelemetrywidget.h - * @author E. Lafargue & The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @addtogroup GCSPlugins GCS Plugins * @{ * @addtogroup ConfigPlugin Config Plugin diff --git a/ground/openpilotgcs/src/plugins/config/config_pro_hw_widget.cpp b/ground/openpilotgcs/src/plugins/config/config_pro_hw_widget.cpp index b3f562acd..e19be52ab 100644 --- a/ground/openpilotgcs/src/plugins/config/config_pro_hw_widget.cpp +++ b/ground/openpilotgcs/src/plugins/config/config_pro_hw_widget.cpp @@ -2,7 +2,7 @@ ****************************************************************************** * * @file configtelemetrywidget.h - * @author E. Lafargue & The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @addtogroup GCSPlugins GCS Plugins * @{ * @addtogroup ConfigPlugin Config Plugin diff --git a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp index 3fc4e8378..4544b4edd 100644 --- a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp @@ -1,7 +1,7 @@ /** ****************************************************************************** * - * @file configservowidget.cpp + * @file configinputwidget.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @addtogroup GCSPlugins GCS Plugins * @{ @@ -39,6 +39,7 @@ #include #include #include +#include #define ACCESS_MIN_MOVE -6 #define ACCESS_MAX_MOVE 6 @@ -92,7 +93,7 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) : ConfigTaskWidget(parent) addUAVObjectToWidgetRelation("ManualControlSettings","Arming",m_config->armControl); addUAVObjectToWidgetRelation("ManualControlSettings","armTimeout",m_config->armTimeout,0,1000); - + addUAVObject("ManualControlCommand"); addWidget(goWizard); enableControls(false); @@ -254,6 +255,12 @@ void ConfigInputWidget::openHelp() void ConfigInputWidget::goToWizard() { + QMessageBox msgBox; + msgBox.setText(tr("Arming Settings are now set to Always Disarmed for your safety.")); + msgBox.setDetailedText(tr("You will have to reconfigure arming settings yourself afterwards.")); + msgBox.setStandardButtons(QMessageBox::Ok); + msgBox.setDefaultButton(QMessageBox::Ok); + msgBox.exec(); setupWizardWidget(wizardWelcome); m_config->graphicsView->fitInView(m_txBackground, Qt::KeepAspectRatio ); } @@ -320,10 +327,9 @@ void ConfigInputWidget::setupWizardWidget(int step) manualSettingsData.Arming=ManualControlSettings::ARMING_ALWAYSDISARMED; manualSettingsObj->setData(manualSettingsData); m_config->wzText->setText(tr("Welcome to the inputs configuration wizard.\n" - "Please follow the instruction on the screen and only move your controls when asked to.\n" + "Please follow the instructions on the screen and only move your controls when asked to.\n" "Make sure you already configured your hardware settings on the proper tab and restarted your board.\n" - "At any time you can press 'back' to return to the previous screeen or 'Cancel' to cancel the wizard \n" - "For your safety your arming setting is now 'Always Disarmed' please reenable it after this wizard.")); + "At any time you can press 'back' to return to the previous screeen or 'Cancel' to cancel the wizard.\n")); m_config->stackedWidget->setCurrentIndex(1); m_config->wzBack->setEnabled(false); wizardStep=wizardWelcome; @@ -386,7 +392,7 @@ void ConfigInputWidget::setupWizardWidget(int step) manualCommandObj->setMetadata(manualCommandObj->getDefaultMetadata()); } wizardStep=wizardIdentifyCenter; - m_config->wzText->setText(QString(tr("Please center all control controls and press next when ready"))); + m_config->wzText->setText(QString(tr("Please center all control controls and press next when ready (if your FlightMode switch has only two positions, leave it on either position)"))); } else if(step==wizardIdentifyLimits) { @@ -411,7 +417,7 @@ void ConfigInputWidget::setupWizardWidget(int step) extraWidgets.clear(); disconnect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); wizardStep=wizardIdentifyLimits; - m_config->wzText->setText(QString(tr("Please move all controls to their maximum extends on both directions and press next when ready"))); + m_config->wzText->setText(QString(tr("Please move all controls to their maximum extents on both directions and press next when ready"))); UAVObject::Metadata mdata= manualCommandObj->getMetadata(); mdata.flightTelemetryUpdateMode = UAVObject::UPDATEMODE_PERIODIC; mdata.flightTelemetryUpdatePeriod = 150; @@ -441,33 +447,22 @@ void ConfigInputWidget::setupWizardWidget(int step) QCheckBox * cb=new QCheckBox(name,this); extraWidgets.append(cb); m_config->checkBoxesLayout->layout()->addWidget(cb); + connect(cb,SIGNAL(toggled(bool)),this,SLOT(invertControls())); } } connect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); wizardStep=wizardIdentifyInverted; - m_config->wzText->setText(QString(tr("Please check the picture below and check all the sticks which show an inverted movement and press next when ready"))); + m_config->wzText->setText(QString(tr("Please check the picture below and check all the sticks which show an inverted movement and press next when ready"))); } else if(step==wizardFinish) { - manualSettingsData=manualSettingsObj->getData(); foreach(QWidget * wd,extraWidgets) { QCheckBox * cb=qobject_cast(wd); if(cb) - { - if(cb->isChecked()) - { - int index=manualSettingsObj->getFields().at(0)->getElementNames().indexOf(cb->text()); - qint16 aux; - aux=manualSettingsData.ChannelMax[index]; - manualSettingsData.ChannelMax[index]=manualSettingsData.ChannelMin[index]; - manualSettingsData.ChannelMin[index]=aux; - } - } - delete cb; + delete cb; } wizardStep=wizardFinish; - manualSettingsObj->setData(manualSettingsData); extraWidgets.clear(); m_config->wzText->setText(QString(tr("You have completed this wizard, please check below if the picture below mimics your sticks movement.\n" "This new settings aren't saved to the board yet, after pressing next you will go to the initial screen where you can do that."))); @@ -484,6 +479,12 @@ void ConfigInputWidget::setupWizardWidget(int step) manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_THROTTLE]+ ((manualSettingsData.ChannelMax[ManualControlSettings::CHANNELMAX_THROTTLE]- manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_THROTTLE])*0.02); + if((abs(manualSettingsData.ChannelMax[ManualControlSettings::CHANNELMAX_FLIGHTMODE]-manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_FLIGHTMODE])<100) || + (abs(manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_FLIGHTMODE]-manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_FLIGHTMODE])<100)) + { + manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_FLIGHTMODE]=manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_FLIGHTMODE]+ + (manualSettingsData.ChannelMax[ManualControlSettings::CHANNELMAX_FLIGHTMODE]-manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_FLIGHTMODE])/2; + } manualSettingsObj->setData(manualSettingsData); m_config->stackedWidget->setCurrentIndex(0); wizardStep=wizardWelcome; @@ -549,10 +550,7 @@ void ConfigInputWidget::setMoveFromCommand(int command) //CHANNELNUMBER_ROLL=0, CHANNELNUMBER_PITCH=1, CHANNELNUMBER_YAW=2, CHANNELNUMBER_THROTTLE=3, CHANNELNUMBER_FLIGHTMODE=4, CHANNELNUMBER_ACCESSORY0=5, CHANNELNUMBER_ACCESSORY1=6, CHANNELNUMBER_ACCESSORY2=7 } ChannelNumberElem; if(command==ManualControlSettings::CHANNELNUMBER_ROLL) { - if(transmitterMode==mode2) setTxMovement(moveRightHorizontalStick); - else - setTxMovement(moveLeftHorizontalStick); } else if(command==ManualControlSettings::CHANNELNUMBER_PITCH) { @@ -563,10 +561,7 @@ void ConfigInputWidget::setMoveFromCommand(int command) } else if(command==ManualControlSettings::CHANNELNUMBER_YAW) { - if(transmitterMode==mode2) setTxMovement(moveLeftHorizontalStick); - else - setTxMovement(moveRightHorizontalStick); } else if(command==ManualControlSettings::CHANNELNUMBER_THROTTLE) { @@ -861,9 +856,9 @@ void ConfigInputWidget::moveSticks() else { trans=m_txRightStickOrig; - m_txRightStick->setTransform(trans.translate(manualCommandData.Yaw*STICK_MAX_MOVE*10,-manualCommandData.Throttle*STICK_MAX_MOVE*10),false); + m_txRightStick->setTransform(trans.translate(manualCommandData.Roll*STICK_MAX_MOVE*10,-manualCommandData.Throttle*STICK_MAX_MOVE*10),false); trans=m_txLeftStickOrig; - m_txLeftStick->setTransform(trans.translate(manualCommandData.Roll*STICK_MAX_MOVE*10,manualCommandData.Pitch*STICK_MAX_MOVE*10),false); + m_txLeftStick->setTransform(trans.translate(manualCommandData.Yaw*STICK_MAX_MOVE*10,manualCommandData.Pitch*STICK_MAX_MOVE*10),false); } } @@ -887,3 +882,61 @@ void ConfigInputWidget::enableControls(bool enable) ConfigTaskWidget::enableControls(enable); } + +void ConfigInputWidget::invertControls() +{ + manualSettingsData=manualSettingsObj->getData(); + foreach(QWidget * wd,extraWidgets) + { + QCheckBox * cb=qobject_cast(wd); + if(cb) + { + if(cb->isChecked()) + { + int index=manualSettingsObj->getFields().at(0)->getElementNames().indexOf(cb->text()); + qint16 aux; + aux=manualSettingsData.ChannelMax[index]; + manualSettingsData.ChannelMax[index]=manualSettingsData.ChannelMin[index]; + manualSettingsData.ChannelMin[index]=aux; + } + } + } + manualSettingsObj->setData(manualSettingsData); +} +void ConfigInputWidget::refreshWidgetsValues() +{ + ConfigTaskWidget::refreshWidgetsValues(); + manualSettingsData = manualSettingsObj->getData(); + manualCommandData=manualCommandObj->getData(); + uint chIndex = manualSettingsData.ChannelNumber[ManualControlSettings::CHANNELNUMBER_FLIGHTMODE]; + if (chIndex < 8) { + float valueScaled; + + int chMin = manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_FLIGHTMODE]; + int chMax = manualSettingsData.ChannelMax[ManualControlSettings::CHANNELMAX_FLIGHTMODE]; + int chNeutral = manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_FLIGHTMODE]; + + int value = manualCommandData.Channel[chIndex]; + if ((chMax > chMin && value >= chNeutral) || (chMin > chMax && value <= chNeutral)) + { + if (chMax != chNeutral) + valueScaled = (float)(value - chNeutral) / (float)(chMax - chNeutral); + else + valueScaled = 0; + } + else + { + if (chMin != chNeutral) + valueScaled = (float)(value - chNeutral) / (float)(chNeutral - chMin); + else + valueScaled = 0; + } + + if(valueScaled < -(1.0 / 3.0)) + m_config->fmsSlider->setValue(-100); + else if (valueScaled > (1.0/3.0)) + m_config->fmsSlider->setValue(100); + else + m_config->fmsSlider->setValue(0); + } +} diff --git a/ground/openpilotgcs/src/plugins/config/configinputwidget.h b/ground/openpilotgcs/src/plugins/config/configinputwidget.h index e3917bb85..7c90fef76 100644 --- a/ground/openpilotgcs/src/plugins/config/configinputwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configinputwidget.h @@ -128,9 +128,12 @@ private slots: void moveTxControls(); void moveSticks(); void dimOtherControls(bool value); + void invertControls(); protected: void resizeEvent(QResizeEvent *event); virtual void enableControls(bool enable); +protected slots: + virtual void refreshWidgetsValues(); }; diff --git a/ground/openpilotgcs/src/plugins/config/inputchannelform.ui b/ground/openpilotgcs/src/plugins/config/inputchannelform.ui index 4d7f07d9d..ac4c9e015 100644 --- a/ground/openpilotgcs/src/plugins/config/inputchannelform.ui +++ b/ground/openpilotgcs/src/plugins/config/inputchannelform.ui @@ -76,7 +76,7 @@ - 66 + 70 0 From e4e952f27004d2a393b61b32959cd44d93037e17 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sun, 21 Aug 2011 02:20:10 -0500 Subject: [PATCH 070/145] Manual control settings needs to be initialized in OpenPilot board file for spektrum to work --- flight/OpenPilot/System/pios_board.c | 1 + 1 file changed, 1 insertion(+) diff --git a/flight/OpenPilot/System/pios_board.c b/flight/OpenPilot/System/pios_board.c index f4ac5c367..12d365b67 100644 --- a/flight/OpenPilot/System/pios_board.c +++ b/flight/OpenPilot/System/pios_board.c @@ -1093,6 +1093,7 @@ void PIOS_Board_Init(void) { /* Configure the selected receiver */ uint8_t manualcontrolsettings_inputmode; + ManualControlSettingsInitialize(); ManualControlSettingsInputModeGet(&manualcontrolsettings_inputmode); switch (manualcontrolsettings_inputmode) { From b6b0703c284caebf6b86953391eb806aea178028 Mon Sep 17 00:00:00 2001 From: Dmytro Poplavskiy Date: Sat, 20 Aug 2011 12:28:53 +1000 Subject: [PATCH 071/145] GCS: Fixed stabilization settings layout on a small screen. Put most of widgets into QScrollArea. It looks the same on large enough screen but scrollbars are added on netbook size screen. --- .../src/plugins/config/stabilization.ui | 1264 +++++++++-------- 1 file changed, 652 insertions(+), 612 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/stabilization.ui b/ground/openpilotgcs/src/plugins/config/stabilization.ui index 96c414055..799c50880 100644 --- a/ground/openpilotgcs/src/plugins/config/stabilization.ui +++ b/ground/openpilotgcs/src/plugins/config/stabilization.ui @@ -6,649 +6,689 @@ 0 0 - 639 - 611 + 683 + 685 Form - + - + - + 0 - 0 + 1 - - - 0 - 150 - + + QFrame::NoFrame - - Rate Stabilization Coefficients (Inner Loop) + + true - - - - - Kp - - - Qt::AlignCenter - - - - - - - Ki - - - Qt::AlignCenter - - - - - - - ILimit - - - Qt::AlignCenter - - - - - - - Roll - - - - - - - Slowly raise Kp until you start seeing clear oscillations when you fly. + + + + 0 + 0 + 665 + 627 + + + + + 0 + 0 + + + + + 0 + + + 0 + + + + + + 0 + 0 + + + + + 0 + 150 + + + + Rate Stabilization Coefficients (Inner Loop) + + + + + + Kp + + + Qt::AlignCenter + + + + + + + Ki + + + Qt::AlignCenter + + + + + + + ILimit + + + Qt::AlignCenter + + + + + + + Roll + + + + + + + Slowly raise Kp until you start seeing clear oscillations when you fly. Then lower the value by 20% or so. - - - 6 - - - 0.000100000000000 - - - - - - - I factor for rate stabilization is usually very low or even zero. - - - 6 - - - 0.000100000000000 - - - - - - - 6 - - - 0.000100000000000 - - - - - - - If checked, the Roll and Pitch factors will be identical. + + + 6 + + + 0.000100000000000 + + + + + + + I factor for rate stabilization is usually very low or even zero. + + + 6 + + + 0.000100000000000 + + + + + + + 6 + + + 0.000100000000000 + + + + + + + If checked, the Roll and Pitch factors will be identical. When you change one, the other is updated. - - - Link - - - - - - - Pitch - - - - - - - Slowly raise Kp until you start seeing clear oscillations when you fly. + + + Link + + + + + + + Pitch + + + + + + + Slowly raise Kp until you start seeing clear oscillations when you fly. Then lower the value by 20% or so. - - - 6 - - - 0.000100000000000 - - - - - - - I factor for rate stabilization is usually very low or even zero. - - - 6 - - - 0.000100000000000 - - - - - - - 6 - - - 0.000100000000000 - - - - - - - Yaw - - - - - - - Slowly raise Kp until you start seeing clear oscillations when you fly. + + + 6 + + + 0.000100000000000 + + + + + + + I factor for rate stabilization is usually very low or even zero. + + + 6 + + + 0.000100000000000 + + + + + + + 6 + + + 0.000100000000000 + + + + + + + Yaw + + + + + + + Slowly raise Kp until you start seeing clear oscillations when you fly. Then lower the value by 20% or so. You can usually go for higher values for Yaw factors. - - - 6 - - - 0.000100000000000 - - - - - - - As a rule of thumb, you can set YawRate Ki at roughly the same + + + 6 + + + 0.000100000000000 + + + + + + + As a rule of thumb, you can set YawRate Ki at roughly the same value as YawRate Kp. - - - 6 - - - 0.000100000000000 - - - - - - - 6 - - - 0.000100000000000 - - - - - - - - - - Qt::Vertical - - - QSizePolicy::Fixed - - - - 20 - 5 - - - - - - - - - 0 - 0 - - - - - 0 - 150 - - - - Attitude Stabization Coefficients (Outer Loop) - - - - - - Once Rate stabilization is done, you should increase the Kp factor until the airframe oscillates again, and go back down 20% or so. + + + 6 + + + 0.000100000000000 + + + + + + + 6 + + + 0.000100000000000 + + + + + + + + + + Qt::Vertical + + + QSizePolicy::Fixed + + + + 20 + 13 + + + + + + + + + 0 + 0 + + + + + 0 + 150 + + + + Attitude Stabization Coefficients (Outer Loop) + + + + + + Once Rate stabilization is done, you should increase the Kp factor until the airframe oscillates again, and go back down 20% or so. - - - 6 - - - 0.100000000000000 - - - - - - - Ki can usually be almost identical to Kp. - - - 6 - - - 0.100000000000000 - - - - - - - ILimit can be equal to three to four times Ki, but you can adjust + + + 6 + + + 0.100000000000000 + + + + + + + Ki can usually be almost identical to Kp. + + + 6 + + + 0.100000000000000 + + + + + + + ILimit can be equal to three to four times Ki, but you can adjust depending on whether your airframe is well balanced, and your flying style. - - - 6 - - - 0.100000000000000 - - - - - - - Kp - - - Qt::AlignCenter - - - - - - - Ki - - - Qt::AlignCenter - - - - - - - ILimit - - - Qt::AlignCenter - - - - - - - ILimit can be equal to three to four times Ki, but you can adjust + + + 6 + + + 0.100000000000000 + + + + + + + Kp + + + Qt::AlignCenter + + + + + + + Ki + + + Qt::AlignCenter + + + + + + + ILimit + + + Qt::AlignCenter + + + + + + + ILimit can be equal to three to four times Ki, but you can adjust depending on whether your airframe is well balanced, and your flying style. - - - 6 - - - 0.100000000000000 - - - - - - - Ki can usually be almost identical to Kp. - - - 6 - - - 0.100000000000000 - - - - - - - Once Rate stabilization is done, you should increase the Kp factor until the airframe oscillates again, and go back down 20% or so. + + + 6 + + + 0.100000000000000 + + + + + + + Ki can usually be almost identical to Kp. + + + 6 + + + 0.100000000000000 + + + + + + + Once Rate stabilization is done, you should increase the Kp factor until the airframe oscillates again, and go back down 20% or so. - - - 6 - - - 0.100000000000000 - - - - - - - Once Rate stabilization is done, you should increase the Kp factor until the airframe oscillates again, and go back down 20% or so. + + + 6 + + + 0.100000000000000 + + + + + + + Once Rate stabilization is done, you should increase the Kp factor until the airframe oscillates again, and go back down 20% or so. - - - 6 - - - 0.100000000000000 - - - - - - - Yaw - - - - - - - Pitch - - - - - - - Roll - - - - - - - Ki can usually be almost identical to Kp. - - - 6 - - - 0.100000000000000 - - - - - - - ILimit can be equal to three to four times Ki, but you can adjust + + + 6 + + + 0.100000000000000 + + + + + + + Yaw + + + + + + + Pitch + + + + + + + Roll + + + + + + + Ki can usually be almost identical to Kp. + + + 6 + + + 0.100000000000000 + + + + + + + ILimit can be equal to three to four times Ki, but you can adjust depending on whether your airframe is well balanced, and your flying style. - - - 6 - - - 0.100000000000000 - - - - - - - If checked, the Roll and Pitch factors will be identical. + + + 6 + + + 0.100000000000000 + + + + + + + If checked, the Roll and Pitch factors will be identical. When you change one, the other is updated. - - - Link - - - - + + + Link + + + + + + + + + + Qt::Vertical + + + QSizePolicy::Fixed + + + + 20 + 13 + + + + + + + + + 0 + 0 + + + + + 0 + 0 + + + + Stick range and limits + + + + QLayout::SetMinAndMaxSize + + + + + Roll + + + Qt::AlignCenter + + + + + + + Pitch + + + Qt::AlignCenter + + + + + + + Yaw + + + Qt::AlignCenter + + + + + + + 180 + + + + + + + 180 + + + + + + + 180 + + + + + + + + 150 + 0 + + + + + 50 + false + + + + Full stick angle (deg) + + + + + + + + 150 + 0 + + + + + 50 + false + + + + Full stick rate (deg/s) + + + + + + + 300 + + + + + + + 300 + + + + + + + 300 + + + + + + + + 150 + 0 + + + + + 50 + false + + + + + + + Maximum rate in attitude mode (deg/s) + + + + + + + 300 + + + + + + + 300 + + + + + + + 300 + + + + + + + + + + Zero the integral when throttle is low + + + + + + + Qt::Vertical + + + + 20 + 0 + + + + + + - - - - Qt::Vertical - - - QSizePolicy::Fixed - - - - 20 - 5 - - - - - - - - - 0 - 0 - - - - - 0 - 0 - - - - Stick range and limits - - - - QLayout::SetMinAndMaxSize - - - - - Roll - - - Qt::AlignCenter - - - - - - - Pitch - - - Qt::AlignCenter - - - - - - - Yaw - - - Qt::AlignCenter - - - - - - - 180 - - - - - - - 180 - - - - - - - 180 - - - - - - - - 150 - 0 - - - - - 50 - false - - - - Full stick angle (deg) - - - - - - - - 150 - 0 - - - - - 50 - false - - - - Full stick rate (deg/s) - - - - - - - 300 - - - - - - - 300 - - - - - - - 300 - - - - - - - - 150 - 0 - - - - - 50 - false - - - - - - - Maximum rate in attitude mode (deg/s) - - - - - - - 300 - - - - - - - 300 - - - - - - - 300 - - - - - - - - - - Zero the integral when throttle is low - - - - - - - Qt::Vertical - - - - 20 - 40 - - - - From bbc137eabe0dc5ac62afa682710e4b8535dd415d Mon Sep 17 00:00:00 2001 From: Dmytro Poplavskiy Date: Sun, 21 Aug 2011 21:36:41 +1000 Subject: [PATCH 072/145] Added initial CachedSvgItem implementation Texture is regenerated each time item is scaled but it's reused during rotation, unlike the default DeviceCoordinateCache mode. --- .../src/libs/utils/cachedsvgitem.cpp | 155 ++++++++++++++++++ .../src/libs/utils/cachedsvgitem.h | 52 ++++++ ground/openpilotgcs/src/libs/utils/utils.pro | 10 +- 3 files changed, 214 insertions(+), 3 deletions(-) create mode 100644 ground/openpilotgcs/src/libs/utils/cachedsvgitem.cpp create mode 100644 ground/openpilotgcs/src/libs/utils/cachedsvgitem.h diff --git a/ground/openpilotgcs/src/libs/utils/cachedsvgitem.cpp b/ground/openpilotgcs/src/libs/utils/cachedsvgitem.cpp new file mode 100644 index 000000000..4fc80e6d2 --- /dev/null +++ b/ground/openpilotgcs/src/libs/utils/cachedsvgitem.cpp @@ -0,0 +1,155 @@ +/** + ****************************************************************************** + * + * @file cachedsvgitem.h + * @author Dmytro Poplavskiy Copyright (C) 2011. + * @{ + * @brief OpenGL texture cached SVG item + *****************************************************************************/ +/* + * 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 "cachedsvgitem.h" +#include +#include + +CachedSvgItem::CachedSvgItem(QGraphicsItem * parent) : + QGraphicsSvgItem(parent), + m_context(0), + m_texture(0), + m_scale(1.0) +{ + setCacheMode(NoCache); +} + +CachedSvgItem::CachedSvgItem(const QString & fileName, QGraphicsItem * parent): + QGraphicsSvgItem(fileName, parent), + m_context(0), + m_texture(0), + m_scale(1.0) +{ + setCacheMode(NoCache); +} + +CachedSvgItem::~CachedSvgItem() +{ + if (m_context && m_texture) { + m_context->makeCurrent(); + glDeleteTextures(1, &m_texture); + } +} + +void CachedSvgItem::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget) +{ + + if (painter->paintEngine()->type() != QPaintEngine::OpenGL && + painter->paintEngine()->type() != QPaintEngine::OpenGL2) { + //Fallback to direct painting + QGraphicsSvgItem::paint(painter, option, widget); + return; + } + + QRectF br = boundingRect(); + QTransform transform = painter->worldTransform(); + qreal sceneScale = transform.map(QLineF(0,0,1,0)).length(); + + bool stencilTestEnabled = glIsEnabled(GL_STENCIL_TEST); + bool scissorTestEnabled = glIsEnabled(GL_SCISSOR_TEST); + + painter->beginNativePainting(); + + if (stencilTestEnabled) + glEnable(GL_STENCIL_TEST); + if (scissorTestEnabled) + glEnable(GL_SCISSOR_TEST); + + bool dirty = false; + if (!m_texture) { + glGenTextures(1, &m_texture); + m_context = const_cast(QGLContext::currentContext()); + + dirty = true; + } + + if (!qFuzzyCompare(sceneScale, m_scale)) { + m_scale = sceneScale; + dirty = true; + } + + int textureWidth = (int(br.width()*m_scale) + 3) & ~3; + int textureHeight = (int(br.height()*m_scale) + 3) & ~3; + + if (dirty) { + //qDebug() << "re-render image"; + + QImage img(textureWidth, textureHeight, QImage::Format_ARGB32); + { + img.fill(Qt::transparent); + QPainter p; + p.begin(&img); + p.setRenderHints(painter->renderHints()); + p.translate(br.topLeft()); + p.scale(m_scale, m_scale); + QGraphicsSvgItem::paint(&p, option, 0); + p.end(); + + img = img.rgbSwapped(); + } + + glEnable(GL_TEXTURE_2D); + + glBindTexture(GL_TEXTURE_2D, m_texture); + glTexImage2D( + GL_TEXTURE_2D, + 0, + GL_RGBA, + textureWidth, + textureHeight, + 0, + GL_RGBA, + GL_UNSIGNED_BYTE, + img.bits()); + + glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); + glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); + glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE); + glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE); + + glDisable(GL_TEXTURE_2D); + + dirty = false; + } + + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + glEnable(GL_TEXTURE_2D); + + glBindTexture(GL_TEXTURE_2D, m_texture); + + //texture may be slightly large than svn image, ensure only used area is rendered + qreal tw = br.width()*m_scale/textureWidth; + qreal th = br.height()*m_scale/textureHeight; + + glBegin(GL_QUADS); + glTexCoord2d(0, 0 ); glVertex3d(br.left(), br.top(), -1); + glTexCoord2d(tw, 0 ); glVertex3d(br.right(), br.top(), -1); + glTexCoord2d(tw, th); glVertex3d(br.right(), br.bottom(), -1); + glTexCoord2d(0, th); glVertex3d(br.left(), br.bottom(), -1); + glEnd(); + glDisable(GL_TEXTURE_2D); + + painter->endNativePainting(); +} diff --git a/ground/openpilotgcs/src/libs/utils/cachedsvgitem.h b/ground/openpilotgcs/src/libs/utils/cachedsvgitem.h new file mode 100644 index 000000000..19f9fd4eb --- /dev/null +++ b/ground/openpilotgcs/src/libs/utils/cachedsvgitem.h @@ -0,0 +1,52 @@ +/** + ****************************************************************************** + * + * @file cachedsvgitem.h + * @author Dmytro Poplavskiy Copyright (C) 2011. + * @{ + * @brief OpenGL texture cached SVG item + *****************************************************************************/ +/* + * 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 CACHEDSVGITEM_H +#define CACHEDSVGITEM_H + +#include +#include + +class QGLContext; + +//Cache Svg item as GL Texture. +//Texture is regenerated each time item is scaled +//but it's reused during rotation, unlike DeviceCoordinateCache mode +class CachedSvgItem: public QGraphicsSvgItem +{ + Q_OBJECT +public: + CachedSvgItem(QGraphicsItem * parent = 0); + CachedSvgItem(const QString & fileName, QGraphicsItem * parent = 0); + ~CachedSvgItem(); + + void paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget); + +private: + QGLContext *m_context; + GLuint m_texture; + qreal m_scale; +}; + +#endif diff --git a/ground/openpilotgcs/src/libs/utils/utils.pro b/ground/openpilotgcs/src/libs/utils/utils.pro index 804f7c813..003497b4d 100644 --- a/ground/openpilotgcs/src/libs/utils/utils.pro +++ b/ground/openpilotgcs/src/libs/utils/utils.pro @@ -3,7 +3,9 @@ TARGET = Utils QT += gui \ network \ - xml + xml \ + svg \ + opengl DEFINES += QTCREATOR_UTILS_LIB @@ -48,7 +50,8 @@ SOURCES += reloadpromptutils.cpp \ homelocationutil.cpp \ mytabbedstackwidget.cpp \ mytabwidget.cpp \ - mylistwidget.cpp + mylistwidget.cpp \ + cachedsvgitem.cpp SOURCES += xmlconfig.cpp win32 { @@ -102,7 +105,8 @@ HEADERS += utils_global.h \ homelocationutil.h \ mytabbedstackwidget.h \ mytabwidget.h \ - mylistwidget.h + mylistwidget.h \ + cachedsvgitem.h HEADERS += xmlconfig.h FORMS += filewizardpage.ui \ From 6b0da6bc72fc8ea5125a850dfe277e9a20856033 Mon Sep 17 00:00:00 2001 From: Dmytro Poplavskiy Date: Sun, 21 Aug 2011 21:38:58 +1000 Subject: [PATCH 073/145] Changed PFDGadgetWidget to use CachedSvgItem --- .../src/plugins/pfd/pfdgadgetwidget.cpp | 41 ++++++++++--------- 1 file changed, 21 insertions(+), 20 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/pfd/pfdgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/pfd/pfdgadgetwidget.cpp index 079a206b5..a38e943a3 100644 --- a/ground/openpilotgcs/src/plugins/pfd/pfdgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/pfd/pfdgadgetwidget.cpp @@ -27,6 +27,7 @@ #include "pfdgadgetwidget.h" #include +#include #include #include #include @@ -383,7 +384,7 @@ void PFDGadgetWidget::setDialFile(QString dfn) - Battery stats: battery-txt */ l_scene->clear(); // Deletes all items contained in the scene as well. - m_background = new QGraphicsSvgItem(); + m_background = new CachedSvgItem(); // All other items will be clipped to the shape of the background m_background->setFlags(QGraphicsItem::ItemClipsChildrenToShape| QGraphicsItem::ItemClipsToShape); @@ -391,28 +392,28 @@ void PFDGadgetWidget::setDialFile(QString dfn) m_background->setElementId("background"); l_scene->addItem(m_background); - m_world = new QGraphicsSvgItem(); + m_world = new CachedSvgItem(); m_world->setParentItem(m_background); m_world->setSharedRenderer(m_renderer); m_world->setElementId("world"); l_scene->addItem(m_world); // red Roll scale: rollscale - m_rollscale = new QGraphicsSvgItem(); + m_rollscale = new CachedSvgItem(); m_rollscale->setSharedRenderer(m_renderer); m_rollscale->setElementId("rollscale"); l_scene->addItem(m_rollscale); // Home point: - m_homewaypoint = new QGraphicsSvgItem(); + m_homewaypoint = new CachedSvgItem(); // Next point: - m_nextwaypoint = new QGraphicsSvgItem(); + m_nextwaypoint = new CachedSvgItem(); // Home point bearing: - m_homepointbearing = new QGraphicsSvgItem(); + m_homepointbearing = new CachedSvgItem(); // Next point bearing: - m_nextpointbearing = new QGraphicsSvgItem(); + m_nextpointbearing = new CachedSvgItem(); - QGraphicsSvgItem *m_foreground = new QGraphicsSvgItem(); + QGraphicsSvgItem *m_foreground = new CachedSvgItem(); m_foreground->setParentItem(m_background); m_foreground->setSharedRenderer(m_renderer); m_foreground->setElementId("foreground"); @@ -429,7 +430,7 @@ void PFDGadgetWidget::setDialFile(QString dfn) // into a QGraphicsSvgItem which we will display at the same // place: we do this so that the heading scale can be clipped to // the compass dial region. - m_compass = new QGraphicsSvgItem(); + m_compass = new CachedSvgItem(); m_compass->setSharedRenderer(m_renderer); m_compass->setElementId("compass"); m_compass->setFlags(QGraphicsItem::ItemClipsChildrenToShape| @@ -440,7 +441,7 @@ void PFDGadgetWidget::setDialFile(QString dfn) m_compass->setTransform(matrix,false); // Now place the compass scale inside: - m_compassband = new QGraphicsSvgItem(); + m_compassband = new CachedSvgItem(); m_compassband->setSharedRenderer(m_renderer); m_compassband->setElementId("compass-band"); m_compassband->setParentItem(m_compass); @@ -462,7 +463,7 @@ void PFDGadgetWidget::setDialFile(QString dfn) compassMatrix = m_renderer->matrixForElement("speed-bg"); startX = compassMatrix.mapRect(m_renderer->boundsOnElement("speed-bg")).x(); startY = compassMatrix.mapRect(m_renderer->boundsOnElement("speed-bg")).y(); - QGraphicsSvgItem *verticalbg = new QGraphicsSvgItem(); + QGraphicsSvgItem *verticalbg = new CachedSvgItem(); verticalbg->setSharedRenderer(m_renderer); verticalbg->setElementId("speed-bg"); verticalbg->setFlags(QGraphicsItem::ItemClipsChildrenToShape| @@ -477,7 +478,7 @@ void PFDGadgetWidget::setDialFile(QString dfn) m_speedscale = new QGraphicsItemGroup(); m_speedscale->setParentItem(verticalbg); - QGraphicsSvgItem *speedscalelines = new QGraphicsSvgItem(); + QGraphicsSvgItem *speedscalelines = new CachedSvgItem(); speedscalelines->setSharedRenderer(m_renderer); speedscalelines->setElementId("speed-scale"); speedScaleHeight = m_renderer->matrixForElement("speed-scale").mapRect( @@ -523,7 +524,7 @@ void PFDGadgetWidget::setDialFile(QString dfn) startX = compassMatrix.mapRect(m_renderer->boundsOnElement("speed-window")).x(); startY = compassMatrix.mapRect(m_renderer->boundsOnElement("speed-window")).y(); qreal speedWindowHeight = compassMatrix.mapRect(m_renderer->boundsOnElement("speed-window")).height(); - QGraphicsSvgItem *speedwindow = new QGraphicsSvgItem(); + QGraphicsSvgItem *speedwindow = new CachedSvgItem(); speedwindow->setSharedRenderer(m_renderer); speedwindow->setElementId("speed-window"); speedwindow->setFlags(QGraphicsItem::ItemClipsChildrenToShape| @@ -548,7 +549,7 @@ void PFDGadgetWidget::setDialFile(QString dfn) compassMatrix = m_renderer->matrixForElement("altitude-bg"); startX = compassMatrix.mapRect(m_renderer->boundsOnElement("altitude-bg")).x(); startY = compassMatrix.mapRect(m_renderer->boundsOnElement("altitude-bg")).y(); - verticalbg = new QGraphicsSvgItem(); + verticalbg = new CachedSvgItem(); verticalbg->setSharedRenderer(m_renderer); verticalbg->setElementId("altitude-bg"); verticalbg->setFlags(QGraphicsItem::ItemClipsChildrenToShape| @@ -563,7 +564,7 @@ void PFDGadgetWidget::setDialFile(QString dfn) m_altitudescale = new QGraphicsItemGroup(); m_altitudescale->setParentItem(verticalbg); - QGraphicsSvgItem *altitudescalelines = new QGraphicsSvgItem(); + QGraphicsSvgItem *altitudescalelines = new CachedSvgItem(); altitudescalelines->setSharedRenderer(m_renderer); altitudescalelines->setElementId("altitude-scale"); altitudeScaleHeight = m_renderer->matrixForElement("altitude-scale").mapRect( @@ -604,7 +605,7 @@ void PFDGadgetWidget::setDialFile(QString dfn) startX = compassMatrix.mapRect(m_renderer->boundsOnElement("altitude-window")).x(); startY = compassMatrix.mapRect(m_renderer->boundsOnElement("altitude-window")).y(); qreal altitudeWindowHeight = compassMatrix.mapRect(m_renderer->boundsOnElement("altitude-window")).height(); - QGraphicsSvgItem *altitudewindow = new QGraphicsSvgItem(); + QGraphicsSvgItem *altitudewindow = new CachedSvgItem(); altitudewindow->setSharedRenderer(m_renderer); altitudewindow->setElementId("altitude-window"); altitudewindow->setFlags(QGraphicsItem::ItemClipsChildrenToShape| @@ -633,7 +634,7 @@ void PFDGadgetWidget::setDialFile(QString dfn) compassMatrix = m_renderer->matrixForElement("gcstelemetry-Disconnected"); startX = compassMatrix.mapRect(m_renderer->boundsOnElement("gcstelemetry-Disconnected")).x(); startY = compassMatrix.mapRect(m_renderer->boundsOnElement("gcstelemetry-Disconnected")).y(); - gcsTelemetryArrow = new QGraphicsSvgItem(); + gcsTelemetryArrow = new CachedSvgItem(); gcsTelemetryArrow->setSharedRenderer(m_renderer); gcsTelemetryArrow->setElementId("gcstelemetry-Disconnected"); l_scene->addItem(gcsTelemetryArrow); @@ -669,7 +670,7 @@ void PFDGadgetWidget::setDialFile(QString dfn) compassMatrix = m_renderer->matrixForElement("gcstelemetry-Disconnected"); startX = compassMatrix.mapRect(m_renderer->boundsOnElement("gcstelemetry-Disconnected")).x(); startY = compassMatrix.mapRect(m_renderer->boundsOnElement("gcstelemetry-Disconnected")).y(); - gcsTelemetryArrow = new QGraphicsSvgItem(); + gcsTelemetryArrow = new CachedSvgItem(); gcsTelemetryArrow->setSharedRenderer(m_renderer); gcsTelemetryArrow->setElementId("gcstelemetry-Disconnected"); l_scene->addItem(gcsTelemetryArrow); @@ -702,7 +703,7 @@ void PFDGadgetWidget::setDialFile(QString dfn) compassMatrix = m_renderer->matrixForElement("gcstelemetry-Disconnected"); startX = compassMatrix.mapRect(m_renderer->boundsOnElement("gcstelemetry-Disconnected")).x(); startY = compassMatrix.mapRect(m_renderer->boundsOnElement("gcstelemetry-Disconnected")).y(); - gcsTelemetryArrow = new QGraphicsSvgItem(); + gcsTelemetryArrow = new CachedSvgItem(); gcsTelemetryArrow->setSharedRenderer(m_renderer); gcsTelemetryArrow->setElementId("gcstelemetry-Disconnected"); l_scene->addItem(gcsTelemetryArrow); @@ -771,7 +772,7 @@ void PFDGadgetWidget::setDialFile(QString dfn) { qDebug()<<"Error on PFD artwork file."; m_renderer->load(QString(":/pfd/images/pfd-default.svg")); l_scene->clear(); // This also deletes all items contained in the scene. - m_background = new QGraphicsSvgItem(); + m_background = new CachedSvgItem(); m_background->setSharedRenderer(m_renderer); l_scene->addItem(m_background); pfdError = true; From 69d5e7fe7e9d22ab62783c0065f549465101c247 Mon Sep 17 00:00:00 2001 From: Dmytro Poplavskiy Date: Sun, 21 Aug 2011 23:40:11 +1000 Subject: [PATCH 074/145] Added missing Q_DECL_EXPORT to CachedSvgItem --- ground/openpilotgcs/src/libs/utils/cachedsvgitem.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/ground/openpilotgcs/src/libs/utils/cachedsvgitem.h b/ground/openpilotgcs/src/libs/utils/cachedsvgitem.h index 19f9fd4eb..747ef391c 100644 --- a/ground/openpilotgcs/src/libs/utils/cachedsvgitem.h +++ b/ground/openpilotgcs/src/libs/utils/cachedsvgitem.h @@ -28,12 +28,14 @@ #include #include +#include "utils_global.h" + class QGLContext; //Cache Svg item as GL Texture. //Texture is regenerated each time item is scaled //but it's reused during rotation, unlike DeviceCoordinateCache mode -class CachedSvgItem: public QGraphicsSvgItem +class QTCREATOR_UTILS_EXPORT CachedSvgItem: public QGraphicsSvgItem { Q_OBJECT public: From 0ce337b42d44ff42602d67befdfd5a50b343f5ad Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 21 Aug 2011 18:15:10 +0200 Subject: [PATCH 075/145] UAVTalk: fix off by one error preventing the largest UAvObject (gpssatellites) from being sent. --- flight/UAVTalk/inc/uavtalk_priv.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/UAVTalk/inc/uavtalk_priv.h b/flight/UAVTalk/inc/uavtalk_priv.h index 1bcf01fd9..06f08b997 100644 --- a/flight/UAVTalk/inc/uavtalk_priv.h +++ b/flight/UAVTalk/inc/uavtalk_priv.h @@ -51,7 +51,7 @@ typedef struct { typedef uint8_t uavtalk_checksum; #define UAVTALK_CHECKSUM_LENGTH sizeof(uavtalk_checksum) -#define UAVTALK_MAX_PAYLOAD_LENGTH UAVOBJECTS_LARGEST +#define UAVTALK_MAX_PAYLOAD_LENGTH (UAVOBJECTS_LARGEST + 1) #define UAVTALK_MIN_PACKET_LENGTH UAVTALK_MAX_HEADER_LENGTH + UAVTALK_CHECKSUM_LENGTH #define UAVTALK_MAX_PACKET_LENGTH UAVTALK_MIN_PACKET_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH From 838149b53fcd19781723e94478549b52164a3e9e Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 21 Aug 2011 14:23:07 +0200 Subject: [PATCH 076/145] GPS: homelocation init is optional, satellites must be initialized --- flight/Modules/GPS/GPS.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/flight/Modules/GPS/GPS.c b/flight/Modules/GPS/GPS.c index 60ff80184..9bb873654 100644 --- a/flight/Modules/GPS/GPS.c +++ b/flight/Modules/GPS/GPS.c @@ -46,6 +46,7 @@ #include "gpsposition.h" #include "homelocation.h" #include "gpstime.h" +#include "gpssatellites.h" #include "WorldMagModel.h" #include "CoordinateConversions.h" @@ -126,7 +127,10 @@ int32_t GPSInitialize(void) { GPSPositionInitialize(); GPSTimeInitialize(); + GPSSatellitesInitialize(); +#ifdef PIOS_GPS_SETS_HOMELOCATION HomeLocationInitialize(); +#endif // TODO: Get gps settings object gpsPort = PIOS_COM_GPS; From 6e506161e0975915aaa536ff6ca4eb1410b27c9b Mon Sep 17 00:00:00 2001 From: Dmytro Poplavskiy Date: Mon, 22 Aug 2011 06:59:34 +1000 Subject: [PATCH 077/145] Compilation fix It's necessary to include GL/glext.h on windows for GL_CLAMP_TO_EDGE define. --- ground/openpilotgcs/src/libs/utils/cachedsvgitem.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ground/openpilotgcs/src/libs/utils/cachedsvgitem.cpp b/ground/openpilotgcs/src/libs/utils/cachedsvgitem.cpp index 4fc80e6d2..93a38afb1 100644 --- a/ground/openpilotgcs/src/libs/utils/cachedsvgitem.cpp +++ b/ground/openpilotgcs/src/libs/utils/cachedsvgitem.cpp @@ -26,6 +26,8 @@ #include #include +#include + CachedSvgItem::CachedSvgItem(QGraphicsItem * parent) : QGraphicsSvgItem(parent), m_context(0), From ed51756191782579fd6811e986a74a61fe252ea2 Mon Sep 17 00:00:00 2001 From: Dmytro Poplavskiy Date: Mon, 22 Aug 2011 09:10:25 +1000 Subject: [PATCH 078/145] Compilation fix on mac It's safer to define GL_CLAMP_TO_EDGE if not available than to include GL/glext.h. --- ground/openpilotgcs/src/libs/utils/cachedsvgitem.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/ground/openpilotgcs/src/libs/utils/cachedsvgitem.cpp b/ground/openpilotgcs/src/libs/utils/cachedsvgitem.cpp index 93a38afb1..2edef8ddf 100644 --- a/ground/openpilotgcs/src/libs/utils/cachedsvgitem.cpp +++ b/ground/openpilotgcs/src/libs/utils/cachedsvgitem.cpp @@ -26,7 +26,9 @@ #include #include -#include +#ifndef GL_CLAMP_TO_EDGE +#define GL_CLAMP_TO_EDGE 0x812F +#endif CachedSvgItem::CachedSvgItem(QGraphicsItem * parent) : QGraphicsSvgItem(parent), From edc0caf5215f32bfc4ad69a56a391a7626f561a6 Mon Sep 17 00:00:00 2001 From: Stacey Sheldon Date: Sun, 21 Aug 2011 23:14:32 -0400 Subject: [PATCH 079/145] heap: set memory critical alarm on malloc failures Activate the FreeRTOS malloc failed hook and use it to set the memory critical alarm. --- .../CopterControl/System/inc/FreeRTOSConfig.h | 1 + flight/Modules/System/systemmod.c | 37 +++++++++++++++++-- 2 files changed, 34 insertions(+), 4 deletions(-) diff --git a/flight/CopterControl/System/inc/FreeRTOSConfig.h b/flight/CopterControl/System/inc/FreeRTOSConfig.h index 8da6a8667..994956008 100644 --- a/flight/CopterControl/System/inc/FreeRTOSConfig.h +++ b/flight/CopterControl/System/inc/FreeRTOSConfig.h @@ -26,6 +26,7 @@ #define configUSE_PREEMPTION 1 #define configUSE_IDLE_HOOK 1 #define configUSE_TICK_HOOK 0 +#define configUSE_MALLOC_FAILED_HOOK 1 #define configCPU_CLOCK_HZ ( ( unsigned long ) 72000000 ) #define configTICK_RATE_HZ ( ( portTickType ) 1000 ) #define configMAX_PRIORITIES ( ( unsigned portBASE_TYPE ) 5 ) diff --git a/flight/Modules/System/systemmod.c b/flight/Modules/System/systemmod.c index b4f959f2c..168b46aa6 100644 --- a/flight/Modules/System/systemmod.c +++ b/flight/Modules/System/systemmod.c @@ -75,7 +75,8 @@ static uint32_t idleCounter; static uint32_t idleCounterClear; static xTaskHandle systemTaskHandle; -static int32_t stackOverflow; +static bool stackOverflow; +static bool mallocFailed; // Private functions static void objectUpdatedCb(UAVObjEvent * ev); @@ -93,7 +94,8 @@ static void updateWDGstats(); int32_t SystemModStart(void) { // Initialize vars - stackOverflow = 0; + stackOverflow = false; + mallocFailed = false; // Create system task xTaskCreate(systemTask, (signed char *)"System", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &systemTaskHandle); // Register task @@ -417,12 +419,19 @@ static void updateSystemAlarms() } // Check for stack overflow - if (stackOverflow == 1) { + if (stackOverflow) { AlarmsSet(SYSTEMALARMS_ALARM_STACKOVERFLOW, SYSTEMALARMS_ALARM_CRITICAL); } else { AlarmsClear(SYSTEMALARMS_ALARM_STACKOVERFLOW); } + // Check for malloc failures + if (mallocFailed) { + AlarmsSet(SYSTEMALARMS_ALARM_OUTOFMEMORY, SYSTEMALARMS_ALARM_CRITICAL); + } else { + AlarmsClear(SYSTEMALARMS_ALARM_OUTOFMEMORY); + } + #if defined(PIOS_INCLUDE_SDCARD) // Check for SD card if (PIOS_SDCARD_IsMounted() == 0) { @@ -461,9 +470,29 @@ void vApplicationIdleHook(void) /** * Called by the RTOS when a stack overflow is detected. */ +#define DEBUG_STACK_OVERFLOW 0 void vApplicationStackOverflowHook(xTaskHandle * pxTask, signed portCHAR * pcTaskName) { - stackOverflow = 1; + stackOverflow = true; +#if DEBUG_STACK_OVERFLOW + static volatile bool wait_here = true; + while(wait_here); + wait_here = true; +#endif +} + +/** + * Called by the RTOS when a malloc call fails. + */ +#define DEBUG_MALLOC_FAILURES 0 +void vApplicationMallocFailedHook(void) +{ + mallocFailed = true; +#if DEBUG_MALLOC_FAILURES + static volatile bool wait_here = true; + while(wait_here); + wait_here = true; +#endif } /** From 0d92e00125ca62882e078db6d8143dad5fcee3f1 Mon Sep 17 00:00:00 2001 From: zedamota Date: Tue, 23 Aug 2011 11:25:28 +0100 Subject: [PATCH 080/145] Several bugfixes. Created "simple wizard" button witch bypasses the sticks identification screen. Small bugfix to fix GCS crashing if trying to upload a firmware bigger then the HW capacity. --- .../plugins/config/configccattitudewidget.cpp | 2 - .../src/plugins/config/configinputwidget.cpp | 82 ++++++++++++++----- .../src/plugins/config/configinputwidget.h | 12 ++- .../src/plugins/config/configtaskwidget.cpp | 18 ++-- .../src/plugins/config/configtaskwidget.h | 1 + .../src/plugins/config/smartsavebutton.cpp | 53 ++++++------ .../src/plugins/config/smartsavebutton.h | 8 +- .../src/plugins/uploader/devicewidget.cpp | 5 +- .../src/plugins/uploader/op_dfu.cpp | 2 +- 9 files changed, 119 insertions(+), 64 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp index c2242f1ef..c659180af 100644 --- a/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configccattitudewidget.cpp @@ -45,8 +45,6 @@ ConfigCCAttitudeWidget::ConfigCCAttitudeWidget(QWidget *parent) : setupButtons(ui->applyButton,ui->saveButton); addUAVObject("AttitudeSettings"); - UAVObject * settings = AttitudeSettings::GetInstance(getObjectManager()); - // Connect the help button connect(ui->ccAttitudeHelp, SIGNAL(clicked()), this, SLOT(openHelp())); addUAVObjectToWidgetRelation("AttitudeSettings","ZeroDuringArming",ui->zeroGyroBiasOnArming); diff --git a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp index 4544b4edd..6313c45b1 100644 --- a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp @@ -71,7 +71,11 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) : ConfigTaskWidget(parent) } goWizard=new QPushButton(tr("Start Wizard"),this); m_config->advancedPage->layout()->addWidget(goWizard); - connect(goWizard,SIGNAL(clicked()),this,SLOT(goToWizard())); + connect(goWizard,SIGNAL(clicked()),this,SLOT(goToNormalWizard())); + goSimpleWizard=new QPushButton(tr("Start Simple Wizard"),this); + m_config->advancedPage->layout()->addWidget(goSimpleWizard); + connect(goSimpleWizard,SIGNAL(clicked()),this,SLOT(goToSimpleWizard())); + connect(m_config->wzNext,SIGNAL(clicked()),this,SLOT(wzNext())); connect(m_config->wzCancel,SIGNAL(clicked()),this,SLOT(wzCancel())); connect(m_config->wzBack,SIGNAL(clicked()),this,SLOT(wzBack())); @@ -92,9 +96,10 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) : ConfigTaskWidget(parent) addUAVObjectToWidgetRelation("ManualControlSettings","Stabilization3Settings",m_config->fmsSsPos3Yaw,"Yaw"); addUAVObjectToWidgetRelation("ManualControlSettings","Arming",m_config->armControl); - addUAVObjectToWidgetRelation("ManualControlSettings","armTimeout",m_config->armTimeout,0,1000); - addUAVObject("ManualControlCommand"); + addUAVObjectToWidgetRelation("ManualControlSettings","ArmedTimeout",m_config->armTimeout,0,1000); + connect( ManualControlCommand::GetInstance(getObjectManager()),SIGNAL(objectUpdated(UAVObject*)),this,SLOT(moveFMSlider())); addWidget(goWizard); + addWidget(goSimpleWizard); enableControls(false); populateWidgets(); @@ -252,7 +257,16 @@ void ConfigInputWidget::openHelp() QDesktopServices::openUrl( QUrl("http://wiki.openpilot.org/display/Doc/Input+Configuration", QUrl::StrictMode) ); } - +void ConfigInputWidget::goToSimpleWizard() +{ + isSimple=true; + goToWizard(); +} +void ConfigInputWidget::goToNormalWizard() +{ + isSimple=false; + goToWizard(); +} void ConfigInputWidget::goToWizard() { QMessageBox msgBox; @@ -357,7 +371,7 @@ void ConfigInputWidget::setupWizardWidget(int step) m_config->checkBoxesLayout->layout()->addWidget(mode2); wizardStep=wizardChooseMode; } - else if(step==wizardIdentifySticks) + else if(step==wizardIdentifySticks && !isSimple) { usedChannels.clear(); if(wizardStep==wizardChooseMode) @@ -380,8 +394,19 @@ void ConfigInputWidget::setupWizardWidget(int step) connect(receiverActivityObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(identifyControls())); m_config->wzNext->setEnabled(false); } - else if(step==wizardIdentifyCenter) + else if(step==wizardIdentifyCenter || (isSimple && step==wizardIdentifySticks)) { + if(wizardStep==wizardChooseMode) + { + QRadioButton * mode=qobject_cast(extraWidgets.at(0)); + if(mode->isChecked()) + transmitterMode=mode1; + else + transmitterMode=mode2; + delete extraWidgets.at(0); + delete extraWidgets.at(1); + extraWidgets.clear(); + } setTxMovement(centerAll); if(wizardStep==wizardIdentifySticks) disconnect(receiverActivityObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(identifyControls())); @@ -409,10 +434,17 @@ void ConfigInputWidget::setupWizardWidget(int step) } manualSettingsObj->setData(manualSettingsData); } - foreach (QWidget * wd, extraWidgets) + if(wizardStep==wizardIdentifyInverted) { - if(wd) - delete wd; + foreach(QWidget * wd,extraWidgets) + { + QCheckBox * cb=qobject_cast(wd); + if(cb) + { + disconnect(cb,SIGNAL(toggled(bool)),this,SLOT(invertControls())); + delete cb; + } + } } extraWidgets.clear(); disconnect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); @@ -423,7 +455,7 @@ void ConfigInputWidget::setupWizardWidget(int step) mdata.flightTelemetryUpdatePeriod = 150; manualCommandObj->setMetadata(mdata); manualSettingsData=manualSettingsObj->getData(); - for(int i=0;i(wd); if(cb) + { + disconnect(cb,SIGNAL(toggled(bool)),this,SLOT(invertControls())); delete cb; + } } wizardStep=wizardFinish; extraWidgets.clear(); @@ -537,7 +572,7 @@ void ConfigInputWidget::identifyControls() void ConfigInputWidget::identifyLimits() { manualCommandData=manualCommandObj->getData(); - for(int i=0;imanualCommandData.Channel[i]) manualSettingsData.ChannelMin[i]=manualCommandData.Channel[i]; @@ -878,7 +913,10 @@ void ConfigInputWidget::dimOtherControls(bool value) void ConfigInputWidget::enableControls(bool enable) { if(goWizard) + { goWizard->setEnabled(enable); + goSimpleWizard->setEnabled(enable); + } ConfigTaskWidget::enableControls(enable); } @@ -891,9 +929,10 @@ void ConfigInputWidget::invertControls() QCheckBox * cb=qobject_cast(wd); if(cb) { - if(cb->isChecked()) + int index=manualSettingsObj->getFields().at(0)->getElementNames().indexOf(cb->text()); + if((cb->isChecked() && (manualSettingsData.ChannelMax[index]>manualSettingsData.ChannelMin[index])) || + (!cb->isChecked() && (manualSettingsData.ChannelMax[index]getFields().at(0)->getElementNames().indexOf(cb->text()); qint16 aux; aux=manualSettingsData.ChannelMax[index]; manualSettingsData.ChannelMax[index]=manualSettingsData.ChannelMin[index]; @@ -903,20 +942,19 @@ void ConfigInputWidget::invertControls() } manualSettingsObj->setData(manualSettingsData); } -void ConfigInputWidget::refreshWidgetsValues() +void ConfigInputWidget::moveFMSlider() { - ConfigTaskWidget::refreshWidgetsValues(); - manualSettingsData = manualSettingsObj->getData(); - manualCommandData=manualCommandObj->getData(); - uint chIndex = manualSettingsData.ChannelNumber[ManualControlSettings::CHANNELNUMBER_FLIGHTMODE]; + ManualControlSettings::DataFields manualSettingsDataPriv = manualSettingsObj->getData(); + ManualControlCommand::DataFields manualCommandDataPriv=manualCommandObj->getData(); + uint chIndex = manualSettingsDataPriv.ChannelNumber[ManualControlSettings::CHANNELNUMBER_FLIGHTMODE]; if (chIndex < 8) { float valueScaled; - int chMin = manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_FLIGHTMODE]; - int chMax = manualSettingsData.ChannelMax[ManualControlSettings::CHANNELMAX_FLIGHTMODE]; - int chNeutral = manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_FLIGHTMODE]; + int chMin = manualSettingsDataPriv.ChannelMin[ManualControlSettings::CHANNELMIN_FLIGHTMODE]; + int chMax = manualSettingsDataPriv.ChannelMax[ManualControlSettings::CHANNELMAX_FLIGHTMODE]; + int chNeutral = manualSettingsDataPriv.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_FLIGHTMODE]; - int value = manualCommandData.Channel[chIndex]; + int value = manualCommandDataPriv.Channel[chIndex]; if ((chMax > chMin && value >= chNeutral) || (chMin > chMax && value <= chNeutral)) { if (chMax != chNeutral) diff --git a/ground/openpilotgcs/src/plugins/config/configinputwidget.h b/ground/openpilotgcs/src/plugins/config/configinputwidget.h index 7c90fef76..2249528cb 100644 --- a/ground/openpilotgcs/src/plugins/config/configinputwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configinputwidget.h @@ -82,7 +82,7 @@ private: QEventLoop * loop; bool skipflag; - int currentCommand; + uint currentCommand; ManualControlCommand * manualCommandObj; ManualControlCommand::DataFields manualCommandData; @@ -117,23 +117,27 @@ private: void resetTxControls(); void setMoveFromCommand(int command); QPushButton * goWizard; + QPushButton * goSimpleWizard; + bool isSimple; + void goToWizard(); private slots: void wzNext(); void wzBack(); void wzCancel(); - void goToWizard(); + void goToNormalWizard(); + void goToSimpleWizard(); void openHelp(); void identifyControls(); void identifyLimits(); void moveTxControls(); void moveSticks(); void dimOtherControls(bool value); + void moveFMSlider(); void invertControls(); protected: void resizeEvent(QResizeEvent *event); virtual void enableControls(bool enable); -protected slots: - virtual void refreshWidgetsValues(); + }; diff --git a/ground/openpilotgcs/src/plugins/config/configtaskwidget.cpp b/ground/openpilotgcs/src/plugins/config/configtaskwidget.cpp index 03b84804c..eeb6c08aa 100644 --- a/ground/openpilotgcs/src/plugins/config/configtaskwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configtaskwidget.cpp @@ -28,7 +28,7 @@ #include -ConfigTaskWidget::ConfigTaskWidget(QWidget *parent) : QWidget(parent),smartsave(NULL),dirty(false) +ConfigTaskWidget::ConfigTaskWidget(QWidget *parent) : QWidget(parent),isConnected(false),smartsave(NULL),dirty(false) { pm = ExtensionSystem::PluginManager::instance(); objManager = pm->getObject(); @@ -72,7 +72,7 @@ void ConfigTaskWidget::addUAVObjectToWidgetRelation(QString object, QString fiel ow->scale=scale; objOfInterest.append(ow); if(obj) - smartsave->addObject(obj); + smartsave->addObject((UAVDataObject*)obj); if(widget==NULL) { // do nothing @@ -148,13 +148,16 @@ double ConfigTaskWidget::listMean(QList list) void ConfigTaskWidget::onAutopilotDisconnect() { - enableControls(false); + isConnected=false; + enableControls(false); } void ConfigTaskWidget::onAutopilotConnect() { - enableControls(true); - refreshWidgetsValues(); + dirty=false; + isConnected=true; + enableControls(true); + refreshWidgetsValues(); } void ConfigTaskWidget::populateWidgets() @@ -262,7 +265,10 @@ void ConfigTaskWidget::setDirty(bool value) } bool ConfigTaskWidget::isDirty() { - return dirty; + if(isConnected) + return dirty; + else + return false; } void ConfigTaskWidget::refreshValues() diff --git a/ground/openpilotgcs/src/plugins/config/configtaskwidget.h b/ground/openpilotgcs/src/plugins/config/configtaskwidget.h index 1db0dde9c..b2a72cbc6 100644 --- a/ground/openpilotgcs/src/plugins/config/configtaskwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configtaskwidget.h @@ -79,6 +79,7 @@ private slots: virtual void refreshValues(); virtual void updateObjectsFromWidgets(); private: + bool isConnected; QStringList objectsList; QList objOfInterest; ExtensionSystem::PluginManager *pm; diff --git a/ground/openpilotgcs/src/plugins/config/smartsavebutton.cpp b/ground/openpilotgcs/src/plugins/config/smartsavebutton.cpp index 7335570eb..c74f22691 100644 --- a/ground/openpilotgcs/src/plugins/config/smartsavebutton.cpp +++ b/ground/openpilotgcs/src/plugins/config/smartsavebutton.cpp @@ -24,8 +24,11 @@ void smartSaveButton::processClick() bool error=false; ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectUtilManager* utilMngr = pm->getObject(); - foreach(UAVObject * obj,objects) + foreach(UAVDataObject * obj,objects) { + UAVObject::Metadata mdata= obj->getMetadata(); + if(mdata.gcsAccess==UAVObject::ACCESS_READONLY) + continue; up_result=false; current_object=obj; for(int i=0;i<3;++i) @@ -35,9 +38,9 @@ void smartSaveButton::processClick() connect(&timer,SIGNAL(timeout()),&loop,SLOT(quit())); obj->updated(); timer.start(1000); - qDebug()<<"begin loop"; + //qDebug()<<"begin loop"; loop.exec(); - qDebug()<<"end loop"; + //qDebug()<<"end loop"; timer.stop(); disconnect(obj,SIGNAL(transactionCompleted(UAVObject*,bool)),this,SLOT(transaction_finished(UAVObject*, bool))); disconnect(&timer,SIGNAL(timeout()),&loop,SLOT(quit())); @@ -46,30 +49,32 @@ void smartSaveButton::processClick() } if(up_result==false) { + //qDebug()<<"Object upload error:"<getName(); error=true; continue; } sv_result=false; current_objectID=obj->getObjID(); - if(save) + if(save && (obj->isSettings())) { - for(int i=0;i<3;++i) - { - connect(utilMngr,SIGNAL(saveCompleted(int,bool)),this,SLOT(saving_finished(int,bool))); - connect(&timer,SIGNAL(timeout()),&loop,SLOT(quit())); - utilMngr->saveObjectToSD(obj); - timer.start(1000); - loop.exec(); - timer.stop(); - disconnect(utilMngr,SIGNAL(saveCompleted(int,bool)),this,SLOT(saving_finished(int,bool))); - disconnect(&timer,SIGNAL(timeout()),&loop,SLOT(quit())); - if(sv_result) - break; - } - if(sv_result==false) - { - error=true; - } + for(int i=0;i<3;++i) + { + //qDebug()<<"try to save:"<getName(); + connect(utilMngr,SIGNAL(saveCompleted(int,bool)),this,SLOT(saving_finished(int,bool))); + connect(&timer,SIGNAL(timeout()),&loop,SLOT(quit())); + utilMngr->saveObjectToSD(obj); + timer.start(1000); + loop.exec(); + timer.stop(); + disconnect(utilMngr,SIGNAL(saveCompleted(int,bool)),this,SLOT(saving_finished(int,bool))); + disconnect(&timer,SIGNAL(timeout()),&loop,SLOT(quit())); + if(sv_result) + break; + } + if(sv_result==false) + { + error=true; + } } } button->setEnabled(true); @@ -85,12 +90,12 @@ void smartSaveButton::processClick() emit endOp(); } -void smartSaveButton::setObjects(QList list) +void smartSaveButton::setObjects(QList list) { objects=list; } -void smartSaveButton::addObject(UAVObject * obj) +void smartSaveButton::addObject(UAVDataObject * obj) { if(!objects.contains(obj)) objects.append(obj); @@ -114,7 +119,7 @@ void smartSaveButton::saving_finished(int id, bool result) if(id==current_objectID) { sv_result=result; - qDebug()<<"saving_finished result="<lblCRCL->setText( QString::number(DFUObject::CRCFromQBArray(loadedFW,m_dfu->devices[deviceID].SizeOfCode))); //myDevice->lblFirmwareSizeL->setText(QString("Firmware size: ")+QVariant(loadedFW.length()).toString()+ QString(" bytes")); if (populateLoadedStructuredDescription(desc)) { diff --git a/ground/openpilotgcs/src/plugins/uploader/op_dfu.cpp b/ground/openpilotgcs/src/plugins/uploader/op_dfu.cpp index 8a9045233..b1e215f91 100644 --- a/ground/openpilotgcs/src/plugins/uploader/op_dfu.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/op_dfu.cpp @@ -956,7 +956,7 @@ quint32 DFUObject::CRC32WideFast(quint32 Crc, quint32 Size, quint32 *Buffer) */ quint32 DFUObject::CRCFromQBArray(QByteArray array, quint32 Size) { - int pad=Size-array.length(); + quint32 pad=Size-array.length(); array.append(QByteArray(pad,255)); quint32 t[Size/4]; for(int x=0;x Date: Thu, 25 Aug 2011 12:20:19 +0200 Subject: [PATCH 081/145] OP-567 Modules/GPS: Block in PIOS_COM, not in TaskDelay - or we will miss updates! --- flight/Modules/GPS/GPS.c | 12 +++--------- 1 file changed, 3 insertions(+), 9 deletions(-) diff --git a/flight/Modules/GPS/GPS.c b/flight/Modules/GPS/GPS.c index 9bb873654..d2ab2b3c5 100644 --- a/flight/Modules/GPS/GPS.c +++ b/flight/Modules/GPS/GPS.c @@ -199,14 +199,12 @@ static void gpsTask(void *parameters) // Loop forever while (1) { + uint8_t c; #ifdef ENABLE_GPS_BINARY_GTOP // GTOP BINARY GPS mode - while (PIOS_COM_ReceiveBufferUsed(gpsPort) > 0) + while (PIOS_COM_ReceiveBuffer(gpsPort, &c, 1, xDelay) > 0) { - uint8_t c; - PIOS_COM_ReceiveBuffer(gpsPort, &c, 1, 0); - if (GTOP_BIN_update_position(c, &numChecksumErrors, &numParsingErrors) >= 0) { numUpdates++; @@ -221,10 +219,8 @@ static void gpsTask(void *parameters) // NMEA or SINGLE-SENTENCE GPS mode // This blocks the task until there is something on the buffer - while (PIOS_COM_ReceiveBufferUsed(gpsPort) > 0) + while (PIOS_COM_ReceiveBuffer(gpsPort, &c, 1, xDelay) > 0) { - uint8_t c; - PIOS_COM_ReceiveBuffer(gpsPort, &c, 1, 0); // detect start while acquiring stream if (!start_flag && (c == '$')) @@ -362,8 +358,6 @@ static void gpsTask(void *parameters) AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_CRITICAL); } - // Block task until next update - vTaskDelay(xDelay); } } From dfdea1606576e7261ca4d8aca0a0b7d6d3d96281 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Thu, 25 Aug 2011 14:46:30 +0200 Subject: [PATCH 082/145] Modules/GPS: Make GPS COM unidirectional (rx only) and use smaller, runtime allocated heap buffers --- flight/Modules/GPS/GPS.c | 252 +++++++------------- flight/Modules/GPS/GTOP_BIN.c | 262 --------------------- flight/Modules/GPS/NMEA.c | 171 +++----------- flight/Modules/GPS/inc/GPS.h | 2 - flight/Modules/GPS/inc/GTOP_BIN.h | 42 ---- flight/Modules/GPS/inc/NMEA.h | 7 +- flight/Modules/GPS/inc/gps_mode.h | 58 ----- flight/OpenPilot/System/pios_board.c | 5 +- flight/OpenPilot/System/pios_board_posix.c | 6 +- 9 files changed, 123 insertions(+), 682 deletions(-) delete mode 100644 flight/Modules/GPS/GTOP_BIN.c delete mode 100644 flight/Modules/GPS/inc/GTOP_BIN.h delete mode 100644 flight/Modules/GPS/inc/gps_mode.h diff --git a/flight/Modules/GPS/GPS.c b/flight/Modules/GPS/GPS.c index d2ab2b3c5..44b039eb1 100644 --- a/flight/Modules/GPS/GPS.c +++ b/flight/Modules/GPS/GPS.c @@ -35,13 +35,7 @@ #include -#ifdef ENABLE_GPS_BINARY_GTOP - #include "GTOP_BIN.h" -#endif - -#if defined(ENABLE_GPS_ONESENTENCE_GTOP) || defined(ENABLE_GPS_NMEA) - #include "NMEA.h" -#endif +#include "NMEA.h" #include "gpsposition.h" #include "homelocation.h" @@ -63,25 +57,16 @@ static float GravityAccel(float latitude, float longitude, float altitude); // **************** // Private constants -//#define FULL_COLD_RESTART // uncomment this to tell the GPS to do a FULL COLD restart -//#define DISABLE_GPS_THRESHOLD // - #define GPS_TIMEOUT_MS 500 -#define GPS_COMMAND_RESEND_TIMEOUT_MS 2000 +#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 - #ifdef ENABLE_GPS_BINARY_GTOP - #define STACK_SIZE_BYTES 800 - #else - #define STACK_SIZE_BYTES 800 - #endif + #define STACK_SIZE_BYTES 800 #else - #ifdef ENABLE_GPS_BINARY_GTOP - #define STACK_SIZE_BYTES 440 - #else - #define STACK_SIZE_BYTES 440 - #endif + #define STACK_SIZE_BYTES 650 #endif #define TASK_PRIORITY (tskIDLE_PRIORITY + 1) @@ -93,9 +78,7 @@ static uint32_t gpsPort; static xTaskHandle gpsTaskHandle; -#ifndef ENABLE_GPS_BINARY_GTOP - static char gps_rx_buffer[128]; -#endif +static char* gps_rx_buffer; static uint32_t timeOfLastCommandMs; static uint32_t timeOfLastUpdateMs; @@ -135,6 +118,8 @@ int32_t GPSInitialize(void) // TODO: Get gps settings object gpsPort = PIOS_COM_GPS; + gps_rx_buffer = pvPortMalloc(NMEA_MAX_PACKET_LENGTH); + return 0; } MODULE_INITCALL(GPSInitialize, GPSStart) @@ -150,45 +135,11 @@ static void gpsTask(void *parameters) uint32_t timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;; GPSPositionData GpsData; -#ifdef ENABLE_GPS_BINARY_GTOP - GTOP_BIN_init(); -#else uint8_t rx_count = 0; bool start_flag = false; bool found_cr = false; int32_t gpsRxOverflow = 0; -#endif -#ifdef FULL_COLD_RESTART - // tell the GPS to do a FULL COLD restart - PIOS_COM_SendStringNonBlocking(gpsPort, "$PMTK104*37\r\n"); - timeOfLastCommandMs = timeNowMs; - while (timeNowMs - timeOfLastCommandMs < 300) // delay for 300ms to let the GPS sort itself out - { - vTaskDelay(xDelay); // Block task until next update - timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;; - } -#endif - -#ifdef DISABLE_GPS_THRESHOLD - PIOS_COM_SendStringNonBlocking(gpsPort, "$PMTK397,0*23\r\n"); -#endif - -#ifdef ENABLE_GPS_BINARY_GTOP - // switch to GTOP binary mode - PIOS_COM_SendStringNonBlocking(gpsPort ,"$PGCMD,21,1*6F\r\n"); -#endif - -#ifdef ENABLE_GPS_ONESENTENCE_GTOP - // switch to single sentence mode - PIOS_COM_SendStringNonBlocking(gpsPort, "$PGCMD,21,2*6C\r\n"); -#endif - -#ifdef ENABLE_GPS_NMEA - // switch to NMEA mode - PIOS_COM_SendStringNonBlocking(gpsPort, "$PGCMD,21,3*6D\r\n"); -#endif - numUpdates = 0; numChecksumErrors = 0; numParsingErrors = 0; @@ -200,103 +151,86 @@ static void gpsTask(void *parameters) while (1) { uint8_t c; - #ifdef ENABLE_GPS_BINARY_GTOP - // GTOP BINARY GPS mode + // NMEA or SINGLE-SENTENCE GPS mode - while (PIOS_COM_ReceiveBuffer(gpsPort, &c, 1, xDelay) > 0) + // 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 + if (!start_flag && (c == '$')) { - if (GTOP_BIN_update_position(c, &numChecksumErrors, &numParsingErrors) >= 0) - { - numUpdates++; + start_flag = true; + found_cr = false; + rx_count = 0; + } + else + if (!start_flag) + continue; + + 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++; + start_flag = false; + 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 + 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); + ++numChecksumErrors; + //PIOS_DEBUG_PinLow(2); + } + else + { // Valid checksum, use this packet to update the GPS position + if (!NMEA_update_position(&gps_rx_buffer[1])) { + //PIOS_DEBUG_PinHigh(2); + ++numParsingErrors; + //PIOS_DEBUG_PinLow(2); + } + else + ++numUpdates; timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; timeOfLastUpdateMs = timeNowMs; timeOfLastCommandMs = timeNowMs; } } - - #else - // 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 - if (!start_flag && (c == '$')) - { - start_flag = true; - found_cr = false; - rx_count = 0; - } - else - if (!start_flag) - continue; - - if (rx_count >= sizeof(gps_rx_buffer)) - { - // The buffer is already full and we haven't found a valid NMEA sentence. - // Flush the buffer and note the overflow event. - gpsRxOverflow++; - start_flag = false; - 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 - 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); - ++numChecksumErrors; - //PIOS_DEBUG_PinLow(2); - } - else - { // Valid checksum, use this packet to update the GPS position - if (!NMEA_update_position(&gps_rx_buffer[1])) { - //PIOS_DEBUG_PinHigh(2); - ++numParsingErrors; - //PIOS_DEBUG_PinLow(2); - } - else - ++numUpdates; - - timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; - timeOfLastUpdateMs = timeNowMs; - timeOfLastCommandMs = timeNowMs; - } - } - } - #endif + } // Check for GPS timeout timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS; @@ -309,30 +243,6 @@ static void gpsTask(void *parameters) GPSPositionSet(&GpsData); AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR); - if ((timeNowMs - timeOfLastCommandMs) >= GPS_COMMAND_RESEND_TIMEOUT_MS) - { // resend the command .. just incase the gps has only just been plugged in or the gps did not get our last command - timeOfLastCommandMs = timeNowMs; - - #ifdef ENABLE_GPS_BINARY_GTOP - GTOP_BIN_init(); - // switch to binary mode - PIOS_COM_SendStringNonBlocking(gpsPort,"$PGCMD,21,1*6F\r\n"); - #endif - - #ifdef ENABLE_GPS_ONESENTENCE_GTOP - // switch to single sentence mode - PIOS_COM_SendStringNonBlocking(gpsPort,"$PGCMD,21,2*6C\r\n"); - #endif - - #ifdef ENABLE_GPS_NMEA - // switch to NMEA mode - PIOS_COM_SendStringNonBlocking(gpsPort,"$PGCMD,21,3*6D\r\n"); - #endif - - #ifdef DISABLE_GPS_TRESHOLD - PIOS_COM_SendStringNonBlocking(gpsPort,"$PMTK397,0*23\r\n"); - #endif - } } else { // we appear to be receiving GPS sentences OK, we've had an update diff --git a/flight/Modules/GPS/GTOP_BIN.c b/flight/Modules/GPS/GTOP_BIN.c deleted file mode 100644 index 7bd2964d9..000000000 --- a/flight/Modules/GPS/GTOP_BIN.c +++ /dev/null @@ -1,262 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup OpenPilotModules OpenPilot Modules - * @{ - * @addtogroup GSPModule GPS Module - * @brief Process GPS information - * @{ - * - * @file GTOP_BIN.c - * @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 - */ - -#include "openpilot.h" -#include "pios.h" -#include "GTOP_BIN.h" -#include "gpsposition.h" -#include "gpstime.h" -#include "gpssatellites.h" - -#include // memmove - -#ifdef ENABLE_GPS_BINARY_GTOP - -// ************ -// the structure of the binary packet - -typedef struct -{ - uint32_t utc_time; - - int32_t latitude; - uint8_t ns_indicator; - - int32_t longitude; - uint8_t ew_indicator; - - uint8_t fix_quality; - - uint8_t satellites_used; - - uint16_t hdop; - - int32_t msl_altitude; - - int32_t geoidal_seperation; - - uint8_t fix_type; - - int32_t course_over_ground; - - int32_t speed_over_ground; - - uint8_t day; - uint8_t month; - uint16_t year; -} __attribute__((__packed__)) t_gps_bin_packet_data; - -typedef struct -{ - uint16_t header; - t_gps_bin_packet_data data; - uint8_t asterisk; - uint8_t checksum; - uint16_t end_word; -} __attribute__((__packed__)) t_gps_bin_packet; - -// ************ - -// buffer that holds the incoming binary packet -static uint8_t gps_rx_buffer[sizeof(t_gps_bin_packet)] __attribute__ ((aligned(4))); - -// number of bytes currently in the rx buffer -static int16_t gps_rx_buffer_wr = 0; - -// ************ -// endian swapping functions - -static uint16_t swap2Bytes(uint16_t data) -{ - return (((data >> 8) & 0x00ff) | - ((data << 8) & 0xff00)); -} - -static uint32_t swap4Bytes(uint32_t data) -{ - return (((data >> 24) & 0x000000ff) | - ((data >> 8) & 0x0000ff00) | - ((data << 8) & 0x00ff0000) | - ((data << 24) & 0xff000000)); -} - -// ************ -/** - * Parses a complete binary packet and update the GPSPosition and GPSTime UAVObjects - * - * param[in] .. b = a new received byte from the GPS - * - * return '0' if we have found a valid binary packet - * return <0 if any errors were encountered with the packet or no packet found - */ - -int GTOP_BIN_update_position(uint8_t b, volatile uint32_t *chksum_errors, volatile uint32_t *parsing_errors) -{ - if (gps_rx_buffer_wr >= sizeof(gps_rx_buffer)) - { // make room for the new byte .. this will actually never get executed, just here as a safe guard really - memmove(gps_rx_buffer, gps_rx_buffer + 1, sizeof(gps_rx_buffer) - 1); - gps_rx_buffer_wr = sizeof(gps_rx_buffer) - 1; - } - - // add the new byte into the buffer - gps_rx_buffer[gps_rx_buffer_wr++] = b; - - int16_t i = 0; - - while (gps_rx_buffer_wr > 0) - { - t_gps_bin_packet *rx_packet = (t_gps_bin_packet *)(gps_rx_buffer + i); - - // scan for the start of a binary packet (the header bytes) - while (gps_rx_buffer_wr - i >= sizeof(rx_packet->header)) - { - if (rx_packet->header == 0x2404) - break; // found a valid header marker - rx_packet = (t_gps_bin_packet *)(gps_rx_buffer + ++i); - } - - // remove unwanted bytes before the start of the packet header - if (i > 0) - { - gps_rx_buffer_wr -= i; - if (gps_rx_buffer_wr > 0) - memmove(gps_rx_buffer, gps_rx_buffer + i, gps_rx_buffer_wr); - i = 0; - } - - if (gps_rx_buffer_wr < sizeof(t_gps_bin_packet)) - break; // not yet enough bytes for a complete binary packet - - // we have enough bytes for a complete binary packet - - // check to see if certain parameters in the binary packet are valid - if (rx_packet->header != 0x2404 || - rx_packet->end_word != 0x0A0D || - rx_packet->asterisk != 0x2A || - (rx_packet->data.ns_indicator != 1 && rx_packet->data.ns_indicator != 2) || - (rx_packet->data.ew_indicator != 1 && rx_packet->data.ew_indicator != 2) || - (rx_packet->data.fix_quality > 2) || - (rx_packet->data.fix_type < 1 || rx_packet->data.fix_type > 3) ) - { // invalid packet - if (parsing_errors) *parsing_errors++; - i++; - continue; - } - - { // check the checksum is valid - uint8_t *p = (uint8_t *)&rx_packet->data; - uint8_t checksum = 0; - for (int i = 0; i < sizeof(t_gps_bin_packet_data); i++) - checksum ^= *p++; - - if (checksum != rx_packet->checksum) - { // checksum error - if (chksum_errors) *chksum_errors++; - i++; - continue; - } - } - - // we now have a valid complete binary packet, update the GpsData and GpsTime objects - - // correct the endian order of the parameters - rx_packet->data.utc_time = swap4Bytes(rx_packet->data.utc_time); - rx_packet->data.latitude = swap4Bytes(rx_packet->data.latitude); - rx_packet->data.longitude = swap4Bytes(rx_packet->data.longitude); - rx_packet->data.hdop = swap2Bytes(rx_packet->data.hdop); - rx_packet->data.msl_altitude = swap4Bytes(rx_packet->data.msl_altitude); - rx_packet->data.geoidal_seperation = swap4Bytes(rx_packet->data.geoidal_seperation); - rx_packet->data.course_over_ground = swap4Bytes(rx_packet->data.course_over_ground); - rx_packet->data.speed_over_ground = swap4Bytes(rx_packet->data.speed_over_ground); - rx_packet->data.year = swap2Bytes(rx_packet->data.year); - - // set the gps time object - GPSTimeData GpsTime; -// GPSTimeGet(&GpsTime); - uint32_t utc_time = rx_packet->data.utc_time / 1000; - GpsTime.Second = utc_time % 100; // seconds - GpsTime.Minute = (utc_time / 100) % 100; // minutes - GpsTime.Hour = utc_time / 10000; // hours - GpsTime.Day = rx_packet->data.day; // day - GpsTime.Month = rx_packet->data.month; // month - GpsTime.Year = rx_packet->data.year; // year - GPSTimeSet(&GpsTime); - - // set the gps position object - GPSPositionData GpsData; -// GPSPositionGet(&GpsData); - switch (rx_packet->data.fix_type) - { - case 1: GpsData.Status = GPSPOSITION_STATUS_NOFIX; break; - case 2: GpsData.Status = GPSPOSITION_STATUS_FIX2D; break; - case 3: GpsData.Status = GPSPOSITION_STATUS_FIX3D; break; - default: GpsData.Status = GPSPOSITION_STATUS_NOGPS; break; - } - GpsData.Latitude = rx_packet->data.latitude * (rx_packet->data.ns_indicator == 1 ? +1 : -1) * 10; // degrees * 10e6 - GpsData.Longitude = rx_packet->data.longitude * (rx_packet->data.ew_indicator == 1 ? +1 : -1) * 10; // degrees * 10e6 - GpsData.Altitude = (float)rx_packet->data.msl_altitude / 1000; // meters - GpsData.GeoidSeparation = (float)rx_packet->data.geoidal_seperation / 1000; // meters - GpsData.Heading = (float)rx_packet->data.course_over_ground / 1000; // degrees - GpsData.Groundspeed = (float)rx_packet->data.speed_over_ground / 3600; // m/s - GpsData.Satellites = rx_packet->data.satellites_used; // - GpsData.PDOP = 99.99; // not available in binary mode - GpsData.HDOP = (float)rx_packet->data.hdop / 100; // - GpsData.VDOP = 99.99; // not available in binary mode - GPSPositionSet(&GpsData); - - // set the number of satellites -// GPSSatellitesData SattelliteData; -//// GPSSatellitesGet(&SattelliteData); -// memset(&SattelliteData, 0, sizeof(SattelliteData)); -// SattelliteData.SatsInView = rx_packet->data.satellites_used; // -// GPSSatellitesSet(&SattelliteData); - - // remove the spent binary packet from the buffer - gps_rx_buffer_wr -= sizeof(t_gps_bin_packet); - if (gps_rx_buffer_wr > 0) - memmove(gps_rx_buffer, gps_rx_buffer + sizeof(t_gps_bin_packet), gps_rx_buffer_wr); - - return 0; // found a valid packet - } - - return -1; // no valid packet found -} - -// ************ - -void GTOP_BIN_init(void) -{ - gps_rx_buffer_wr = 0; -} - -// ************ - -#endif // ENABLE_GPS_BINARY_GTOP - diff --git a/flight/Modules/GPS/NMEA.c b/flight/Modules/GPS/NMEA.c index 3dfd98d40..adadc588a 100644 --- a/flight/Modules/GPS/NMEA.c +++ b/flight/Modules/GPS/NMEA.c @@ -40,8 +40,6 @@ -#if defined(ENABLE_GPS_NMEA) || defined(ENABLE_GPS_ONESENTENCE_GTOP) - // Debugging #ifdef ENABLE_DEBUG_MSG //#define DEBUG_MSG_IN ///< define to display the incoming NMEA messages @@ -54,7 +52,6 @@ //#define NMEA_DEBUG_GSA ///< define to enable debug of GSA messages //#define NMEA_DEBUG_GSV ///< define to enable debug of GSV messages //#define NMEA_DEBUG_ZDA ///< define to enable debug of ZDA messages -//#define NMEA_DEBUG_PGTOP ///< define to enable debug of PGTOP messages #define DEBUG_MSG(format, ...) PIOS_COM_SendFormattedString(DEBUG_PORT, format, ## __VA_ARGS__) #else #define DEBUG_MSG(format, ...) @@ -69,56 +66,45 @@ struct nmea_parser { uint32_t cnt; }; - #ifdef ENABLE_GPS_NMEA - static bool nmeaProcessGPGGA(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam); - static bool nmeaProcessGPRMC(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam); - static bool nmeaProcessGPVTG(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam); - static bool nmeaProcessGPGSA(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam); - static bool nmeaProcessGPZDA(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam); - static bool nmeaProcessGPGSV(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam); - #endif +static bool nmeaProcessGPGGA(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam); +static bool nmeaProcessGPRMC(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam); +static bool nmeaProcessGPVTG(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam); +static bool nmeaProcessGPGSA(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam); +static bool nmeaProcessGPZDA(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam); +static bool nmeaProcessGPGSV(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam); - static bool nmeaProcessPGTOP(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam); - static struct nmea_parser nmea_parsers[] = { - - #ifdef ENABLE_GPS_NMEA - { - .prefix = "GPGGA", - .handler = nmeaProcessGPGGA, - .cnt = 0, - }, - { - .prefix = "GPVTG", - .handler = nmeaProcessGPVTG, - .cnt = 0, - }, - { - .prefix = "GPGSA", - .handler = nmeaProcessGPGSA, - .cnt = 0, - }, - { - .prefix = "GPRMC", - .handler = nmeaProcessGPRMC, - .cnt = 0, - }, - { - .prefix = "GPZDA", - .handler = nmeaProcessGPZDA, - .cnt = 0, - }, - { - .prefix = "GPGSV", - .handler = nmeaProcessGPGSV, - .cnt = 0, - }, - #endif - { - .prefix = "PGTOP", - .handler = nmeaProcessPGTOP, - .cnt = 0, - }, +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, + }, + { + .prefix = "GPZDA", + .handler = nmeaProcessGPZDA, + .cnt = 0, + }, + { + .prefix = "GPGSV", + .handler = nmeaProcessGPGSV, + .cnt = 0, + }, }; static struct nmea_parser *NMEA_find_parser_by_prefix(const char *prefix) @@ -229,7 +215,6 @@ static float NMEA_real_to_float(char *nmea_real) return (((float)whole) + fract * pow(10, -fract_units)); } -#ifdef ENABLE_GPS_NMEA /* * Parse a field in the format: * DD[D]MM.mmmm[mm] @@ -287,7 +272,6 @@ static bool NMEA_latlon_to_fixed_point(int32_t * latlon, char *nmea_latlon, bool return true; } -#endif // ENABLE_GPS_NMEA /** @@ -376,7 +360,6 @@ bool NMEA_update_position(char *nmea_sentence) return true; } -#ifdef ENABLE_GPS_NMEA /** * Parse an NMEA GPGGA sentence and update the given UAVObject @@ -675,83 +658,3 @@ static bool nmeaProcessGPGSA(GPSPositionData * GpsData, bool* gpsDataUpdated, ch return true; } -#endif // ENABLE_GPS_NMEA - -/** - * Parse an NMEA PGTOP sentence and update the given UAVObject - * \param[in] A pointer to a GPSPosition UAVObject to be updated. - * \param[in] An NMEA sentence with a valid checksum - */ -static bool nmeaProcessPGTOP(GPSPositionData * GpsData, bool* gpsDataUpdated, char* param[], uint8_t nbParam) -{ - if (nbParam != 17) - return false; - - GPSTimeData gpst; - GPSTimeGet(&gpst); - - *gpsDataUpdated = true; - - // get UTC time [hhmmss.sss] - float hms = NMEA_real_to_float(param[1]); - gpst.Second = (int)hms % 100; - gpst.Minute = (((int)hms - gpst.Second) / 100) % 100; - gpst.Hour = (int)hms / 10000; - - // get latitude decimal degrees - GpsData->Latitude = NMEA_real_to_float(param[2])*1e7; - if (param[3][0] == 'S') - GpsData->Latitude = -GpsData->Latitude; - - - // get longitude decimal degrees - GpsData->Longitude = NMEA_real_to_float(param[4])*1e7; - if (param[5][0] == 'W') - GpsData->Longitude = -GpsData->Longitude; - - // get number of satellites used in GPS solution - GpsData->Satellites = atoi(param[7]); - - // next field: HDOP - GpsData->HDOP = NMEA_real_to_float(param[8]); - - // get altitude (in meters mm.m) - GpsData->Altitude = NMEA_real_to_float(param[9]); - - // next field: geoid separation - GpsData->GeoidSeparation = NMEA_real_to_float(param[10]); - - // Mode: 1=Fix not available, 2=2D, 3=3D - switch (atoi(param[11])) { - case 1: - GpsData->Status = GPSPOSITION_STATUS_NOFIX; - break; - case 2: - GpsData->Status = GPSPOSITION_STATUS_FIX2D; - break; - case 3: - GpsData->Status = GPSPOSITION_STATUS_FIX3D; - break; - default: - /* Unhandled */ - return false; - break; - } - - // get course over ground in degrees [ddd.dd] - GpsData->Heading = NMEA_real_to_float(param[12]); - - // get speed in km/h - GpsData->Groundspeed = NMEA_real_to_float(param[13]); - // to m/s - GpsData->Groundspeed /= 3.6; - - gpst.Day = atoi(param[14]); - gpst.Month = atoi(param[15]); - gpst.Year = atoi(param[16]); - GPSTimeSet(&gpst); - - return true; -} - -#endif // #if defined(ENABLE_GPS_NMEA) || defined(ENABLE_GPS_ONESENTENCE_GTOP) diff --git a/flight/Modules/GPS/inc/GPS.h b/flight/Modules/GPS/inc/GPS.h index 1ae683c9a..86a92d285 100644 --- a/flight/Modules/GPS/inc/GPS.h +++ b/flight/Modules/GPS/inc/GPS.h @@ -34,8 +34,6 @@ #ifndef GPS_H #define GPS_H -#include "gps_mode.h" - int32_t GPSInitialize(void); #endif // GPS_H diff --git a/flight/Modules/GPS/inc/GTOP_BIN.h b/flight/Modules/GPS/inc/GTOP_BIN.h deleted file mode 100644 index 6a5234430..000000000 --- a/flight/Modules/GPS/inc/GTOP_BIN.h +++ /dev/null @@ -1,42 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup OpenPilotModules OpenPilot Modules - * @{ - * @addtogroup GSPModule GPS Module - * @brief Process GPS information - * @{ - * - * @file GTOP_BIN.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 GTOP_BIN_H -#define GTOP_BIN_H - -#include -#include "gps_mode.h" - -#ifdef ENABLE_GPS_BINARY_GTOP - extern int GTOP_BIN_update_position(uint8_t b, volatile uint32_t *chksum_errors, volatile uint32_t *parsing_errors); - extern void GTOP_BIN_init(void); -#endif - -#endif diff --git a/flight/Modules/GPS/inc/NMEA.h b/flight/Modules/GPS/inc/NMEA.h index 5c1b13f8b..6ff6b1195 100644 --- a/flight/Modules/GPS/inc/NMEA.h +++ b/flight/Modules/GPS/inc/NMEA.h @@ -33,11 +33,8 @@ #include #include -#include "gps_mode.h" -#if defined(ENABLE_GPS_NMEA) || defined(ENABLE_GPS_ONESENTENCE_GTOP) - extern bool NMEA_update_position(char *nmea_sentence); - extern bool NMEA_checksum(char *nmea_sentence); -#endif +extern bool NMEA_update_position(char *nmea_sentence); +extern bool NMEA_checksum(char *nmea_sentence); #endif /* NMEA_H */ diff --git a/flight/Modules/GPS/inc/gps_mode.h b/flight/Modules/GPS/inc/gps_mode.h deleted file mode 100644 index 43e5b0398..000000000 --- a/flight/Modules/GPS/inc/gps_mode.h +++ /dev/null @@ -1,58 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup OpenPilotModules OpenPilot Modules - * @{ - * @addtogroup GSPModule GPS Module - * @brief Process GPS information - * @{ - * - * @file gps_mode.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Include file of the GPS module. - * As with all modules only the initialize function is exposed all other - * interactions with the module take place through the event queue and - * objects. - * @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 GPS_MODE_H -#define GPS_MODE_H - -// **************** -// you MUST have one of these uncommented - and ONLY one - -//#define ENABLE_GPS_BINARY_GTOP // uncomment this if we are using GTOP BINARY mode -//#define ENABLE_GPS_ONESENTENCE_GTOP // uncomment this if we are using GTOP SINGLE SENTENCE mode -#define ENABLE_GPS_NMEA // uncomment this if we are using NMEA mode - -// **************** -// make sure they have defined a protocol to use - -#if !defined(ENABLE_GPS_BINARY_GTOP) && !defined(ENABLE_GPS_ONESENTENCE_GTOP) && !defined(ENABLE_GPS_NMEA) - #error YOU MUST SELECT THE DESIRED GPS PROTOCOL IN gps_mode.h! -#endif - -// **************** - -#endif - -/** - * @} - * @} - */ diff --git a/flight/OpenPilot/System/pios_board.c b/flight/OpenPilot/System/pios_board.c index 12d365b67..150bfb938 100644 --- a/flight/OpenPilot/System/pios_board.c +++ b/flight/OpenPilot/System/pios_board.c @@ -525,7 +525,6 @@ static const struct pios_spektrum_cfg pios_spektrum_cfg = { #define PIOS_COM_TELEM_RF_TX_BUF_LEN 192 #define PIOS_COM_GPS_RX_BUF_LEN 96 -#define PIOS_COM_GPS_TX_BUF_LEN 96 #define PIOS_COM_TELEM_USB_RX_BUF_LEN 192 #define PIOS_COM_TELEM_USB_TX_BUF_LEN 192 @@ -1075,12 +1074,10 @@ void PIOS_Board_Init(void) { PIOS_Assert(0); } uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN); - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_TX_BUF_LEN); PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id, rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, - tx_buffer, PIOS_COM_GPS_TX_BUF_LEN)) { + NULL, 0)) { PIOS_Assert(0); } } diff --git a/flight/OpenPilot/System/pios_board_posix.c b/flight/OpenPilot/System/pios_board_posix.c index e1e8dcc54..37fadd760 100644 --- a/flight/OpenPilot/System/pios_board_posix.c +++ b/flight/OpenPilot/System/pios_board_posix.c @@ -71,7 +71,7 @@ const struct pios_udp_cfg pios_udp_aux_cfg = { #define PIOS_COM_TELEM_RF_RX_BUF_LEN 192 #define PIOS_COM_TELEM_RF_TX_BUF_LEN 192 -#define PIOS_COM_GPS_RX_BUF_LEN 192 +#define PIOS_COM_GPS_RX_BUF_LEN 96 /* * Board specific number of devices. @@ -164,12 +164,10 @@ void PIOS_Board_Init(void) { PIOS_Assert(0); } uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN); - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN); PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); if (PIOS_COM_Init(&pios_com_gps_id, &pios_udp_com_driver, pios_udp_gps_id, rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, - tx_buffer, PIOS_COM_GPS_RX_BUF_LEN)) { + NULL, 0)) { PIOS_Assert(0); } } From 7e396efbbf90a133d50791427c04bd996e7d7cb2 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Thu, 25 Aug 2011 15:11:29 +0200 Subject: [PATCH 083/145] Modules/GPS: Assert buffer creation --- flight/Modules/GPS/GPS.c | 1 + 1 file changed, 1 insertion(+) diff --git a/flight/Modules/GPS/GPS.c b/flight/Modules/GPS/GPS.c index 44b039eb1..3d9cd1b20 100644 --- a/flight/Modules/GPS/GPS.c +++ b/flight/Modules/GPS/GPS.c @@ -119,6 +119,7 @@ int32_t GPSInitialize(void) gpsPort = PIOS_COM_GPS; gps_rx_buffer = pvPortMalloc(NMEA_MAX_PACKET_LENGTH); + PIOS_ASSERT(gps_rx_buffer); return 0; } From 54ecf233f31fc8cb812e0a7b9fde8ef6f344cf39 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Thu, 25 Aug 2011 15:13:50 +0200 Subject: [PATCH 084/145] Modules/PS: typo fix --- flight/Modules/GPS/GPS.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/Modules/GPS/GPS.c b/flight/Modules/GPS/GPS.c index 3d9cd1b20..2e6b0901b 100644 --- a/flight/Modules/GPS/GPS.c +++ b/flight/Modules/GPS/GPS.c @@ -119,7 +119,7 @@ int32_t GPSInitialize(void) gpsPort = PIOS_COM_GPS; gps_rx_buffer = pvPortMalloc(NMEA_MAX_PACKET_LENGTH); - PIOS_ASSERT(gps_rx_buffer); + PIOS_Assert(gps_rx_buffer); return 0; } From 55f64802d1b13c381c08d9c4b4849ea45187a63a Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sat, 27 Aug 2011 00:12:16 -0500 Subject: [PATCH 085/145] Also enable malloc error catching on OpenPilot --- flight/OpenPilot/System/inc/FreeRTOSConfig.h | 1 + 1 file changed, 1 insertion(+) diff --git a/flight/OpenPilot/System/inc/FreeRTOSConfig.h b/flight/OpenPilot/System/inc/FreeRTOSConfig.h index 48b7c1560..4b37f7e44 100644 --- a/flight/OpenPilot/System/inc/FreeRTOSConfig.h +++ b/flight/OpenPilot/System/inc/FreeRTOSConfig.h @@ -26,6 +26,7 @@ #define configUSE_PREEMPTION 1 #define configUSE_IDLE_HOOK 1 #define configUSE_TICK_HOOK 0 +#define configUSE_MALLOC_FAILED_HOOK 1 #define configCPU_CLOCK_HZ ( ( unsigned long ) 72000000 ) #define configTICK_RATE_HZ ( ( portTickType ) 1000 ) #define configMAX_PRIORITIES ( ( unsigned portBASE_TYPE ) 5 ) From 89d23df8cdff4f6ba9e9c65ec2ffb8910eb88efc Mon Sep 17 00:00:00 2001 From: Stacey Sheldon Date: Sat, 27 Aug 2011 10:37:20 -0400 Subject: [PATCH 086/145] openocd: Add default config for stm32f2x Pulled from origin/james/ins. --- flight/Project/OpenOCD/stm32f2x.cfg | 61 +++++++++++++++++++++++++++++ 1 file changed, 61 insertions(+) create mode 100644 flight/Project/OpenOCD/stm32f2x.cfg diff --git a/flight/Project/OpenOCD/stm32f2x.cfg b/flight/Project/OpenOCD/stm32f2x.cfg new file mode 100644 index 000000000..b8de38437 --- /dev/null +++ b/flight/Project/OpenOCD/stm32f2x.cfg @@ -0,0 +1,61 @@ +# script for stm32f2xxx + +if { [info exists CHIPNAME] } { + set _CHIPNAME $CHIPNAME +} else { + set _CHIPNAME stm32f2xxx +} + +if { [info exists ENDIAN] } { + set _ENDIAN $ENDIAN +} else { + set _ENDIAN little +} + +# Work-area is a space in RAM used for flash programming +# By default use 64kB +if { [info exists WORKAREASIZE] } { + set _WORKAREASIZE $WORKAREASIZE +} else { + set _WORKAREASIZE 0x10000 +} + +# JTAG speed should be <= F_CPU/6. F_CPU after reset is 8MHz, so use F_JTAG = 1MHz +# +# Since we may be running of an RC oscilator, we crank down the speed a +# bit more to be on the safe side. Perhaps superstition, but if are +# running off a crystal, we can run closer to the limit. Note +# that there can be a pretty wide band where things are more or less stable. +jtag_khz 1000 + +jtag_nsrst_delay 100 +jtag_ntrst_delay 100 + +#jtag scan chain +if { [info exists CPUTAPID ] } { + set _CPUTAPID $CPUTAPID +} else { + # See STM Document RM0033 + # Section 32.6.3 - corresponds to Cortex-M3 r2p0 + set _CPUTAPID 0x4ba00477 +} +jtag newtap $_CHIPNAME cpu -irlen 4 -ircapture 0x1 -irmask 0xf -expected-id $_CPUTAPID + +if { [info exists BSTAPID ] } { + set _BSTAPID $BSTAPID +} else { + # See STM Document RM0033 + # Section 32.6.2 + # + set _BSTAPID 0x06411041 +} +jtag newtap $_CHIPNAME bs -irlen 5 -expected-id $_BSTAPID + +set _TARGETNAME $_CHIPNAME.cpu +target create $_TARGETNAME cortex_m3 -endian $_ENDIAN -chain-position $_TARGETNAME + +$_TARGETNAME configure -work-area-phys 0x20000000 -work-area-size $_WORKAREASIZE -work-area-backup 0 + +set _FLASHNAME $_CHIPNAME.flash +flash bank $_FLASHNAME stm32f2x 0 0 0 0 $_TARGETNAME + From 5a51077493ed1f6fc62660bfe1efe14809081bc3 Mon Sep 17 00:00:00 2001 From: Stacey Sheldon Date: Sat, 27 Aug 2011 10:38:50 -0400 Subject: [PATCH 087/145] openocd: add support for RTOS autodetection This allows things like 'info threads' to work in gdb when combined with openocd 0.5.0+. --- flight/Project/OpenOCD/stm32f1x.cfg | 2 +- flight/Project/OpenOCD/stm32f2x.cfg | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/flight/Project/OpenOCD/stm32f1x.cfg b/flight/Project/OpenOCD/stm32f1x.cfg index 8007ff57a..9047b5aff 100644 --- a/flight/Project/OpenOCD/stm32f1x.cfg +++ b/flight/Project/OpenOCD/stm32f1x.cfg @@ -62,7 +62,7 @@ jtag newtap $_CHIPNAME bs -irlen 5 -expected-id $_BSTAPID1 \ -expected-id $_BSTAPID6 -expected-id $_BSTAPID7 set _TARGETNAME $_CHIPNAME.cpu -target create $_TARGETNAME cortex_m3 -endian $_ENDIAN -chain-position $_TARGETNAME +target create $_TARGETNAME cortex_m3 -endian $_ENDIAN -chain-position $_TARGETNAME -rtos auto $_TARGETNAME configure -work-area-phys 0x20000000 -work-area-size $_WORKAREASIZE -work-area-backup 0 diff --git a/flight/Project/OpenOCD/stm32f2x.cfg b/flight/Project/OpenOCD/stm32f2x.cfg index b8de38437..b5cea55e5 100644 --- a/flight/Project/OpenOCD/stm32f2x.cfg +++ b/flight/Project/OpenOCD/stm32f2x.cfg @@ -52,7 +52,7 @@ if { [info exists BSTAPID ] } { jtag newtap $_CHIPNAME bs -irlen 5 -expected-id $_BSTAPID set _TARGETNAME $_CHIPNAME.cpu -target create $_TARGETNAME cortex_m3 -endian $_ENDIAN -chain-position $_TARGETNAME +target create $_TARGETNAME cortex_m3 -endian $_ENDIAN -chain-position $_TARGETNAME -rtos auto $_TARGETNAME configure -work-area-phys 0x20000000 -work-area-size $_WORKAREASIZE -work-area-backup 0 From 8198cc881f73a04beebe724bfb37e4eb0ecf14ce Mon Sep 17 00:00:00 2001 From: Stacey Sheldon Date: Sat, 27 Aug 2011 10:41:53 -0400 Subject: [PATCH 088/145] openocd: upgrade to 0.5.0 Upgrade is needed to support STM32F2 chip on the new boards. Use 'openocd_install' to download/build the new version on Linux. --- Makefile | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/Makefile b/Makefile index 18cdb3b2e..118b424f2 100644 --- a/Makefile +++ b/Makefile @@ -202,8 +202,8 @@ arm_sdk_clean: OPENOCD_DIR := $(TOOLS_DIR)/openocd .PHONY: openocd_install -openocd_install: OPENOCD_URL := http://sourceforge.net/projects/openocd/files/openocd/0.4.0/openocd-0.4.0.tar.bz2/download -openocd_install: OPENOCD_FILE := openocd-0.4.0.tar.bz2 +openocd_install: OPENOCD_URL := http://sourceforge.net/projects/openocd/files/openocd/0.5.0/openocd-0.5.0.tar.bz2/download +openocd_install: OPENOCD_FILE := openocd-0.5.0.tar.bz2 # order-only prereq on directory existance: openocd_install: | $(DL_DIR) $(TOOLS_DIR) openocd_install: openocd_clean @@ -218,8 +218,8 @@ openocd_install: openocd_clean # build and install $(V1) mkdir -p "$(OPENOCD_DIR)" $(V1) ( \ - cd $(DL_DIR)/openocd-build/openocd-0.4.0 ; \ - ./configure --prefix="$(OPENOCD_DIR)" --enable-ft2232_libftdi ; \ + cd $(DL_DIR)/openocd-build/openocd-0.5.0 ; \ + ./configure --prefix="$(OPENOCD_DIR)" --enable-ft2232_libftdi --enable-buspirate; \ $(MAKE) ; \ $(MAKE) install ; \ ) From f29b99521e2c256b42bf766e35d32d1625cb6345 Mon Sep 17 00:00:00 2001 From: Stacey Sheldon Date: Sat, 27 Aug 2011 10:43:02 -0400 Subject: [PATCH 089/145] openocd: collapse JTAG rules for STM32F1 and STM32F2 --- flight/AHRS/Makefile | 2 +- flight/CopterControl/Makefile | 2 +- flight/INS/Makefile | 2 +- flight/OpenPilot/Makefile | 2 +- flight/PipXtreme/Makefile | 2 +- make/boards/ahrs/board-info.mk | 2 ++ make/boards/coptercontrol/board-info.mk | 2 ++ make/boards/ins/board-info.mk | 2 ++ make/boards/openpilot/board-info.mk | 2 ++ make/firmware-defs.mk | 48 ++----------------------- 10 files changed, 15 insertions(+), 51 deletions(-) diff --git a/flight/AHRS/Makefile b/flight/AHRS/Makefile index 46405ca6c..1943cc3a6 100644 --- a/flight/AHRS/Makefile +++ b/flight/AHRS/Makefile @@ -355,7 +355,7 @@ $(OUTDIR)/$(TARGET).bin.o: $(OUTDIR)/$(TARGET).bin $(eval $(call OPFW_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(BOARD_TYPE),$(BOARD_REVISION))) # Add jtag targets (program and wipe) -$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(FW_BANK_BASE),$(FW_BANK_SIZE))) +$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(FW_BANK_BASE),$(FW_BANK_SIZE),$(OPENOCD_CONFIG))) .PHONY: elf lss sym hex bin bino opfw elf: $(OUTDIR)/$(TARGET).elf diff --git a/flight/CopterControl/Makefile b/flight/CopterControl/Makefile index 2d7663853..80404d6ea 100644 --- a/flight/CopterControl/Makefile +++ b/flight/CopterControl/Makefile @@ -550,7 +550,7 @@ $(OUTDIR)/$(TARGET).bin.o: $(OUTDIR)/$(TARGET).bin $(eval $(call OPFW_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(BOARD_TYPE),$(BOARD_REVISION))) # Add jtag targets (program and wipe) -$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(FW_BANK_BASE),$(FW_BANK_SIZE))) +$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(FW_BANK_BASE),$(FW_BANK_SIZE),$(OPENOCD_CONFIG))) .PHONY: elf lss sym hex bin bino opfw elf: $(OUTDIR)/$(TARGET).elf diff --git a/flight/INS/Makefile b/flight/INS/Makefile index bd3bb52a9..4b39267f7 100644 --- a/flight/INS/Makefile +++ b/flight/INS/Makefile @@ -363,7 +363,7 @@ $(OUTDIR)/$(TARGET).bin.o: $(OUTDIR)/$(TARGET).bin $(eval $(call OPFW_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(BOARD_TYPE),$(BOARD_REVISION))) # Add jtag targets (program and wipe) -$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(FW_BANK_BASE),$(FW_BANK_SIZE))) +$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(FW_BANK_BASE),$(FW_BANK_SIZE),$(OPENOCD_CONFIG))) .PHONY: elf lss sym hex bin bino opfw elf: $(OUTDIR)/$(TARGET).elf diff --git a/flight/OpenPilot/Makefile b/flight/OpenPilot/Makefile index 7d3cdf514..32fc32373 100644 --- a/flight/OpenPilot/Makefile +++ b/flight/OpenPilot/Makefile @@ -514,7 +514,7 @@ $(OUTDIR)/$(TARGET).bin.o: $(OUTDIR)/$(TARGET).bin $(eval $(call OPFW_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(BOARD_TYPE),$(BOARD_REVISION))) # Add jtag targets (program and wipe) -$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(FW_BANK_BASE),$(FW_BANK_SIZE))) +$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(FW_BANK_BASE),$(FW_BANK_SIZE),$(OPENOCD_CONFIG))) .PHONY: elf lss sym hex bin bino opfw elf: $(OUTDIR)/$(TARGET).elf diff --git a/flight/PipXtreme/Makefile b/flight/PipXtreme/Makefile index 9a70ac417..8a939d411 100644 --- a/flight/PipXtreme/Makefile +++ b/flight/PipXtreme/Makefile @@ -394,7 +394,7 @@ $(OUTDIR)/$(TARGET).bin.o: $(OUTDIR)/$(TARGET).bin $(eval $(call OPFW_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(BOARD_TYPE),$(BOARD_REVISION))) # Add jtag targets (program and wipe) -$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(FW_BANK_BASE),$(FW_BANK_SIZE))) +$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(FW_BANK_BASE),$(FW_BANK_SIZE),$(OPENOCD_CONFIG))) .PHONY: elf lss sym hex bin bino opfw elf: $(OUTDIR)/$(TARGET).elf diff --git a/make/boards/ahrs/board-info.mk b/make/boards/ahrs/board-info.mk index 2103357ba..85135ea25 100644 --- a/make/boards/ahrs/board-info.mk +++ b/make/boards/ahrs/board-info.mk @@ -9,6 +9,8 @@ BOARD := STM32103CB_AHRS MODEL := MD MODEL_SUFFIX := +OPENOCD_CONFIG := stm32f1x.cfg + # Note: These must match the values in link_$(BOARD)_memory.ld BL_BANK_BASE := 0x08000000 # Start of bootloader flash BL_BANK_SIZE := 0x00002000 # Should include BD_INFO region diff --git a/make/boards/coptercontrol/board-info.mk b/make/boards/coptercontrol/board-info.mk index 012c6ef16..547692212 100644 --- a/make/boards/coptercontrol/board-info.mk +++ b/make/boards/coptercontrol/board-info.mk @@ -9,6 +9,8 @@ BOARD := STM32103CB_CC_Rev1 MODEL := MD MODEL_SUFFIX := _CC +OPENOCD_CONFIG := stm32f1x.cfg + # Note: These must match the values in link_$(BOARD)_memory.ld BL_BANK_BASE := 0x08000000 # Start of bootloader flash BL_BANK_SIZE := 0x00003000 # Should include BD_INFO region diff --git a/make/boards/ins/board-info.mk b/make/boards/ins/board-info.mk index 8a57661bf..a20bd606c 100644 --- a/make/boards/ins/board-info.mk +++ b/make/boards/ins/board-info.mk @@ -9,6 +9,8 @@ BOARD := STM3210E_INS MODEL := HD MODEL_SUFFIX := _OP +OPENOCD_CONFIG := stm32f1x.cfg + # Note: These must match the values in link_$(BOARD)_memory.ld BL_BANK_BASE := 0x08000000 # Start of bootloader flash BL_BANK_SIZE := 0x00002000 # Should include BD_INFO region diff --git a/make/boards/openpilot/board-info.mk b/make/boards/openpilot/board-info.mk index f1e35505c..d9cff166d 100644 --- a/make/boards/openpilot/board-info.mk +++ b/make/boards/openpilot/board-info.mk @@ -9,6 +9,8 @@ BOARD := STM3210E_OP MODEL := HD MODEL_SUFFIX := _OP +OPENOCD_CONFIG := stm32f1x.cfg + # Note: These must match the values in link_$(BOARD)_memory.ld BL_BANK_BASE := 0x08000000 # Start of bootloader flash BL_BANK_SIZE := 0x00005000 # Should include BD_INFO region diff --git a/make/firmware-defs.mk b/make/firmware-defs.mk index 0bbbb54b8..b22a66639 100644 --- a/make/firmware-defs.mk +++ b/make/firmware-defs.mk @@ -201,6 +201,7 @@ endef # $(1) = Name of binary image to write # $(2) = Base of flash region to write/wipe # $(3) = Size of flash region to write/wipe +# $(4) = OpenOCD configuration file to use define JTAG_TEMPLATE # --------------------------------------------------------------------------- # Options for OpenOCD flash-programming @@ -213,7 +214,7 @@ OOCD_EXE ?= openocd OOCD_JTAG_SETUP = -d0 # interface and board/target settings (using the OOCD target-library here) OOCD_JTAG_SETUP += -s $(TOP)/flight/Project/OpenOCD -OOCD_JTAG_SETUP += -f foss-jtag.revb.cfg -f stm32f1x.cfg +OOCD_JTAG_SETUP += -f foss-jtag.revb.cfg -f $(4) # initialize OOCD_BOARD_RESET = -c init @@ -244,48 +245,3 @@ wipe: -c "shutdown" endef -# $(1) = Name of binary image to write -# $(2) = Base of flash region to write/wipe -# $(3) = Size of flash region to write/wipe -define JTAG_TEMPLATE_F2X -# --------------------------------------------------------------------------- -# Options for OpenOCD flash-programming -# see openocd.pdf/openocd.texi for further information - -# if OpenOCD is in the $PATH just set OPENOCDEXE=openocd -OOCD_EXE ?= openocd - -# debug level -OOCD_JTAG_SETUP = -d0 -# interface and board/target settings (using the OOCD target-library here) -OOCD_JTAG_SETUP += -s $(TOP)/flight/Project/OpenOCD -OOCD_JTAG_SETUP += -f foss-jtag.revb.cfg -f stm32f2x.cfg - -# initialize -OOCD_BOARD_RESET = -c init -# show the targets -#OOCD_BOARD_RESET += -c targets -# commands to prepare flash-write -OOCD_BOARD_RESET += -c "reset halt" - -.PHONY: program -program: $(1) - @echo $(MSG_JTAG_PROGRAM) $$(call toprel, $$<) - $(V1) $(OOCD_EXE) \ - $$(OOCD_JTAG_SETUP) \ - $$(OOCD_BOARD_RESET) \ - -c "flash write_image erase $$< $(2) bin" \ - -c "verify_image $$< $(2) bin" \ - -c "reset run" \ - -c "shutdown" - -.PHONY: wipe -wipe: - @echo $(MSG_JTAG_WIPE) wiping $(3) bytes starting from $(2) - $(V1) $(OOCD_EXE) \ - $$(OOCD_JTAG_SETUP) \ - $$(OOCD_BOARD_RESET) \ - -c "flash erase_address pad $(2) $(3)" \ - -c "reset run" \ - -c "shutdown" -endef From c5746335c62b4d2da97ec61d9115a0223f01e835 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sat, 27 Aug 2011 15:57:09 -0500 Subject: [PATCH 090/145] Initialize GCSReceiver object if it is being used --- flight/CopterControl/System/pios_board.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/flight/CopterControl/System/pios_board.c b/flight/CopterControl/System/pios_board.c index 0fd7a6011..7108a94ff 100644 --- a/flight/CopterControl/System/pios_board.c +++ b/flight/CopterControl/System/pios_board.c @@ -32,6 +32,7 @@ #include #include #include +#include #if defined(PIOS_INCLUDE_SPI) @@ -1134,6 +1135,7 @@ void PIOS_Board_Init(void) { } #if defined(PIOS_INCLUDE_GCSRCVR) + GCSReceiverInitialize(); PIOS_GCSRCVR_Init(); uint32_t pios_gcsrcvr_rcvr_id; if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, 0)) { From 6514fd95c7b9ad2e7f5fa31d24810dda5675bcad Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sat, 27 Aug 2011 16:08:57 -0500 Subject: [PATCH 091/145] Also initialize the ReceiverActivity object in manual control --- flight/Modules/ManualControl/manualcontrol.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c index 3cc215de9..c528a599b 100644 --- a/flight/Modules/ManualControl/manualcontrol.c +++ b/flight/Modules/ManualControl/manualcontrol.c @@ -129,7 +129,7 @@ int32_t ManualControlInitialize() ManualControlCommandInitialize(); FlightStatusInitialize(); StabilizationDesiredInitialize(); - + ReceiverActivityInitialize(); ManualControlSettingsInitialize(); return 0; From a72c657e2109eebc8f5dc233d85750a068096dac Mon Sep 17 00:00:00 2001 From: Stacey Sheldon Date: Wed, 24 Aug 2011 08:49:34 -0400 Subject: [PATCH 092/145] rcvr activity: Speed up activity acquisition Activity detection logic can now move through the active receivers more quickly. --- flight/Modules/ManualControl/manualcontrol.c | 160 ++++++++++-------- .../src/plugins/config/configinputwidget.cpp | 2 +- 2 files changed, 92 insertions(+), 70 deletions(-) diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c index c528a599b..9165f06f8 100644 --- a/flight/Modules/ManualControl/manualcontrol.c +++ b/flight/Modules/ManualControl/manualcontrol.c @@ -89,7 +89,7 @@ static bool okToArm(void); static bool validInputRange(int16_t min, int16_t max, uint16_t value); #define RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP 12 -#define RCVR_ACTIVITY_MONITOR_MIN_RANGE 5 +#define RCVR_ACTIVITY_MONITOR_MIN_RANGE 10 struct rcvr_activity_fsm { ManualControlSettingsChannelGroupsOptions group; uint16_t prev[RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP]; @@ -347,6 +347,66 @@ static void resetRcvrActivity(struct rcvr_activity_fsm * fsm) fsm->sample_count = 0; } +static void updateRcvrActivitySample(uint32_t rcvr_id, uint16_t samples[], uint8_t max_channels) { + for (uint8_t channel = 0; channel < max_channels; channel++) { + samples[channel] = PIOS_RCVR_Read(rcvr_id, channel); + } +} + +static bool updateRcvrActivityCompare(uint32_t rcvr_id, struct rcvr_activity_fsm * fsm) +{ + bool activity_updated = false; + + /* Compare the current value to the previous sampled value */ + for (uint8_t channel = 0; + channel < RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP; + channel++) { + uint16_t delta; + uint16_t prev = fsm->prev[channel]; + uint16_t curr = PIOS_RCVR_Read(rcvr_id, channel); + if (curr > prev) { + delta = curr - prev; + } else { + delta = prev - curr; + } + + if (delta > RCVR_ACTIVITY_MONITOR_MIN_RANGE) { + /* Mark this channel as active */ + ReceiverActivityActiveGroupOptions group; + + /* Don't assume manualcontrolsettings and receiveractivity are in the same order. */ + switch (fsm->group) { + case MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM: + group = RECEIVERACTIVITY_ACTIVEGROUP_PWM; + break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM: + group = RECEIVERACTIVITY_ACTIVEGROUP_PPM; + break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_SPEKTRUM1: + group = RECEIVERACTIVITY_ACTIVEGROUP_SPEKTRUM1; + break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_SPEKTRUM2: + group = RECEIVERACTIVITY_ACTIVEGROUP_SPEKTRUM2; + break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS: + group = RECEIVERACTIVITY_ACTIVEGROUP_SBUS; + break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS: + group = RECEIVERACTIVITY_ACTIVEGROUP_GCS; + break; + default: + PIOS_Assert(0); + break; + } + + ReceiverActivityActiveGroupSet(&group); + ReceiverActivityActiveChannelSet(&channel); + activity_updated = true; + } + } + return (activity_updated); +} + static bool updateRcvrActivity(struct rcvr_activity_fsm * fsm) { bool activity_updated = false; @@ -362,82 +422,44 @@ static bool updateRcvrActivity(struct rcvr_activity_fsm * fsm) goto group_completed; } - /* Sample the channel */ - for (uint8_t channel = 0; - channel < RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP; - channel++) { - if (fsm->sample_count == 0) { - fsm->prev[channel] = PIOS_RCVR_Read(pios_rcvr_group_map[fsm->group], channel); - } else { - uint16_t delta; - uint16_t prev = fsm->prev[channel]; - uint16_t curr = PIOS_RCVR_Read(pios_rcvr_group_map[fsm->group], channel); - if (curr > prev) { - delta = curr - prev; - } else { - delta = prev - curr; - } - - if (delta > RCVR_ACTIVITY_MONITOR_MIN_RANGE) { - /* Mark this channel as active */ - ReceiverActivityActiveGroupOptions group; - - /* Don't assume manualcontrolsettings and receiveractivity are in the same order. */ - switch (fsm->group) { - case MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM: - group = RECEIVERACTIVITY_ACTIVEGROUP_PWM; - break; - case MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM: - group = RECEIVERACTIVITY_ACTIVEGROUP_PPM; - break; - case MANUALCONTROLSETTINGS_CHANNELGROUPS_SPEKTRUM1: - group = RECEIVERACTIVITY_ACTIVEGROUP_SPEKTRUM1; - break; - case MANUALCONTROLSETTINGS_CHANNELGROUPS_SPEKTRUM2: - group = RECEIVERACTIVITY_ACTIVEGROUP_SPEKTRUM2; - break; - case MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS: - group = RECEIVERACTIVITY_ACTIVEGROUP_SBUS; - break; - case MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS: - group = RECEIVERACTIVITY_ACTIVEGROUP_GCS; - break; - default: - PIOS_Assert(0); - break; - } - - ReceiverActivityActiveGroupSet(&group); - ReceiverActivityActiveChannelSet(&channel); - activity_updated = true; - } - } + if (fsm->sample_count == 0) { + /* Take a sample of each channel in this group */ + updateRcvrActivitySample(pios_rcvr_group_map[fsm->group], + fsm->prev, + NELEMENTS(fsm->prev)); + fsm->sample_count++; + return (false); } - if (++fsm->sample_count < 2) { - return (activity_updated); - } + /* Compare with previous sample */ + activity_updated = updateRcvrActivityCompare(pios_rcvr_group_map[fsm->group], fsm); +group_completed: /* Reset the sample counter */ fsm->sample_count = 0; - /* - * Group Completed - */ - -group_completed: - /* Start over at channel zero */ - if (++fsm->group < MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { - return (activity_updated); + /* Find the next active group, but limit search so we can't loop forever here */ + for (uint8_t i = 0; i < MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE; i++) { + /* Move to the next group */ + fsm->group++; + if (fsm->group >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + /* Wrap back to the first group */ + fsm->group = 0; + } + if (pios_rcvr_group_map[fsm->group]) { + /* + * Found an active group, take a sample here to avoid an + * extra 20ms delay in the main thread so we can speed up + * this algorithm. + */ + updateRcvrActivitySample(pios_rcvr_group_map[fsm->group], + fsm->prev, + NELEMENTS(fsm->prev)); + fsm->sample_count++; + break; + } } - /* - * All groups completed - */ - - /* Start over at group zero */ - fsm->group = 0; - return (activity_updated); } diff --git a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp index 6313c45b1..1590f52f0 100644 --- a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp @@ -542,7 +542,7 @@ void ConfigInputWidget::identifyControls() ++debounce; lastChannel.group= currentChannel.group; lastChannel.number=currentChannel.number; - if(!usedChannels.contains(lastChannel) && debounce>5) + if(!usedChannels.contains(lastChannel) && debounce>1) { debounce=0; usedChannels.append(lastChannel); From b3c43da90acba6be89da00787b1d77a34a2babaa Mon Sep 17 00:00:00 2001 From: Stacey Sheldon Date: Fri, 12 Aug 2011 21:19:48 -0400 Subject: [PATCH 093/145] stdperiph: Make TIM related APIs use const pointers This allows the configuration parameters to be stored in flash instead of copied to RAM. --- .../STM32F10x_StdPeriph_Driver/inc/stm32f10x_tim.h | 12 ++++++------ .../STM32F10x_StdPeriph_Driver/src/stm32f10x_tim.c | 12 ++++++------ 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/flight/PiOS/STM32F10x/Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_tim.h b/flight/PiOS/STM32F10x/Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_tim.h index 6529c0b0a..388d276e4 100644 --- a/flight/PiOS/STM32F10x/Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_tim.h +++ b/flight/PiOS/STM32F10x/Libraries/STM32F10x_StdPeriph_Driver/inc/stm32f10x_tim.h @@ -1025,12 +1025,12 @@ typedef struct */ void TIM_DeInit(TIM_TypeDef* TIMx); -void TIM_TimeBaseInit(TIM_TypeDef* TIMx, TIM_TimeBaseInitTypeDef* TIM_TimeBaseInitStruct); -void TIM_OC1Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct); -void TIM_OC2Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct); -void TIM_OC3Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct); -void TIM_OC4Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct); -void TIM_ICInit(TIM_TypeDef* TIMx, TIM_ICInitTypeDef* TIM_ICInitStruct); +void TIM_TimeBaseInit(TIM_TypeDef* TIMx, const TIM_TimeBaseInitTypeDef* TIM_TimeBaseInitStruct); +void TIM_OC1Init(TIM_TypeDef* TIMx, const TIM_OCInitTypeDef* TIM_OCInitStruct); +void TIM_OC2Init(TIM_TypeDef* TIMx, const TIM_OCInitTypeDef* TIM_OCInitStruct); +void TIM_OC3Init(TIM_TypeDef* TIMx, const TIM_OCInitTypeDef* TIM_OCInitStruct); +void TIM_OC4Init(TIM_TypeDef* TIMx, const TIM_OCInitTypeDef* TIM_OCInitStruct); +void TIM_ICInit(TIM_TypeDef* TIMx, const TIM_ICInitTypeDef* TIM_ICInitStruct); void TIM_PWMIConfig(TIM_TypeDef* TIMx, TIM_ICInitTypeDef* TIM_ICInitStruct); void TIM_BDTRConfig(TIM_TypeDef* TIMx, TIM_BDTRInitTypeDef *TIM_BDTRInitStruct); void TIM_TimeBaseStructInit(TIM_TimeBaseInitTypeDef* TIM_TimeBaseInitStruct); diff --git a/flight/PiOS/STM32F10x/Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_tim.c b/flight/PiOS/STM32F10x/Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_tim.c index 780105589..2f31c9a1a 100755 --- a/flight/PiOS/STM32F10x/Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_tim.c +++ b/flight/PiOS/STM32F10x/Libraries/STM32F10x_StdPeriph_Driver/src/stm32f10x_tim.c @@ -221,7 +221,7 @@ void TIM_DeInit(TIM_TypeDef* TIMx) * structure that contains the configuration information for the specified TIM peripheral. * @retval None */ -void TIM_TimeBaseInit(TIM_TypeDef* TIMx, TIM_TimeBaseInitTypeDef* TIM_TimeBaseInitStruct) +void TIM_TimeBaseInit(TIM_TypeDef* TIMx, const TIM_TimeBaseInitTypeDef* TIM_TimeBaseInitStruct) { uint16_t tmpcr1 = 0; @@ -274,7 +274,7 @@ void TIM_TimeBaseInit(TIM_TypeDef* TIMx, TIM_TimeBaseInitTypeDef* TIM_TimeBaseIn * that contains the configuration information for the specified TIM peripheral. * @retval None */ -void TIM_OC1Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct) +void TIM_OC1Init(TIM_TypeDef* TIMx, const TIM_OCInitTypeDef* TIM_OCInitStruct) { uint16_t tmpccmrx = 0, tmpccer = 0, tmpcr2 = 0; @@ -357,7 +357,7 @@ void TIM_OC1Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct) * that contains the configuration information for the specified TIM peripheral. * @retval None */ -void TIM_OC2Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct) +void TIM_OC2Init(TIM_TypeDef* TIMx, const TIM_OCInitTypeDef* TIM_OCInitStruct) { uint16_t tmpccmrx = 0, tmpccer = 0, tmpcr2 = 0; @@ -439,7 +439,7 @@ void TIM_OC2Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct) * that contains the configuration information for the specified TIM peripheral. * @retval None */ -void TIM_OC3Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct) +void TIM_OC3Init(TIM_TypeDef* TIMx, const TIM_OCInitTypeDef* TIM_OCInitStruct) { uint16_t tmpccmrx = 0, tmpccer = 0, tmpcr2 = 0; @@ -518,7 +518,7 @@ void TIM_OC3Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct) * that contains the configuration information for the specified TIM peripheral. * @retval None */ -void TIM_OC4Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct) +void TIM_OC4Init(TIM_TypeDef* TIMx, const TIM_OCInitTypeDef* TIM_OCInitStruct) { uint16_t tmpccmrx = 0, tmpccer = 0, tmpcr2 = 0; @@ -582,7 +582,7 @@ void TIM_OC4Init(TIM_TypeDef* TIMx, TIM_OCInitTypeDef* TIM_OCInitStruct) * that contains the configuration information for the specified TIM peripheral. * @retval None */ -void TIM_ICInit(TIM_TypeDef* TIMx, TIM_ICInitTypeDef* TIM_ICInitStruct) +void TIM_ICInit(TIM_TypeDef* TIMx, const TIM_ICInitTypeDef* TIM_ICInitStruct) { /* Check the parameters */ assert_param(IS_TIM_CHANNEL(TIM_ICInitStruct->TIM_Channel)); From 2f86e4dd4f0654668989e71a6b4b8b3440a300da Mon Sep 17 00:00:00 2001 From: Stacey Sheldon Date: Fri, 12 Aug 2011 23:23:16 -0400 Subject: [PATCH 094/145] Make PWM/PPM and Servo drivers play nicely together PWM and PPM can now coexist in the same load and be selected at boot time via the hwsettings UAVObject. This is basically a complete restructuring of the way the drivers interact with the TIM peripheral in the STM32. As a side effect, the PWM and PPM drivers are now ready to support multiple instances of each. This also provides the first step toward being able to reassign some of the PWM input pins to be servo output pins. Still more work required, but this is a good start. --- flight/CopterControl/Makefile | 1 + flight/CopterControl/System/inc/pios_config.h | 2 +- flight/CopterControl/System/pios_board.c | 438 ++++++++++------- flight/Libraries/ahrs_comm_objects.c | 1 + flight/OpenPilot/Makefile | 1 + flight/OpenPilot/System/inc/pios_config.h | 2 +- flight/OpenPilot/System/pios_board.c | 454 +++++++++++------- flight/PiOS/Boards/STM32103CB_CC_Rev1.h | 7 + flight/PiOS/Boards/STM3210E_OP.h | 7 + .../link_STM32103CB_CC_Rev1_sections.ld | 2 +- flight/PiOS/STM32F10x/pios_debug.c | 105 ++-- flight/PiOS/STM32F10x/pios_ppm.c | 415 +++++++++------- flight/PiOS/STM32F10x/pios_pwm.c | 321 +++++++------ flight/PiOS/STM32F10x/pios_servo.c | 154 +++--- flight/PiOS/STM32F10x/pios_tim.c | 391 +++++++++++++++ flight/PiOS/inc/pios_debug.h | 4 +- flight/PiOS/inc/pios_ppm_priv.h | 16 +- flight/PiOS/inc/pios_pwm_priv.h | 21 +- flight/PiOS/inc/pios_servo.h | 1 - flight/PiOS/inc/pios_servo_priv.h | 13 +- flight/PiOS/inc/pios_tim.h | 4 + flight/PiOS/inc/pios_tim_priv.h | 28 ++ 22 files changed, 1511 insertions(+), 877 deletions(-) create mode 100644 flight/PiOS/STM32F10x/pios_tim.c create mode 100644 flight/PiOS/inc/pios_tim.h create mode 100644 flight/PiOS/inc/pios_tim_priv.h diff --git a/flight/CopterControl/Makefile b/flight/CopterControl/Makefile index 9485d03d9..6bfb6e5a6 100644 --- a/flight/CopterControl/Makefile +++ b/flight/CopterControl/Makefile @@ -194,6 +194,7 @@ SRC += $(PIOSSTM32F10X)/pios_gpio.c SRC += $(PIOSSTM32F10X)/pios_exti.c SRC += $(PIOSSTM32F10X)/pios_rtc.c SRC += $(PIOSSTM32F10X)/pios_wdg.c +SRC += $(PIOSSTM32F10X)/pios_tim.c # PIOS USB related files (seperated to make code maintenance more easy) diff --git a/flight/CopterControl/System/inc/pios_config.h b/flight/CopterControl/System/inc/pios_config.h index e8fff8bbc..bab05551d 100644 --- a/flight/CopterControl/System/inc/pios_config.h +++ b/flight/CopterControl/System/inc/pios_config.h @@ -48,7 +48,7 @@ /* Supported receiver interfaces */ #define PIOS_INCLUDE_SPEKTRUM #define PIOS_INCLUDE_SBUS -//#define PIOS_INCLUDE_PPM +#define PIOS_INCLUDE_PPM #define PIOS_INCLUDE_PWM #define PIOS_INCLUDE_GCSRCVR diff --git a/flight/CopterControl/System/pios_board.c b/flight/CopterControl/System/pios_board.c index 7108a94ff..d9023ffb9 100644 --- a/flight/CopterControl/System/pios_board.c +++ b/flight/CopterControl/System/pios_board.c @@ -196,6 +196,220 @@ void PIOS_ADC_handler() { PIOS_ADC_DMA_Handler(); } +#include "pios_tim_priv.h" + +static const TIM_TimeBaseInitTypeDef tim_1_2_3_4_time_base = { + .TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1, + .TIM_ClockDivision = TIM_CKD_DIV1, + .TIM_CounterMode = TIM_CounterMode_Up, + .TIM_Period = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1), + .TIM_RepetitionCounter = 0x0000, +}; + +static const struct pios_tim_clock_cfg tim_1_cfg = { + .timer = TIM1, + .time_base_init = &tim_1_2_3_4_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM1_CC_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_2_cfg = { + .timer = TIM2, + .time_base_init = &tim_1_2_3_4_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM2_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_3_cfg = { + .timer = TIM3, + .time_base_init = &tim_1_2_3_4_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_4_cfg = { + .timer = TIM4, + .time_base_init = &tim_1_2_3_4_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_channel pios_tim_rcvrport_all_channels[] = { + { + .timer = TIM4, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Mode = GPIO_Mode_IPD, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM3, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_5, + .GPIO_Mode = GPIO_Mode_IPD, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + .remap = GPIO_PartialRemap_TIM3, + }, + { + .timer = TIM3, + .timer_chan = TIM_Channel_3, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_0, + .GPIO_Mode = GPIO_Mode_IPD, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM3, + .timer_chan = TIM_Channel_4, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_1, + .GPIO_Mode = GPIO_Mode_IPD, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM2, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_0, + .GPIO_Mode = GPIO_Mode_IPD, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM2, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_1, + .GPIO_Mode = GPIO_Mode_IPD, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, +}; + +static const struct pios_tim_channel pios_tim_servoport_all_pins[] = { + { + .timer = TIM4, + .timer_chan = TIM_Channel_4, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM4, + .timer_chan = TIM_Channel_3, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_8, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM4, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM1, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_8, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + { + .timer = TIM3, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_4, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + .remap = GPIO_PartialRemap_TIM3, + }, + { + .timer = TIM2, + .timer_chan = TIM_Channel_3, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_2, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, +}; + #if defined(PIOS_INCLUDE_USART) #include "pios_usart_priv.h" @@ -565,121 +779,40 @@ void PIOS_RTC_IRQ_Handler (void) * Servo outputs */ #include -static const struct pios_servo_channel pios_servo_channels[] = { - { - .timer = TIM4, - .port = GPIOB, - .channel = TIM_Channel_4, - .pin = GPIO_Pin_9, - }, - { - .timer = TIM4, - .port = GPIOB, - .channel = TIM_Channel_3, - .pin = GPIO_Pin_8, - }, - { - .timer = TIM4, - .port = GPIOB, - .channel = TIM_Channel_2, - .pin = GPIO_Pin_7, - }, - { - .timer = TIM1, - .port = GPIOA, - .channel = TIM_Channel_1, - .pin = GPIO_Pin_8, - }, - { /* needs to remap to alternative function */ - .timer = TIM3, - .port = GPIOB, - .channel = TIM_Channel_1, - .pin = GPIO_Pin_4, - }, - { - .timer = TIM2, - .port = GPIOA, - .channel = TIM_Channel_3, - .pin = GPIO_Pin_2, - }, -}; const struct pios_servo_cfg pios_servo_cfg = { - .tim_base_init = { - .TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1, - .TIM_ClockDivision = TIM_CKD_DIV1, - .TIM_CounterMode = TIM_CounterMode_Up, - .TIM_Period = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1), - .TIM_RepetitionCounter = 0x0000, - }, .tim_oc_init = { .TIM_OCMode = TIM_OCMode_PWM1, .TIM_OutputState = TIM_OutputState_Enable, .TIM_OutputNState = TIM_OutputNState_Disable, - .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, + .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, .TIM_OCPolarity = TIM_OCPolarity_High, .TIM_OCNPolarity = TIM_OCPolarity_High, .TIM_OCIdleState = TIM_OCIdleState_Reset, .TIM_OCNIdleState = TIM_OCNIdleState_Reset, }, - .gpio_init = { - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - .remap = GPIO_PartialRemap_TIM3, - .channels = pios_servo_channels, - .num_channels = NELEMENTS(pios_servo_channels), + .channels = pios_tim_servoport_all_pins, + .num_channels = NELEMENTS(pios_tim_servoport_all_pins), }; -#if defined(PIOS_INCLUDE_PWM) && defined(PIOS_INCLUDE_PPM) -#error Cannot define both PIOS_INCLUDE_PWM and PIOS_INCLUDE_PPM at the same time (yet) -#endif - /* * PPM Inputs */ #if defined(PIOS_INCLUDE_PPM) #include -void TIM4_IRQHandler(); -void TIM4_IRQHandler() __attribute__ ((alias ("PIOS_TIM4_irq_handler"))); const struct pios_ppm_cfg pios_ppm_cfg = { - .tim_base_init = { - .TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1, /* For 1 uS accuracy */ - .TIM_ClockDivision = TIM_CKD_DIV1, - .TIM_CounterMode = TIM_CounterMode_Up, - .TIM_Period = 0xFFFF, /* shared timer, make sure init correctly in outputs */ - .TIM_RepetitionCounter = 0x0000, - }, .tim_ic_init = { - .TIM_Channel = TIM_Channel_1, .TIM_ICPolarity = TIM_ICPolarity_Rising, .TIM_ICSelection = TIM_ICSelection_DirectTI, .TIM_ICPrescaler = TIM_ICPSC_DIV1, .TIM_ICFilter = 0x0, }, - .gpio_init = { - .GPIO_Pin = GPIO_Pin_6, - .GPIO_Mode = GPIO_Mode_IPD, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - .remap = 0, - .irq = { - .init = { - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .timer = TIM4, - .port = GPIOB, - .ccr = TIM_IT_CC1, + /* Use only the first channel for ppm */ + .channels = &pios_tim_rcvrport_all_channels[0], + .num_channels = 1, }; -void PIOS_TIM4_irq_handler() -{ - PIOS_PPM_irq_handler(); -} #endif /* PIOS_INCLUDE_PPM */ /* @@ -688,98 +821,16 @@ void PIOS_TIM4_irq_handler() #if defined(PIOS_INCLUDE_PWM) #include -static const struct pios_pwm_channel pios_pwm_channels[] = { - { - .timer = TIM4, - .port = GPIOB, - .ccr = TIM_IT_CC1, - .channel = TIM_Channel_1, - .pin = GPIO_Pin_6, - }, - { - .timer = TIM3, - .port = GPIOB, - .ccr = TIM_IT_CC2, - .channel = TIM_Channel_2, - .pin = GPIO_Pin_5, - }, - { - .timer = TIM3, - .port = GPIOB, - .ccr = TIM_IT_CC3, - .channel = TIM_Channel_3, - .pin = GPIO_Pin_0 - }, - { - .timer = TIM3, - .port = GPIOB, - .ccr = TIM_IT_CC4, - .channel = TIM_Channel_4, - .pin = GPIO_Pin_1, - }, - { - .timer = TIM2, - .port = GPIOA, - .ccr = TIM_IT_CC1, - .channel = TIM_Channel_1, - .pin = GPIO_Pin_0, - }, - { - .timer = TIM2, - .port = GPIOA, - .ccr = TIM_IT_CC2, - .channel = TIM_Channel_2, - .pin = GPIO_Pin_1, - }, -}; - -void TIM2_IRQHandler(); -void TIM3_IRQHandler(); -void TIM4_IRQHandler(); -void TIM2_IRQHandler() __attribute__ ((alias ("PIOS_TIM2_irq_handler"))); -void TIM3_IRQHandler() __attribute__ ((alias ("PIOS_TIM3_irq_handler"))); -void TIM4_IRQHandler() __attribute__ ((alias ("PIOS_TIM4_irq_handler"))); const struct pios_pwm_cfg pios_pwm_cfg = { - .tim_base_init = { - .TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1, - .TIM_ClockDivision = TIM_CKD_DIV1, - .TIM_CounterMode = TIM_CounterMode_Up, - .TIM_Period = 0xFFFF, - .TIM_RepetitionCounter = 0x0000, - }, .tim_ic_init = { .TIM_ICPolarity = TIM_ICPolarity_Rising, .TIM_ICSelection = TIM_ICSelection_DirectTI, .TIM_ICPrescaler = TIM_ICPSC_DIV1, - .TIM_ICFilter = 0x0, + .TIM_ICFilter = 0x0, }, - .gpio_init = { - .GPIO_Mode = GPIO_Mode_IPD, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - .remap = 0, - .irq = { - .init = { - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .channels = pios_pwm_channels, - .num_channels = NELEMENTS(pios_pwm_channels), + .channels = pios_tim_rcvrport_all_channels, + .num_channels = NELEMENTS(pios_tim_rcvrport_all_channels), }; -void PIOS_TIM2_irq_handler() -{ - PIOS_PWM_irq_handler(TIM2); -} -void PIOS_TIM3_irq_handler() -{ - PIOS_PWM_irq_handler(TIM3); -} -void PIOS_TIM4_irq_handler() -{ - PIOS_PWM_irq_handler(TIM4); -} #endif #if defined(PIOS_INCLUDE_I2C) @@ -931,6 +982,12 @@ void PIOS_Board_Init(void) { /* Initialize the task monitor library */ TaskMonitorInitialize(); + /* Set up pulse timers */ + PIOS_TIM_InitClock(&tim_1_cfg); + PIOS_TIM_InitClock(&tim_2_cfg); + PIOS_TIM_InitClock(&tim_3_cfg); + PIOS_TIM_InitClock(&tim_4_cfg); + /* Configure the main IO port */ uint8_t hwsettings_DSMxBind; HwSettingsDSMxBindGet(&hwsettings_DSMxBind); @@ -1114,22 +1171,30 @@ void PIOS_Board_Init(void) { break; case HWSETTINGS_RCVRPORT_PWM: #if defined(PIOS_INCLUDE_PWM) - PIOS_PWM_Init(); - uint32_t pios_pwm_rcvr_id; - if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, 0)) { - PIOS_Assert(0); + { + uint32_t pios_pwm_id; + PIOS_PWM_Init(&pios_pwm_id, &pios_pwm_cfg); + + uint32_t pios_pwm_rcvr_id; + if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id; } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id; #endif /* PIOS_INCLUDE_PWM */ break; case HWSETTINGS_RCVRPORT_PPM: #if defined(PIOS_INCLUDE_PPM) - PIOS_PPM_Init(); - uint32_t pios_ppm_rcvr_id; - if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, 0)) { - PIOS_Assert(0); + { + uint32_t pios_ppm_id; + PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg); + + uint32_t pios_ppm_rcvr_id; + if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; #endif /* PIOS_INCLUDE_PPM */ break; } @@ -1139,14 +1204,19 @@ void PIOS_Board_Init(void) { PIOS_GCSRCVR_Init(); uint32_t pios_gcsrcvr_rcvr_id; if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, 0)) { - PIOS_Assert(0); + PIOS_Assert(0); } pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id; #endif /* PIOS_INCLUDE_GCSRCVR */ /* Remap AFIO pin */ GPIO_PinRemapConfig( GPIO_Remap_SWJ_NoJTRST, ENABLE); - PIOS_Servo_Init(); + +#ifndef PIOS_DEBUG_ENABLE_DEBUG_PINS + PIOS_Servo_Init(&pios_servo_cfg); +#else + PIOS_DEBUG_Init(&pios_tim_servo_all_channels, NELEMENTS(pios_tim_servo_all_channels)); +#endif /* PIOS_DEBUG_ENABLE_DEBUG_PINS */ PIOS_ADC_Init(); PIOS_GPIO_Init(); diff --git a/flight/Libraries/ahrs_comm_objects.c b/flight/Libraries/ahrs_comm_objects.c index 5fc4f1b20..0f98e6a6c 100644 --- a/flight/Libraries/ahrs_comm_objects.c +++ b/flight/Libraries/ahrs_comm_objects.c @@ -24,6 +24,7 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ +#include "pios.h" #include "ahrs_spi_comm.h" #include "pios_debug.h" diff --git a/flight/OpenPilot/Makefile b/flight/OpenPilot/Makefile index 7d3cdf514..5d7de094f 100644 --- a/flight/OpenPilot/Makefile +++ b/flight/OpenPilot/Makefile @@ -164,6 +164,7 @@ SRC += $(PIOSSTM32F10X)/pios_ppm.c SRC += $(PIOSSTM32F10X)/pios_pwm.c SRC += $(PIOSSTM32F10X)/pios_spektrum.c SRC += $(PIOSSTM32F10X)/pios_sbus.c +SRC += $(PIOSSTM32F10X)/pios_tim.c SRC += $(PIOSSTM32F10X)/pios_debug.c SRC += $(PIOSSTM32F10X)/pios_gpio.c SRC += $(PIOSSTM32F10X)/pios_exti.c diff --git a/flight/OpenPilot/System/inc/pios_config.h b/flight/OpenPilot/System/inc/pios_config.h index 4e6657cef..8e37dc95a 100644 --- a/flight/OpenPilot/System/inc/pios_config.h +++ b/flight/OpenPilot/System/inc/pios_config.h @@ -47,7 +47,7 @@ #define PIOS_INCLUDE_SPEKTRUM //#define PIOS_INCLUDE_SBUS #define PIOS_INCLUDE_PWM -//#define PIOS_INCLUDE_PPM +#define PIOS_INCLUDE_PPM #define PIOS_INCLUDE_TELEMETRY_RF diff --git a/flight/OpenPilot/System/pios_board.c b/flight/OpenPilot/System/pios_board.c index 2905c500c..e17ad5653 100644 --- a/flight/OpenPilot/System/pios_board.c +++ b/flight/OpenPilot/System/pios_board.c @@ -302,6 +302,89 @@ void PIOS_ADC_handler() { PIOS_ADC_DMA_Handler(); } +#include "pios_tim_priv.h" + +static const TIM_TimeBaseInitTypeDef tim_4_8_time_base = { + .TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1, + .TIM_ClockDivision = TIM_CKD_DIV1, + .TIM_CounterMode = TIM_CounterMode_Up, + .TIM_Period = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1), + .TIM_RepetitionCounter = 0x0000, +}; + +static const struct pios_tim_clock_cfg tim_4_cfg = { + .timer = TIM4, + .time_base_init = &tim_4_8_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM4_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_8_cfg = { + .timer = TIM8, + .time_base_init = &tim_4_8_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM8_CC_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const TIM_TimeBaseInitTypeDef tim_1_3_5_time_base = { + .TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1, + .TIM_ClockDivision = TIM_CKD_DIV1, + .TIM_CounterMode = TIM_CounterMode_Up, + .TIM_Period = 0xFFFF, + .TIM_RepetitionCounter = 0x0000, +}; + +static const struct pios_tim_clock_cfg tim_1_cfg = { + .timer = TIM1, + .time_base_init = &tim_1_3_5_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM1_CC_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_3_cfg = { + .timer = TIM3, + .time_base_init = &tim_1_3_5_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + +static const struct pios_tim_clock_cfg tim_5_cfg = { + .timer = TIM5, + .time_base_init = &tim_1_3_5_time_base, + .irq = { + .init = { + .NVIC_IRQChannel = TIM5_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, +}; + #if defined(PIOS_INCLUDE_USART) #include "pios_usart_priv.h" @@ -536,65 +619,106 @@ static const struct pios_spektrum_cfg pios_spektrum_cfg = { * Pios servo configuration structures */ #include -static const struct pios_servo_channel pios_servo_channels[] = { +static const struct pios_tim_channel pios_tim_servoport_all_pins[] = { { .timer = TIM4, - .port = GPIOB, - .channel = TIM_Channel_1, - .pin = GPIO_Pin_6, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, }, { .timer = TIM4, - .port = GPIOB, - .channel = TIM_Channel_2, - .pin = GPIO_Pin_7, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, }, { .timer = TIM4, - .port = GPIOB, - .channel = TIM_Channel_3, - .pin = GPIO_Pin_8, + .timer_chan = TIM_Channel_3, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_8, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, }, { .timer = TIM4, - .port = GPIOB, - .channel = TIM_Channel_4, - .pin = GPIO_Pin_9, + .timer_chan = TIM_Channel_4, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, }, { .timer = TIM8, - .port = GPIOC, - .channel = TIM_Channel_1, - .pin = GPIO_Pin_6, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_6, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, }, { .timer = TIM8, - .port = GPIOC, - .channel = TIM_Channel_2, - .pin = GPIO_Pin_7, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_7, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, }, { .timer = TIM8, - .port = GPIOC, - .channel = TIM_Channel_3, - .pin = GPIO_Pin_8, + .timer_chan = TIM_Channel_3, + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_8, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, }, { .timer = TIM8, - .port = GPIOC, - .channel = TIM_Channel_4, - .pin = GPIO_Pin_9, + .timer_chan = TIM_Channel_4, + .pin = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, }, }; const struct pios_servo_cfg pios_servo_cfg = { - .tim_base_init = { - .TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1, - .TIM_ClockDivision = TIM_CKD_DIV1, - .TIM_CounterMode = TIM_CounterMode_Up, - .TIM_Period = ((1000000 / PIOS_SERVO_UPDATE_HZ) - 1), - .TIM_RepetitionCounter = 0x0000, - }, .tim_oc_init = { .TIM_OCMode = TIM_OCMode_PWM1, .TIM_OutputState = TIM_OutputState_Enable, @@ -605,13 +729,8 @@ const struct pios_servo_cfg pios_servo_cfg = { .TIM_OCIdleState = TIM_OCIdleState_Reset, .TIM_OCNIdleState = TIM_OCNIdleState_Reset, }, - .gpio_init = { - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - .remap = 0, - .channels = pios_servo_channels, - .num_channels = NELEMENTS(pios_servo_channels), + .channels = pios_tim_servoport_all_pins, + .num_channels = NELEMENTS(pios_tim_servoport_all_pins), }; @@ -620,112 +739,117 @@ const struct pios_servo_cfg pios_servo_cfg = { */ #if defined(PIOS_INCLUDE_PWM) #include -static const struct pios_pwm_channel pios_pwm_channels[] = { +static const struct pios_tim_channel pios_tim_rcvrport_all_channels[] = { { .timer = TIM1, - .port = GPIOA, - .ccr = TIM_IT_CC2, - .channel = TIM_Channel_2, - .pin = GPIO_Pin_9, - }, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Mode = GPIO_Mode_IPD, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, { .timer = TIM1, - .port = GPIOA, - .ccr = TIM_IT_CC3, - .channel = TIM_Channel_3, - .pin = GPIO_Pin_10, + .timer_chan = TIM_Channel_3, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Mode = GPIO_Mode_IPD, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, }, { .timer = TIM5, - .port = GPIOA, - .ccr = TIM_IT_CC1, - .channel = TIM_Channel_1, - .pin = GPIO_Pin_0 + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_0, + .GPIO_Mode = GPIO_Mode_IPD, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, }, { .timer = TIM1, - .port = GPIOA, - .ccr = TIM_IT_CC1, - .channel = TIM_Channel_1, - .pin = GPIO_Pin_8, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_8, + .GPIO_Mode = GPIO_Mode_IPD, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, }, { .timer = TIM3, - .port = GPIOB, - .ccr = TIM_IT_CC4, - .channel = TIM_Channel_4, - .pin = GPIO_Pin_1, + .timer_chan = TIM_Channel_4, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_1, + .GPIO_Mode = GPIO_Mode_IPD, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, }, { .timer = TIM3, - .port = GPIOB, - .ccr = TIM_IT_CC3, - .channel = TIM_Channel_3, - .pin = GPIO_Pin_0, + .timer_chan = TIM_Channel_3, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_0, + .GPIO_Mode = GPIO_Mode_IPD, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, }, { .timer = TIM3, - .port = GPIOB, - .ccr = TIM_IT_CC1, - .channel = TIM_Channel_1, - .pin = GPIO_Pin_4, + .timer_chan = TIM_Channel_1, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_4, + .GPIO_Mode = GPIO_Mode_IPD, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + .remap = GPIO_PartialRemap_TIM3, }, { .timer = TIM3, - .port = GPIOB, - .ccr = TIM_IT_CC2, - .channel = TIM_Channel_2, - .pin = GPIO_Pin_5, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_5, + .GPIO_Mode = GPIO_Mode_IPD, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + .remap = GPIO_PartialRemap_TIM3, }, }; -void TIM1_CC_IRQHandler(); -void TIM3_IRQHandler(); -void TIM5_IRQHandler(); -void TIM1_CC_IRQHandler() __attribute__ ((alias ("PIOS_TIM1_CC_irq_handler"))); -void TIM3_IRQHandler() __attribute__ ((alias ("PIOS_TIM3_irq_handler"))); -void TIM5_IRQHandler() __attribute__ ((alias ("PIOS_TIM5_irq_handler"))); const struct pios_pwm_cfg pios_pwm_cfg = { - .tim_base_init = { - .TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1, - .TIM_ClockDivision = TIM_CKD_DIV1, - .TIM_CounterMode = TIM_CounterMode_Up, - .TIM_Period = 0xFFFF, - .TIM_RepetitionCounter = 0x0000, - }, .tim_ic_init = { .TIM_ICPolarity = TIM_ICPolarity_Rising, .TIM_ICSelection = TIM_ICSelection_DirectTI, .TIM_ICPrescaler = TIM_ICPSC_DIV1, .TIM_ICFilter = 0x0, }, - .gpio_init = { - .GPIO_Mode = GPIO_Mode_IPD, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - .remap = GPIO_PartialRemap_TIM3, - .irq = { - .init = { - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .channels = pios_pwm_channels, - .num_channels = NELEMENTS(pios_pwm_channels), + .channels = pios_tim_rcvrport_all_channels, + .num_channels = NELEMENTS(pios_tim_rcvrport_all_channels), }; -void PIOS_TIM1_CC_irq_handler() -{ - PIOS_PWM_irq_handler(TIM1); -} -void PIOS_TIM3_irq_handler() -{ - PIOS_PWM_irq_handler(TIM3); -} -void PIOS_TIM5_irq_handler() -{ - PIOS_PWM_irq_handler(TIM5); -} #endif /* @@ -733,42 +857,7 @@ void PIOS_TIM5_irq_handler() */ #if defined(PIOS_INCLUDE_PPM) #include -void TIM6_IRQHandler(); -void TIM6_IRQHandler() __attribute__ ((alias ("PIOS_TIM6_irq_handler"))); -static const struct pios_ppmsv_cfg pios_ppmsv_cfg = { - .tim_base_init = { - .TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1, /* For 1 uS accuracy */ - .TIM_ClockDivision = TIM_CKD_DIV1, - .TIM_CounterMode = TIM_CounterMode_Up, - .TIM_Period = ((1000000 / 25) - 1), /* 25 Hz */ - .TIM_RepetitionCounter = 0x0000, - }, - .irq = { - .init = { - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .timer = TIM6, - .ccr = TIM_IT_Update, -}; - -void PIOS_TIM6_irq_handler(void) -{ - PIOS_PPMSV_irq_handler(); -} - -void TIM1_CC_IRQHandler(); -void TIM1_CC_IRQHandler() __attribute__ ((alias ("PIOS_TIM1_CC_irq_handler"))); static const struct pios_ppm_cfg pios_ppm_cfg = { - .tim_base_init = { - .TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1, /* For 1 uS accuracy */ - .TIM_ClockDivision = TIM_CKD_DIV1, - .TIM_CounterMode = TIM_CounterMode_Up, - .TIM_Period = 0xFFFF, - .TIM_RepetitionCounter = 0x0000, - }, .tim_ic_init = { .TIM_ICPolarity = TIM_ICPolarity_Rising, .TIM_ICSelection = TIM_ICSelection_DirectTI, @@ -776,30 +865,11 @@ static const struct pios_ppm_cfg pios_ppm_cfg = { .TIM_ICFilter = 0x0, .TIM_Channel = TIM_Channel_2, }, - .gpio_init = { - .GPIO_Mode = GPIO_Mode_IPD, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Pin = GPIO_Pin_9, - }, - .remap = 0, - .irq = { - .init = { - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - .NVIC_IRQChannel = TIM1_CC_IRQn, - }, - }, - .timer = TIM1, - .port = GPIOA, - .ccr = TIM_IT_CC2, + /* Use only the first channel for ppm */ + .channels = &pios_tim_rcvrport_all_channels[0], + .num_channels = 1, }; -void PIOS_TIM1_CC_irq_handler(void) -{ - PIOS_PPM_irq_handler(); -} - #endif //PPM #if defined(PIOS_INCLUDE_I2C) @@ -1008,8 +1078,9 @@ void PIOS_Board_Init(void) { /* Remap AFIO pin */ //GPIO_PinRemapConfig( GPIO_Remap_SWJ_NoJTRST, ENABLE); - /* Debug services */ - PIOS_DEBUG_Init(); +#ifdef PIOS_DEBUG_ENABLE_DEBUG_PINS + PIOS_DEBUG_Init(&pios_tim_servo_all_channels, NELEMENTS(pios_tim_servo_all_channels)); +#endif /* PIOS_DEBUG_ENABLE_DEBUG_PINS */ /* Delay system */ PIOS_DELAY_Init(); @@ -1040,6 +1111,14 @@ void PIOS_Board_Init(void) { /* Initialize the task monitor library */ TaskMonitorInitialize(); + /* Set up pulse timers */ + PIOS_TIM_InitClock(&tim_1_cfg); + PIOS_TIM_InitClock(&tim_3_cfg); + PIOS_TIM_InitClock(&tim_5_cfg); + + PIOS_TIM_InitClock(&tim_4_cfg); + PIOS_TIM_InitClock(&tim_8_cfg); + /* Prepare the AHRS Comms upper layer protocol */ AhrsInitComms(); @@ -1107,7 +1186,10 @@ void PIOS_Board_Init(void) { break; } - PIOS_Servo_Init(); +#ifndef PIOS_DEBUG_ENABLE_DEBUG_PINS + PIOS_Servo_Init(&pios_servo_cfg); +#endif /* PIOS_DEBUG_ENABLE_DEBUG_PINS */ + PIOS_ADC_Init(); PIOS_GPIO_Init(); @@ -1153,22 +1235,30 @@ void PIOS_Board_Init(void) { break; case HWSETTINGS_RCVRPORT_PWM: #if defined(PIOS_INCLUDE_PWM) - PIOS_PWM_Init(); - uint32_t pios_pwm_rcvr_id; - if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, 0)) { - PIOS_Assert(0); + { + uint32_t pios_pwm_id; + PIOS_PWM_Init(&pios_pwm_id, &pios_pwm_cfg); + + uint32_t pios_pwm_rcvr_id; + if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, pios_pwm_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id; } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id; #endif /* PIOS_INCLUDE_PWM */ break; case HWSETTINGS_RCVRPORT_PPM: #if defined(PIOS_INCLUDE_PPM) - PIOS_PPM_Init(); - uint32_t pios_ppm_rcvr_id; - if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, 0)) { - PIOS_Assert(0); + { + uint32_t pios_ppm_id; + PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_cfg); + + uint32_t pios_ppm_rcvr_id; + if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; #endif /* PIOS_INCLUDE_PPM */ break; } diff --git a/flight/PiOS/Boards/STM32103CB_CC_Rev1.h b/flight/PiOS/Boards/STM32103CB_CC_Rev1.h index 8c1467b0d..42b3a5e4d 100644 --- a/flight/PiOS/Boards/STM32103CB_CC_Rev1.h +++ b/flight/PiOS/Boards/STM32103CB_CC_Rev1.h @@ -212,11 +212,13 @@ extern uint32_t pios_com_telem_usb_id; //------------------------- // Receiver PPM input //------------------------- +#define PIOS_PPM_MAX_DEVS 1 #define PIOS_PPM_NUM_INPUTS 12 //------------------------- // Receiver PWM input //------------------------- +#define PIOS_PWM_MAX_DEVS 1 #define PIOS_PWM_NUM_INPUTS 6 //------------------------- @@ -231,6 +233,11 @@ extern uint32_t pios_com_telem_usb_id; #define PIOS_SERVO_UPDATE_HZ 50 #define PIOS_SERVOS_INITIAL_POSITION 0 /* dont want to start motors, have no pulse till settings loaded */ +//-------------------------- +// Timer controller settings +//-------------------------- +#define PIOS_TIM_MAX_DEVS 3 + //------------------------- // GPIO //------------------------- diff --git a/flight/PiOS/Boards/STM3210E_OP.h b/flight/PiOS/Boards/STM3210E_OP.h index 159ebbd88..ea925a271 100644 --- a/flight/PiOS/Boards/STM3210E_OP.h +++ b/flight/PiOS/Boards/STM3210E_OP.h @@ -185,11 +185,13 @@ extern uint32_t pios_com_aux_id; //------------------------- // Receiver PPM input //------------------------- +#define PIOS_PPM_MAX_DEVS 1 #define PIOS_PPM_NUM_INPUTS 12 //------------------------- // Receiver PWM input //------------------------- +#define PIOS_PWM_MAX_DEVS 1 #define PIOS_PWM_NUM_INPUTS 8 //------------------------- @@ -204,6 +206,11 @@ extern uint32_t pios_com_aux_id; #define PIOS_SERVO_UPDATE_HZ 50 #define PIOS_SERVOS_INITIAL_POSITION 0 /* dont want to start motors, have no pulse till settings loaded */ +//-------------------------- +// Timer controller settings +//-------------------------- +#define PIOS_TIM_MAX_DEVS 3 + //------------------------- // ADC // PIOS_ADC_PinGet(0) = Temperature Sensor (On-board) diff --git a/flight/PiOS/STM32F10x/link_STM32103CB_CC_Rev1_sections.ld b/flight/PiOS/STM32F10x/link_STM32103CB_CC_Rev1_sections.ld index 57530d320..568dddffa 100644 --- a/flight/PiOS/STM32F10x/link_STM32103CB_CC_Rev1_sections.ld +++ b/flight/PiOS/STM32F10x/link_STM32103CB_CC_Rev1_sections.ld @@ -1,5 +1,5 @@ /* This is the size of the stack for all FreeRTOS IRQs */ -_irq_stack_size = 0x180; +_irq_stack_size = 0x1A0; /* This is the size of the stack for early init: life span is until scheduler starts */ _init_stack_size = 0x100; diff --git a/flight/PiOS/STM32F10x/pios_debug.c b/flight/PiOS/STM32F10x/pios_debug.c index ca4f67dc5..18ea0071b 100644 --- a/flight/PiOS/STM32F10x/pios_debug.c +++ b/flight/PiOS/STM32F10x/pios_debug.c @@ -34,39 +34,41 @@ // Global variables const char *PIOS_DEBUG_AssertMsg = "ASSERT FAILED"; -#include -extern const struct pios_servo_channel pios_servo_channels[]; -#define PIOS_SERVO_GPIO_PORT_1TO4 pios_servo_channels[0].port -#define PIOS_SERVO_GPIO_PORT_5TO8 pios_servo_channels[4].port -#define PIOS_SERVO_GPIO_PIN_1 pios_servo_channels[0].pin -#define PIOS_SERVO_GPIO_PIN_2 pios_servo_channels[1].pin -#define PIOS_SERVO_GPIO_PIN_3 pios_servo_channels[2].pin -#define PIOS_SERVO_GPIO_PIN_4 pios_servo_channels[3].pin -#define PIOS_SERVO_GPIO_PIN_5 pios_servo_channels[4].pin -#define PIOS_SERVO_GPIO_PIN_6 pios_servo_channels[5].pin -#define PIOS_SERVO_GPIO_PIN_7 pios_servo_channels[6].pin -#define PIOS_SERVO_GPIO_PIN_8 pios_servo_channels[7].pin -/* Private Function Prototypes */ +#ifdef PIOS_ENABLE_DEBUG_PINS +static const struct pios_tim_channel * debug_channels; +static uint8_t debug_num_channels; +#endif /* PIOS_ENABLE_DEBUG_PINS */ /** * Initialise Debug-features */ -void PIOS_DEBUG_Init(void) +void PIOS_DEBUG_Init(const struct pios_tim_channel * channels, uint8_t num_channels) { #ifdef PIOS_ENABLE_DEBUG_PINS - // Initialise Servo pins as standard output pins - GPIO_InitTypeDef GPIO_InitStructure; - GPIO_StructInit(&GPIO_InitStructure); - GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; - GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; - GPIO_InitStructure.GPIO_Pin = PIOS_SERVO_GPIO_PIN_1 | PIOS_SERVO_GPIO_PIN_2 | PIOS_SERVO_GPIO_PIN_3 | PIOS_SERVO_GPIO_PIN_4; - GPIO_Init(PIOS_SERVO_GPIO_PORT_1TO4, &GPIO_InitStructure); - GPIO_InitStructure.GPIO_Pin = PIOS_SERVO_GPIO_PIN_5 | PIOS_SERVO_GPIO_PIN_6 | PIOS_SERVO_GPIO_PIN_7 | PIOS_SERVO_GPIO_PIN_8; - GPIO_Init(PIOS_SERVO_GPIO_PORT_5TO8, &GPIO_InitStructure); + PIOS_Assert(channels); + PIOS_Assert(num_channels); - // Drive all pins low - PIOS_SERVO_GPIO_PORT_1TO4->BRR = PIOS_SERVO_GPIO_PIN_1 | PIOS_SERVO_GPIO_PIN_2 | PIOS_SERVO_GPIO_PIN_3 | PIOS_SERVO_GPIO_PIN_4; - PIOS_SERVO_GPIO_PORT_5TO8->BRR = PIOS_SERVO_GPIO_PIN_5 | PIOS_SERVO_GPIO_PIN_6 | PIOS_SERVO_GPIO_PIN_7 | PIOS_SERVO_GPIO_PIN_8; + /* Store away the GPIOs we've been given */ + debug_channels = channels; + debug_num_channels = num_channels; + + /* Configure the GPIOs we've been given */ + for (uint8_t i = 0; i < num_channels; i++) { + const struct pios_tim_channel * chan = &channels[i]; + + // Initialise pins as standard output pins + GPIO_InitTypeDef GPIO_InitStructure; + GPIO_StructInit(&GPIO_InitStructure); + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; + GPIO_InitStructure.GPIO_Pin = chan->init->GPIO_Pin; + + /* Initialize the GPIO */ + GPIO_Init(chan->init->port, &GPIO_InitStructure); + + /* Set the pin low */ + GPIO_WriteBit(chan->init->port, chan->init->GPIO_Pin, Bit_RESET); + } #endif // PIOS_ENABLE_DEBUG_PINS } @@ -74,14 +76,17 @@ void PIOS_DEBUG_Init(void) * Set debug-pin high * \param pin 0 for S1 output */ -void PIOS_DEBUG_PinHigh(uint8_t Pin) +void PIOS_DEBUG_PinHigh(uint8_t pin) { #ifdef PIOS_ENABLE_DEBUG_PINS - if (Pin < 4) { - PIOS_SERVO_GPIO_PORT_1TO4->BSRR = (PIOS_SERVO_GPIO_PIN_1 << Pin); - } else if (Pin <= 7) { - PIOS_SERVO_GPIO_PORT_5TO8->BSRR = (PIOS_SERVO_GPIO_PIN_5 << (Pin - 4)); + if (!debug_channels || pin >= debug_num_channels) { + return; } + + const struct pios_tim_channel * chan = &debug_channels[pin]; + + GPIO_WriteBit(chan->init->port, chan->init->GPIO_Pin, Bit_Set); + #endif // PIOS_ENABLE_DEBUG_PINS } @@ -89,14 +94,17 @@ void PIOS_DEBUG_PinHigh(uint8_t Pin) * Set debug-pin low * \param pin 0 for S1 output */ -void PIOS_DEBUG_PinLow(uint8_t Pin) +void PIOS_DEBUG_PinLow(uint8_t pin) { #ifdef PIOS_ENABLE_DEBUG_PINS - if (Pin < 4) { - PIOS_SERVO_GPIO_PORT_1TO4->BRR = (PIOS_SERVO_GPIO_PIN_1 << Pin); - } else if (Pin <= 7) { - PIOS_SERVO_GPIO_PORT_5TO8->BRR = (PIOS_SERVO_GPIO_PIN_5 << (Pin - 4)); + if (!debug_channels || pin >= debug_num_channels) { + return; } + + const struct pios_tim_channel * chan = &debug_channels[pin]; + + GPIO_WriteBit(chan->init->port, chan->init->GPIO_Pin, Bit_RESET); + #endif // PIOS_ENABLE_DEBUG_PINS } @@ -104,11 +112,22 @@ void PIOS_DEBUG_PinLow(uint8_t Pin) void PIOS_DEBUG_PinValue8Bit(uint8_t value) { #ifdef PIOS_ENABLE_DEBUG_PINS + if (!debug_channels) { + return; + } + uint32_t bsrr_l = ( ((~value)&0x0F)<<(16+6) ) | ((value & 0x0F)<<6); uint32_t bsrr_h = ( ((~value)&0xF0)<<(16+6-4) ) | ((value & 0xF0)<<(6-4)); + PIOS_IRQ_Disable(); - PIOS_SERVO_GPIO_PORT_1TO4->BSRR = bsrr_l; - PIOS_SERVO_GPIO_PORT_5TO8->BSRR = bsrr_h; + + /* + * This is sketchy since it assumes a particular ordering + * and bitwise layout of the channels provided to the debug code. + */ + debug_channels[0].init.port->BSRR = bsrr_l; + debug_channels[4].init.port->BSRR = bsrr_h; + PIOS_IRQ_Enable(); #endif // PIOS_ENABLE_DEBUG_PINS } @@ -116,8 +135,16 @@ void PIOS_DEBUG_PinValue8Bit(uint8_t value) void PIOS_DEBUG_PinValue4BitL(uint8_t value) { #ifdef PIOS_ENABLE_DEBUG_PINS + if (!debug_channels) { + return; + } + + /* + * This is sketchy since it assumes a particular ordering + * and bitwise layout of the channels provided to the debug code. + */ uint32_t bsrr_l = ((~(value & 0x0F)<<(16+6))) | ((value & 0x0F)<<6); - PIOS_SERVO_GPIO_PORT_1TO4->BSRR = bsrr_l; + debug_channels[0].init.port->BSRR = bsrr_l; #endif // PIOS_ENABLE_DEBUG_PINS } diff --git a/flight/PiOS/STM32F10x/pios_ppm.c b/flight/PiOS/STM32F10x/pios_ppm.c index fe7421a17..0b7a1c5cd 100644 --- a/flight/PiOS/STM32F10x/pios_ppm.c +++ b/flight/PiOS/STM32F10x/pios_ppm.c @@ -51,104 +51,136 @@ const struct pios_rcvr_driver pios_ppm_rcvr_driver = { /* Local Variables */ static TIM_ICInitTypeDef TIM_ICInitStructure; -static uint8_t PulseIndex; -static uint32_t PreviousTime; -static uint32_t CurrentTime; -static uint32_t DeltaTime; -static uint32_t CaptureValue[PIOS_PPM_IN_MAX_NUM_CHANNELS]; -static uint32_t CaptureValueNewFrame[PIOS_PPM_IN_MAX_NUM_CHANNELS]; -static uint32_t LargeCounter; -static int8_t NumChannels; -static int8_t NumChannelsPrevFrame; -static uint8_t NumChannelCounter; - -static uint8_t supv_timer = 0; -static bool Tracking; -static bool Fresh; static void PIOS_PPM_Supervisor(uint32_t ppm_id); -void PIOS_PPM_Init(void) +enum pios_ppm_dev_magic { + PIOS_PPM_DEV_MAGIC = 0xee014d8b, +}; + +struct pios_ppm_dev { + enum pios_ppm_dev_magic magic; + const struct pios_ppm_cfg * cfg; + + uint8_t PulseIndex; + uint32_t PreviousTime; + uint32_t CurrentTime; + uint32_t DeltaTime; + uint32_t CaptureValue[PIOS_PPM_IN_MAX_NUM_CHANNELS]; + uint32_t CaptureValueNewFrame[PIOS_PPM_IN_MAX_NUM_CHANNELS]; + uint32_t LargeCounter; + int8_t NumChannels; + int8_t NumChannelsPrevFrame; + uint8_t NumChannelCounter; + + uint8_t supv_timer; + bool Tracking; + bool Fresh; +}; + +static bool PIOS_PPM_validate(struct pios_ppm_dev * ppm_dev) { - /* Flush counter variables */ - int32_t i; + return (ppm_dev->magic == PIOS_PPM_DEV_MAGIC); +} - PulseIndex = 0; - PreviousTime = 0; - CurrentTime = 0; - DeltaTime = 0; - LargeCounter = 0; - NumChannels = -1; - NumChannelsPrevFrame = -1; - NumChannelCounter = 0; - Tracking = FALSE; - Fresh = FALSE; +#if defined(PIOS_INCLUDE_FREERTOS) && 0 +static struct pios_ppm_dev * PIOS_PPM_alloc(void) +{ + struct pios_ppm_dev * ppm_dev; - for (i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) { - CaptureValue[i] = 0; - CaptureValueNewFrame[i] = 0; + ppm_dev = (struct pios_ppm_dev *)malloc(sizeof(*ppm_dev)); + if (!ppm_dev) return(NULL); + + ppm_dev->magic = PIOS_PPM_DEV_MAGIC; + return(ppm_dev); +} +#else +static struct pios_ppm_dev pios_ppm_devs[PIOS_PPM_MAX_DEVS]; +static uint8_t pios_ppm_num_devs; +static struct pios_ppm_dev * PIOS_PPM_alloc(void) +{ + struct pios_ppm_dev * ppm_dev; + + if (pios_ppm_num_devs >= PIOS_PPM_MAX_DEVS) { + return (NULL); } - NVIC_InitTypeDef NVIC_InitStructure = pios_ppm_cfg.irq.init; + ppm_dev = &pios_ppm_devs[pios_ppm_num_devs++]; + ppm_dev->magic = PIOS_PPM_DEV_MAGIC; - /* Enable appropriate clock to timer module */ - switch((int32_t) pios_ppm_cfg.timer) { - case (int32_t)TIM1: - NVIC_InitStructure.NVIC_IRQChannel = TIM1_CC_IRQn; - RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE); - break; - case (int32_t)TIM2: - NVIC_InitStructure.NVIC_IRQChannel = TIM2_IRQn; - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); - break; - case (int32_t)TIM3: - NVIC_InitStructure.NVIC_IRQChannel = TIM3_IRQn; - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE); - break; - case (int32_t)TIM4: - NVIC_InitStructure.NVIC_IRQChannel = TIM4_IRQn; - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE); - break; -#ifdef STM32F10X_HD - case (int32_t)TIM5: - NVIC_InitStructure.NVIC_IRQChannel = TIM5_IRQn; - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, ENABLE); - break; - case (int32_t)TIM6: - NVIC_InitStructure.NVIC_IRQChannel = TIM6_IRQn; - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6, ENABLE); - break; - case (int32_t)TIM7: - NVIC_InitStructure.NVIC_IRQChannel = TIM7_IRQn; - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM7, ENABLE); - break; - case (int32_t)TIM8: - NVIC_InitStructure.NVIC_IRQChannel = TIM8_CC_IRQn; - RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8, ENABLE); - break; + return (ppm_dev); +} #endif + +static void PIOS_PPM_tim_overflow_cb (uint32_t id, uint32_t context, uint8_t channel, uint16_t count); +static void PIOS_PPM_tim_edge_cb (uint32_t id, uint32_t context, uint8_t channel, uint16_t count); +const static struct pios_tim_callbacks tim_callbacks = { + .overflow = PIOS_PPM_tim_overflow_cb, + .edge = PIOS_PPM_tim_edge_cb, +}; + +extern int32_t PIOS_PPM_Init(uint32_t * ppm_id, const struct pios_ppm_cfg * cfg) +{ + PIOS_DEBUG_Assert(ppm_id); + PIOS_DEBUG_Assert(cfg); + + struct pios_ppm_dev * ppm_dev; + + ppm_dev = (struct pios_ppm_dev *) PIOS_PPM_alloc(); + if (!ppm_dev) goto out_fail; + + /* Bind the configuration to the device instance */ + ppm_dev->cfg = cfg; + + /* Set up the state variables */ + ppm_dev->PulseIndex = 0; + ppm_dev->PreviousTime = 0; + ppm_dev->CurrentTime = 0; + ppm_dev->DeltaTime = 0; + ppm_dev->LargeCounter = 0; + ppm_dev->NumChannels = -1; + ppm_dev->NumChannelsPrevFrame = -1; + ppm_dev->NumChannelCounter = 0; + ppm_dev->Tracking = FALSE; + ppm_dev->Fresh = FALSE; + + for (uint8_t i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) { + /* Flush counter variables */ + ppm_dev->CaptureValue[i] = 0; + ppm_dev->CaptureValueNewFrame[i] = 0; + } - /* Enable timer interrupts */ - NVIC_Init(&NVIC_InitStructure); - /* Configure input pins */ - GPIO_InitTypeDef GPIO_InitStructure = pios_ppm_cfg.gpio_init; - GPIO_Init(pios_ppm_cfg.port, &GPIO_InitStructure); + uint32_t tim_id; + if (PIOS_TIM_InitChannels(&tim_id, cfg->channels, cfg->num_channels, &tim_callbacks, (uint32_t)ppm_dev)) { + return -1; + } - /* Configure timer for input capture */ - TIM_ICInitStructure = pios_ppm_cfg.tim_ic_init; - TIM_ICInit(pios_ppm_cfg.timer, &TIM_ICInitStructure); + /* Configure the channels to be in capture/compare mode */ + for (uint8_t i = 0; i < cfg->num_channels; i++) { + const struct pios_tim_channel * chan = &cfg->channels[i]; - /* Configure timer clocks */ - TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = pios_ppm_cfg.tim_base_init; - TIM_InternalClockConfig(pios_ppm_cfg.timer); - TIM_TimeBaseInit(pios_ppm_cfg.timer, &TIM_TimeBaseStructure); + /* Configure timer for input capture */ + TIM_ICInitTypeDef TIM_ICInitStructure = cfg->tim_ic_init; + TIM_ICInitStructure.TIM_Channel = chan->timer_chan; + TIM_ICInit(chan->timer, &TIM_ICInitStructure); - /* Enable the Capture Compare Interrupt Request */ - TIM_ITConfig(pios_ppm_cfg.timer, pios_ppm_cfg.ccr | TIM_IT_Update, ENABLE); - - /* Enable timers */ - TIM_Cmd(pios_ppm_cfg.timer, ENABLE); + /* Enable the Capture Compare Interrupt Request */ + switch (chan->timer_chan) { + case TIM_Channel_1: + TIM_ITConfig(chan->timer, TIM_IT_CC1 | TIM_IT_Update, ENABLE); + break; + case TIM_Channel_2: + TIM_ITConfig(chan->timer, TIM_IT_CC2 | TIM_IT_Update, ENABLE); + break; + case TIM_Channel_3: + TIM_ITConfig(chan->timer, TIM_IT_CC3 | TIM_IT_Update, ENABLE); + break; + case TIM_Channel_4: + TIM_ITConfig(chan->timer, TIM_IT_CC4 | TIM_IT_Update, ENABLE); + break; + } + } /* Setup local variable which stays in this scope */ /* Doing this here and using a local variable saves doing it in the ISR */ @@ -156,9 +188,16 @@ void PIOS_PPM_Init(void) TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1; TIM_ICInitStructure.TIM_ICFilter = 0x0; - if (!PIOS_RTC_RegisterTickCallback(PIOS_PPM_Supervisor, 0)) { + if (!PIOS_RTC_RegisterTickCallback(PIOS_PPM_Supervisor, (uint32_t)ppm_dev)) { PIOS_DEBUG_Assert(0); } + + *ppm_id = (uint32_t)ppm_dev; + + return(0); + +out_fail: + return(-1); } /** @@ -169,142 +208,146 @@ void PIOS_PPM_Init(void) */ static int32_t PIOS_PPM_Get(uint32_t rcvr_id, uint8_t channel) { - /* Return error if channel not available */ - if (channel >= PIOS_PPM_IN_MAX_NUM_CHANNELS) { + struct pios_ppm_dev * ppm_dev = (struct pios_ppm_dev *)rcvr_id; + + if (!PIOS_PPM_validate(ppm_dev)) { + /* Invalid device specified */ return -1; } - return CaptureValue[channel]; + + if (channel >= PIOS_PPM_IN_MAX_NUM_CHANNELS) { + /* Channel out of range */ + return -1; + } + return ppm_dev->CaptureValue[channel]; } -/** -* Handle TIMx global interrupt request -* Some work and testing still needed, need to detect start of frame and decode pulses -* -*/ -void PIOS_PPM_irq_handler(void) +static void PIOS_PPM_tim_overflow_cb (uint32_t tim_id, uint32_t context, uint8_t channel, uint16_t count) { - /* Timer Overflow Interrupt - * The time between timer overflows must be greater than the PPM - * frame period. If a full frame has not decoded in the between timer - * overflows then capture values should be cleared. - */ + struct pios_ppm_dev * ppm_dev = (struct pios_ppm_dev *)context; - if (TIM_GetITStatus(pios_ppm_cfg.timer, TIM_IT_Update) == SET) { - /* Clear TIMx overflow interrupt pending bit */ - TIM_ClearITPendingBit(pios_ppm_cfg.timer, TIM_IT_Update); - - /* If sharing a timer with a servo output the ARR register will - be set according to the PWM period. When timer reaches the - ARR value a timer overflow interrupt will fire. We use the - interrupt accumulate a 32-bit timer. */ - LargeCounter = LargeCounter + pios_ppm_cfg.timer->ARR; + if (!PIOS_PPM_validate(ppm_dev)) { + /* Invalid device specified */ + return; } - /* Signal edge interrupt */ - if (TIM_GetITStatus(pios_ppm_cfg.timer, pios_ppm_cfg.ccr) == SET) { - PreviousTime = CurrentTime; + ppm_dev->LargeCounter += count; - switch((int32_t) pios_ppm_cfg.ccr) { - case (int32_t)TIM_IT_CC1: - CurrentTime = TIM_GetCapture1(pios_ppm_cfg.timer); - break; - case (int32_t)TIM_IT_CC2: - CurrentTime = TIM_GetCapture2(pios_ppm_cfg.timer); - break; - case (int32_t)TIM_IT_CC3: - CurrentTime = TIM_GetCapture3(pios_ppm_cfg.timer); - break; - case (int32_t)TIM_IT_CC4: - CurrentTime = TIM_GetCapture4(pios_ppm_cfg.timer); - break; + return; +} + + +static void PIOS_PPM_tim_edge_cb (uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count) +{ + /* Recover our device context */ + struct pios_ppm_dev * ppm_dev = (struct pios_ppm_dev *)context; + + if (!PIOS_PPM_validate(ppm_dev)) { + /* Invalid device specified */ + return; + } + + if (chan_idx >= ppm_dev->cfg->num_channels) { + /* Channel out of range */ + return; + } + + /* Shift the last measurement out */ + ppm_dev->PreviousTime = ppm_dev->CurrentTime; + + /* Grab the new count */ + ppm_dev->CurrentTime = count; + + /* Convert to 32-bit timer result */ + ppm_dev->CurrentTime += ppm_dev->LargeCounter; + + /* Capture computation */ + ppm_dev->DeltaTime = ppm_dev->CurrentTime - ppm_dev->PreviousTime; + + ppm_dev->PreviousTime = ppm_dev->CurrentTime; + + /* Sync pulse detection */ + if (ppm_dev->DeltaTime > PIOS_PPM_IN_MIN_SYNC_PULSE_US) { + if (ppm_dev->PulseIndex == ppm_dev->NumChannelsPrevFrame + && ppm_dev->PulseIndex >= PIOS_PPM_IN_MIN_NUM_CHANNELS + && ppm_dev->PulseIndex <= PIOS_PPM_IN_MAX_NUM_CHANNELS) + { + /* If we see n simultaneous frames of the same + number of channels we save it as our frame size */ + if (ppm_dev->NumChannelCounter < PIOS_PPM_STABLE_CHANNEL_COUNT) + ppm_dev->NumChannelCounter++; + else + ppm_dev->NumChannels = ppm_dev->PulseIndex; + } else { + ppm_dev->NumChannelCounter = 0; } - /* Clear TIMx Capture compare interrupt pending bit */ - TIM_ClearITPendingBit(pios_ppm_cfg.timer, pios_ppm_cfg.ccr); - - /* Convert to 32-bit timer result */ - CurrentTime = CurrentTime + LargeCounter; - - /* Capture computation */ - DeltaTime = CurrentTime - PreviousTime; - - PreviousTime = CurrentTime; - - /* Sync pulse detection */ - if (DeltaTime > PIOS_PPM_IN_MIN_SYNC_PULSE_US) { - if (PulseIndex == NumChannelsPrevFrame - && PulseIndex >= PIOS_PPM_IN_MIN_NUM_CHANNELS - && PulseIndex <= PIOS_PPM_IN_MAX_NUM_CHANNELS) - { - /* If we see n simultaneous frames of the same - number of channels we save it as our frame size */ - if (NumChannelCounter < PIOS_PPM_STABLE_CHANNEL_COUNT) - NumChannelCounter++; - else - NumChannels = PulseIndex; - } else { - NumChannelCounter = 0; + /* Check if the last frame was well formed */ + if (ppm_dev->PulseIndex == ppm_dev->NumChannels && ppm_dev->Tracking) { + /* The last frame was well formed */ + for (uint32_t i = 0; i < ppm_dev->NumChannels; i++) { + ppm_dev->CaptureValue[i] = ppm_dev->CaptureValueNewFrame[i]; } - - /* Check if the last frame was well formed */ - if (PulseIndex == NumChannels && Tracking) { - /* The last frame was well formed */ - for (uint32_t i = 0; i < NumChannels; i++) { - CaptureValue[i] = CaptureValueNewFrame[i]; - } - for (uint32_t i = NumChannels; - i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) { - CaptureValue[i] = PIOS_PPM_INPUT_INVALID; - } + for (uint32_t i = ppm_dev->NumChannels; + i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) { + ppm_dev->CaptureValue[i] = PIOS_PPM_INPUT_INVALID; } + } - Fresh = TRUE; - Tracking = TRUE; - NumChannelsPrevFrame = PulseIndex; - PulseIndex = 0; + ppm_dev->Fresh = TRUE; + ppm_dev->Tracking = TRUE; + ppm_dev->NumChannelsPrevFrame = ppm_dev->PulseIndex; + ppm_dev->PulseIndex = 0; - /* We rely on the supervisor to set CaptureValue to invalid - if no valid frame is found otherwise we ride over it */ + /* We rely on the supervisor to set CaptureValue to invalid + if no valid frame is found otherwise we ride over it */ - } else if (Tracking) { - /* Valid pulse duration 0.75 to 2.5 ms*/ - if (DeltaTime > PIOS_PPM_IN_MIN_CHANNEL_PULSE_US - && DeltaTime < PIOS_PPM_IN_MAX_CHANNEL_PULSE_US - && PulseIndex < PIOS_PPM_IN_MAX_NUM_CHANNELS) { - - CaptureValueNewFrame[PulseIndex] = DeltaTime; - PulseIndex++; - } else { - /* Not a valid pulse duration */ - Tracking = FALSE; - for (uint32_t i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS ; i++) { - CaptureValueNewFrame[i] = PIOS_PPM_INPUT_INVALID; - } + } else if (ppm_dev->Tracking) { + /* Valid pulse duration 0.75 to 2.5 ms*/ + if (ppm_dev->DeltaTime > PIOS_PPM_IN_MIN_CHANNEL_PULSE_US + && ppm_dev->DeltaTime < PIOS_PPM_IN_MAX_CHANNEL_PULSE_US + && ppm_dev->PulseIndex < PIOS_PPM_IN_MAX_NUM_CHANNELS) { + + ppm_dev->CaptureValueNewFrame[ppm_dev->PulseIndex] = ppm_dev->DeltaTime; + ppm_dev->PulseIndex++; + } else { + /* Not a valid pulse duration */ + ppm_dev->Tracking = FALSE; + for (uint32_t i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS ; i++) { + ppm_dev->CaptureValueNewFrame[i] = PIOS_PPM_INPUT_INVALID; } } } } static void PIOS_PPM_Supervisor(uint32_t ppm_id) { + /* Recover our device context */ + struct pios_ppm_dev * ppm_dev = (struct pios_ppm_dev *)ppm_id; + + if (!PIOS_PPM_validate(ppm_dev)) { + /* Invalid device specified */ + return; + } + /* * RTC runs at 625Hz so divide down the base rate so * that this loop runs at 25Hz. */ - if(++supv_timer < 25) { + if(++(ppm_dev->supv_timer) < 25) { return; } - supv_timer = 0; + ppm_dev->supv_timer = 0; - if (!Fresh) { - Tracking = FALSE; + if (!ppm_dev->Fresh) { + ppm_dev->Tracking = FALSE; for (int32_t i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS ; i++) { - CaptureValue[i] = PIOS_PPM_INPUT_INVALID; - CaptureValueNewFrame[i] = PIOS_PPM_INPUT_INVALID; + ppm_dev->CaptureValue[i] = PIOS_PPM_INPUT_INVALID; + ppm_dev->CaptureValueNewFrame[i] = PIOS_PPM_INPUT_INVALID; } } - Fresh = FALSE; + ppm_dev->Fresh = FALSE; } #endif diff --git a/flight/PiOS/STM32F10x/pios_pwm.c b/flight/PiOS/STM32F10x/pios_pwm.c index 433c272bb..9717f66fc 100644 --- a/flight/PiOS/STM32F10x/pios_pwm.c +++ b/flight/PiOS/STM32F10x/pios_pwm.c @@ -42,96 +42,124 @@ const struct pios_rcvr_driver pios_pwm_rcvr_driver = { }; /* Local Variables */ -static uint8_t CaptureState[PIOS_PWM_NUM_INPUTS]; -static uint16_t RiseValue[PIOS_PWM_NUM_INPUTS]; -static uint16_t FallValue[PIOS_PWM_NUM_INPUTS]; -static uint32_t CaptureValue[PIOS_PWM_NUM_INPUTS]; -static uint32_t CapCounter[PIOS_PWM_NUM_INPUTS]; +enum pios_pwm_dev_magic { + PIOS_PWM_DEV_MAGIC = 0xab30293c, +}; + +struct pios_pwm_dev { + enum pios_pwm_dev_magic magic; + const struct pios_pwm_cfg * cfg; + + uint8_t CaptureState[PIOS_PWM_NUM_INPUTS]; + uint16_t RiseValue[PIOS_PWM_NUM_INPUTS]; + uint16_t FallValue[PIOS_PWM_NUM_INPUTS]; + uint32_t CaptureValue[PIOS_PWM_NUM_INPUTS]; + uint32_t CapCounter[PIOS_PWM_NUM_INPUTS]; +}; + +static bool PIOS_PWM_validate(struct pios_pwm_dev * pwm_dev) +{ + return (pwm_dev->magic == PIOS_PWM_DEV_MAGIC); +} + +#if defined(PIOS_INCLUDE_FREERTOS) && 0 +static struct pios_pwm_dev * PIOS_PWM_alloc(void) +{ + struct pios_pwm_dev * pwm_dev; + + pwm_dev = (struct pios_pwm_dev *)malloc(sizeof(*pwm_dev)); + if (!pwm_dev) return(NULL); + + pwm_dev->magic = PIOS_PWM_DEV_MAGIC; + return(pwm_dev); +} +#else +static struct pios_pwm_dev pios_pwm_devs[PIOS_PWM_MAX_DEVS]; +static uint8_t pios_pwm_num_devs; +static struct pios_pwm_dev * PIOS_PWM_alloc(void) +{ + struct pios_pwm_dev * pwm_dev; + + if (pios_pwm_num_devs >= PIOS_PWM_MAX_DEVS) { + return (NULL); + } + + pwm_dev = &pios_pwm_devs[pios_pwm_num_devs++]; + pwm_dev->magic = PIOS_PWM_DEV_MAGIC; + + return (pwm_dev); +} +#endif + +static void PIOS_PWM_tim_overflow_cb (uint32_t id, uint32_t context, uint8_t channel, uint16_t count); +static void PIOS_PWM_tim_edge_cb (uint32_t id, uint32_t context, uint8_t channel, uint16_t count); +const static struct pios_tim_callbacks tim_callbacks = { + .overflow = PIOS_PWM_tim_overflow_cb, + .edge = PIOS_PWM_tim_edge_cb, +}; /** * Initialises all the pins */ -void PIOS_PWM_Init(void) +int32_t PIOS_PWM_Init(uint32_t * pwm_id, const struct pios_pwm_cfg * cfg) { - for (uint8_t i = 0; i < pios_pwm_cfg.num_channels; i++) { - /* Flush counter variables */ - CaptureState[i] = 0; - RiseValue[i] = 0; - FallValue[i] = 0; - CaptureValue[i] = 0; - - NVIC_InitTypeDef NVIC_InitStructure = pios_pwm_cfg.irq.init; - GPIO_InitTypeDef GPIO_InitStructure = pios_pwm_cfg.gpio_init; - TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = pios_pwm_cfg.tim_base_init; - TIM_ICInitTypeDef TIM_ICInitStructure = pios_pwm_cfg.tim_ic_init; - - struct pios_pwm_channel channel = pios_pwm_cfg.channels[i]; - - /* Enable appropriate clock to timer module */ - switch((int32_t) channel.timer) { - case (int32_t)TIM1: - NVIC_InitStructure.NVIC_IRQChannel = TIM1_CC_IRQn; - RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE); - break; - case (int32_t)TIM2: - NVIC_InitStructure.NVIC_IRQChannel = TIM2_IRQn; - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); - break; - case (int32_t)TIM3: - NVIC_InitStructure.NVIC_IRQChannel = TIM3_IRQn; - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE); - break; - case (int32_t)TIM4: - NVIC_InitStructure.NVIC_IRQChannel = TIM4_IRQn; - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE); - break; -#ifdef STM32F10X_HD - - case (int32_t)TIM5: - NVIC_InitStructure.NVIC_IRQChannel = TIM5_IRQn; - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, ENABLE); - break; - case (int32_t)TIM6: - NVIC_InitStructure.NVIC_IRQChannel = TIM6_IRQn; - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6, ENABLE); - break; - case (int32_t)TIM7: - NVIC_InitStructure.NVIC_IRQChannel = TIM7_IRQn; - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM7, ENABLE); - break; - case (int32_t)TIM8: - NVIC_InitStructure.NVIC_IRQChannel = TIM8_CC_IRQn; - RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8, ENABLE); - break; -#endif - } - NVIC_Init(&NVIC_InitStructure); + PIOS_DEBUG_Assert(pwm_id); + PIOS_DEBUG_Assert(cfg); + + struct pios_pwm_dev * pwm_dev; + + pwm_dev = (struct pios_pwm_dev *) PIOS_PWM_alloc(); + if (!pwm_dev) goto out_fail; + + /* Bind the configuration to the device instance */ + pwm_dev->cfg = cfg; + + for (uint8_t i = 0; i < PIOS_PWM_NUM_INPUTS; i++) { + /* Flush counter variables */ + pwm_dev->CaptureState[i] = 0; + pwm_dev->RiseValue[i] = 0; + pwm_dev->FallValue[i] = 0; + pwm_dev->CaptureValue[i] = 0; + } + + uint32_t tim_id; + if (PIOS_TIM_InitChannels(&tim_id, cfg->channels, cfg->num_channels, &tim_callbacks, (uint32_t)pwm_dev)) { + return -1; + } + + /* Configure the channels to be in capture/compare mode */ + for (uint8_t i = 0; i < cfg->num_channels; i++) { + const struct pios_tim_channel * chan = &cfg->channels[i]; - /* Enable GPIO */ - GPIO_InitStructure.GPIO_Pin = channel.pin; - GPIO_Init(channel.port, &GPIO_InitStructure); - /* Configure timer for input capture */ - TIM_ICInitStructure.TIM_Channel = channel.channel; - TIM_ICInit(channel.timer, &TIM_ICInitStructure); - - /* Configure timer clocks */ - TIM_InternalClockConfig(channel.timer); - if(channel.timer->PSC != ((PIOS_MASTER_CLOCK / 1000000) - 1)) - TIM_TimeBaseInit(channel.timer, &TIM_TimeBaseStructure); + TIM_ICInitTypeDef TIM_ICInitStructure = cfg->tim_ic_init; + TIM_ICInitStructure.TIM_Channel = chan->timer_chan; + TIM_ICInit(chan->timer, &TIM_ICInitStructure); /* Enable the Capture Compare Interrupt Request */ - TIM_ITConfig(channel.timer, channel.ccr, ENABLE); - - /* Enable timers */ - TIM_Cmd(channel.timer, ENABLE); + switch (chan->timer_chan) { + case TIM_Channel_1: + TIM_ITConfig(chan->timer, TIM_IT_CC1, ENABLE); + break; + case TIM_Channel_2: + TIM_ITConfig(chan->timer, TIM_IT_CC2, ENABLE); + break; + case TIM_Channel_3: + TIM_ITConfig(chan->timer, TIM_IT_CC3, ENABLE); + break; + case TIM_Channel_4: + TIM_ITConfig(chan->timer, TIM_IT_CC4, ENABLE); + break; + } } - if(pios_pwm_cfg.remap) { - /* Warning, I don't think this will work for multiple remaps at once */ - GPIO_PinRemapConfig(pios_pwm_cfg.remap, ENABLE); - } + *pwm_id = (uint32_t) pwm_dev; + + return (0); + +out_fail: + return (-1); } /** @@ -142,75 +170,86 @@ void PIOS_PWM_Init(void) */ static int32_t PIOS_PWM_Get(uint32_t rcvr_id, uint8_t channel) { - /* Return error if channel not available */ - if (channel >= pios_pwm_cfg.num_channels) { + struct pios_pwm_dev * pwm_dev = (struct pios_pwm_dev *)rcvr_id; + + if (!PIOS_PWM_validate(pwm_dev)) { + /* Invalid device specified */ return -1; } - return CaptureValue[channel]; + + if (channel >= PIOS_PWM_NUM_INPUTS) { + /* Channel out of range */ + return -1; + } + return pwm_dev->CaptureValue[channel]; } -void PIOS_PWM_irq_handler(TIM_TypeDef * timer) +static void PIOS_PWM_tim_overflow_cb (uint32_t tim_id, uint32_t context, uint8_t channel, uint16_t count) { - uint16_t val = 0; - for(uint8_t i = 0; i < pios_pwm_cfg.num_channels; i++) { - struct pios_pwm_channel channel = pios_pwm_cfg.channels[i]; - if ((channel.timer == timer) && (TIM_GetITStatus(channel.timer, channel.ccr) == SET)) { - - TIM_ClearITPendingBit(channel.timer, channel.ccr); - - switch(channel.channel) { - case TIM_Channel_1: - val = TIM_GetCapture1(channel.timer); - break; - case TIM_Channel_2: - val = TIM_GetCapture2(channel.timer); - break; - case TIM_Channel_3: - val = TIM_GetCapture3(channel.timer); - break; - case TIM_Channel_4: - val = TIM_GetCapture4(channel.timer); - break; - } - - if (CaptureState[i] == 0) { - RiseValue[i] = val; - } else { - FallValue[i] = val; - } - - // flip state machine and capture value here - /* Simple rise or fall state machine */ - TIM_ICInitTypeDef TIM_ICInitStructure = pios_pwm_cfg.tim_ic_init; - if (CaptureState[i] == 0) { - /* Switch states */ - CaptureState[i] = 1; - - /* Switch polarity of input capture */ - TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling; - TIM_ICInitStructure.TIM_Channel = channel.channel; - TIM_ICInit(channel.timer, &TIM_ICInitStructure); - } else { - /* Capture computation */ - if (FallValue[i] > RiseValue[i]) { - CaptureValue[i] = (FallValue[i] - RiseValue[i]); - } else { - CaptureValue[i] = ((channel.timer->ARR - RiseValue[i]) + FallValue[i]); - } - - /* Switch states */ - CaptureState[i] = 0; - - /* Increase supervisor counter */ - CapCounter[i]++; - - /* Switch polarity of input capture */ - TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising; - TIM_ICInitStructure.TIM_Channel = channel.channel; - TIM_ICInit(channel.timer, &TIM_ICInitStructure); - } - } + struct pios_pwm_dev * pwm_dev = (struct pios_pwm_dev *)context; + + if (!PIOS_PWM_validate(pwm_dev)) { + /* Invalid device specified */ + return; } + + return; +} + +static void PIOS_PWM_tim_edge_cb (uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count) +{ + /* Recover our device context */ + struct pios_pwm_dev * pwm_dev = (struct pios_pwm_dev *)context; + + if (!PIOS_PWM_validate(pwm_dev)) { + /* Invalid device specified */ + return; + } + + if (chan_idx >= pwm_dev->cfg->num_channels) { + /* Channel out of range */ + return; + } + + const struct pios_tim_channel * chan = &pwm_dev->cfg->channels[chan_idx]; + + if (pwm_dev->CaptureState[chan_idx] == 0) { + pwm_dev->RiseValue[chan_idx] = count; + } else { + pwm_dev->FallValue[chan_idx] = count; + } + + // flip state machine and capture value here + /* Simple rise or fall state machine */ + TIM_ICInitTypeDef TIM_ICInitStructure = pwm_dev->cfg->tim_ic_init; + if (pwm_dev->CaptureState[chan_idx] == 0) { + /* Switch states */ + pwm_dev->CaptureState[chan_idx] = 1; + + /* Switch polarity of input capture */ + TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling; + TIM_ICInitStructure.TIM_Channel = chan->timer_chan; + TIM_ICInit(chan->timer, &TIM_ICInitStructure); + } else { + /* Capture computation */ + if (pwm_dev->FallValue[chan_idx] > pwm_dev->RiseValue[chan_idx]) { + pwm_dev->CaptureValue[chan_idx] = (pwm_dev->FallValue[chan_idx] - pwm_dev->RiseValue[chan_idx]); + } else { + pwm_dev->CaptureValue[chan_idx] = ((chan->timer->ARR - pwm_dev->RiseValue[chan_idx]) + pwm_dev->FallValue[chan_idx]); + } + + /* Switch states */ + pwm_dev->CaptureState[chan_idx] = 0; + + /* Increase supervisor counter */ + pwm_dev->CapCounter[chan_idx]++; + + /* Switch polarity of input capture */ + TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising; + TIM_ICInitStructure.TIM_Channel = chan->timer_chan; + TIM_ICInit(chan->timer, &TIM_ICInitStructure); + } + } #endif diff --git a/flight/PiOS/STM32F10x/pios_servo.c b/flight/PiOS/STM32F10x/pios_servo.c index 229f371ce..cb7d98292 100644 --- a/flight/PiOS/STM32F10x/pios_servo.c +++ b/flight/PiOS/STM32F10x/pios_servo.c @@ -31,97 +31,55 @@ /* Project Includes */ #include "pios.h" #include "pios_servo_priv.h" +#include "pios_tim_priv.h" /* Private Function Prototypes */ +static const struct pios_servo_cfg * servo_cfg; + /** * Initialise Servos */ -void PIOS_Servo_Init(void) +int32_t PIOS_Servo_Init(const struct pios_servo_cfg * cfg) { -#ifndef PIOS_ENABLE_DEBUG_PINS -#if defined(PIOS_INCLUDE_SERVO) + uint32_t tim_id; + if (PIOS_TIM_InitChannels(&tim_id, cfg->channels, cfg->num_channels, NULL, 0)) { + return -1; + } + /* Store away the requested configuration */ + servo_cfg = cfg; - for (uint8_t i = 0; i < pios_servo_cfg.num_channels; i++) { - GPIO_InitTypeDef GPIO_InitStructure = pios_servo_cfg.gpio_init; - TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = pios_servo_cfg.tim_base_init; - TIM_OCInitTypeDef TIM_OCInitStructure = pios_servo_cfg.tim_oc_init; - - struct pios_servo_channel channel = pios_servo_cfg.channels[i]; - - /* Enable appropriate clock to timer module */ - switch((int32_t) channel.timer) { - case (int32_t)TIM1: - RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE); - break; - case (int32_t)TIM2: - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); - break; - case (int32_t)TIM3: - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE); - break; - case (int32_t)TIM4: - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE); - break; - case (int32_t)TIM5: - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, ENABLE); - break; - case (int32_t)TIM6: - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6, ENABLE); - break; - case (int32_t)TIM7: - RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM7, ENABLE); - break; - case (int32_t)TIM8: - RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8, ENABLE); - break; - } - - /* Enable GPIO */ - GPIO_InitStructure.GPIO_Pin = channel.pin; - GPIO_Init(channel.port, &GPIO_InitStructure); - - /* Enable time base */ - TIM_TimeBaseInit(channel.timer, &TIM_TimeBaseStructure); - - channel.timer->PSC = (PIOS_MASTER_CLOCK / 1000000) - 1; + /* Configure the channels to be in output compare mode */ + for (uint8_t i = 0; i < cfg->num_channels; i++) { + const struct pios_tim_channel * chan = &cfg->channels[i]; /* Set up for output compare function */ - switch(channel.channel) { + switch(chan->timer_chan) { case TIM_Channel_1: - TIM_OC1Init(channel.timer, &TIM_OCInitStructure); - TIM_OC1PreloadConfig(channel.timer, TIM_OCPreload_Enable); + TIM_OC1Init(chan->timer, &cfg->tim_oc_init); + TIM_OC1PreloadConfig(chan->timer, TIM_OCPreload_Enable); break; case TIM_Channel_2: - TIM_OC2Init(channel.timer, &TIM_OCInitStructure); - TIM_OC2PreloadConfig(channel.timer, TIM_OCPreload_Enable); + TIM_OC2Init(chan->timer, &cfg->tim_oc_init); + TIM_OC2PreloadConfig(chan->timer, TIM_OCPreload_Enable); break; case TIM_Channel_3: - TIM_OC3Init(channel.timer, &TIM_OCInitStructure); - TIM_OC3PreloadConfig(channel.timer, TIM_OCPreload_Enable); + TIM_OC3Init(chan->timer, &cfg->tim_oc_init); + TIM_OC3PreloadConfig(chan->timer, TIM_OCPreload_Enable); break; case TIM_Channel_4: - TIM_OC4Init(channel.timer, &TIM_OCInitStructure); - TIM_OC4PreloadConfig(channel.timer, TIM_OCPreload_Enable); + TIM_OC4Init(chan->timer, &cfg->tim_oc_init); + TIM_OC4PreloadConfig(chan->timer, TIM_OCPreload_Enable); break; } - RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); - TIM_ARRPreloadConfig(channel.timer, ENABLE); - TIM_CtrlPWMOutputs(channel.timer, ENABLE); - TIM_Cmd(channel.timer, ENABLE); - + TIM_ARRPreloadConfig(chan->timer, ENABLE); + TIM_CtrlPWMOutputs(chan->timer, ENABLE); + TIM_Cmd(chan->timer, ENABLE); } - if(pios_servo_cfg.remap) { - /* Warning, I don't think this will work for multiple remaps at once */ - GPIO_PinRemapConfig(pios_servo_cfg.remap, ENABLE); - } - - -#endif // PIOS_INCLUDE_SERVO -#endif // PIOS_ENABLE_DEBUG_PINS + return 0; } /** @@ -131,31 +89,31 @@ void PIOS_Servo_Init(void) */ void PIOS_Servo_SetHz(uint16_t * speeds, uint8_t banks) { -#ifndef PIOS_ENABLE_DEBUG_PINS -#if defined(PIOS_INCLUDE_SERVO) - TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = pios_servo_cfg.tim_base_init; + if (!servo_cfg) { + return; + } + + TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = servo_cfg->tim_base_init; TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1; uint8_t set = 0; - for(uint8_t i = 0; (i < pios_servo_cfg.num_channels) && (set < banks); i++) { + for(uint8_t i = 0; (i < servo_cfg->num_channels) && (set < banks); i++) { bool new = true; - struct pios_servo_channel channel = pios_servo_cfg.channels[i]; + const struct pios_tim_channel * chan = &servo_cfg->channels[i]; /* See if any previous channels use that same timer */ for(uint8_t j = 0; (j < i) && new; j++) - new &= channel.timer != pios_servo_cfg.channels[j].timer; + new &= chan->timer != servo_cfg->channels[j].timer; if(new) { TIM_TimeBaseStructure.TIM_Period = ((1000000 / speeds[set]) - 1); - TIM_TimeBaseInit(channel.timer, &TIM_TimeBaseStructure); + TIM_TimeBaseInit(chan->timer, &TIM_TimeBaseStructure); set++; } } -#endif // PIOS_INCLUDE_SERVO -#endif // PIOS_ENABLE_DEBUG_PINS } /** @@ -163,29 +121,27 @@ void PIOS_Servo_SetHz(uint16_t * speeds, uint8_t banks) * \param[in] Servo Servo number (0-7) * \param[in] Position Servo position in microseconds */ -void PIOS_Servo_Set(uint8_t Servo, uint16_t Position) +void PIOS_Servo_Set(uint8_t servo, uint16_t position) { -#ifndef PIOS_ENABLE_DEBUG_PINS -#if defined(PIOS_INCLUDE_SERVO) /* Make sure servo exists */ - if (Servo < pios_servo_cfg.num_channels && Servo >= 0) { - /* Update the position */ - - switch(pios_servo_cfg.channels[Servo].channel) { - case TIM_Channel_1: - TIM_SetCompare1(pios_servo_cfg.channels[Servo].timer, Position); - break; - case TIM_Channel_2: - TIM_SetCompare2(pios_servo_cfg.channels[Servo].timer, Position); - break; - case TIM_Channel_3: - TIM_SetCompare3(pios_servo_cfg.channels[Servo].timer, Position); - break; - case TIM_Channel_4: - TIM_SetCompare4(pios_servo_cfg.channels[Servo].timer, Position); - break; - } + if (!servo_cfg || servo >= servo_cfg->num_channels) { + return; + } + + /* Update the position */ + const struct pios_tim_channel * chan = &servo_cfg->channels[servo]; + switch(chan->timer_chan) { + case TIM_Channel_1: + TIM_SetCompare1(chan->timer, position); + break; + case TIM_Channel_2: + TIM_SetCompare2(chan->timer, position); + break; + case TIM_Channel_3: + TIM_SetCompare3(chan->timer, position); + break; + case TIM_Channel_4: + TIM_SetCompare4(chan->timer, position); + break; } -#endif // PIOS_INCLUDE_SERVO -#endif // PIOS_ENABLE_DEBUG_PINS } diff --git a/flight/PiOS/STM32F10x/pios_tim.c b/flight/PiOS/STM32F10x/pios_tim.c new file mode 100644 index 000000000..345c9e36e --- /dev/null +++ b/flight/PiOS/STM32F10x/pios_tim.c @@ -0,0 +1,391 @@ +#include "pios.h" + +#include "pios_tim.h" +#include "pios_tim_priv.h" + +enum pios_tim_dev_magic { + PIOS_TIM_DEV_MAGIC = 0x87654098, +}; + +struct pios_tim_dev { + enum pios_tim_dev_magic magic; + + const struct pios_tim_channel * channels; + uint8_t num_channels; + + const struct pios_tim_callbacks * callbacks; + uint32_t context; +}; + +#if 0 +static bool PIOS_TIM_validate(struct pios_tim_dev * tim_dev) +{ + return (tim_dev->magic == PIOS_TIM_DEV_MAGIC); +} +#endif + +#if defined(PIOS_INCLUDE_FREERTOS) && 0 +static struct pios_tim_dev * PIOS_TIM_alloc(void) +{ + struct pios_tim_dev * tim_dev; + + tim_dev = (struct pios_tim_dev *)malloc(sizeof(*tim_dev)); + if (!tim_dev) return(NULL); + + tim_dev->magic = PIOS_TIM_DEV_MAGIC; + return(tim_dev); +} +#else +static struct pios_tim_dev pios_tim_devs[PIOS_TIM_MAX_DEVS]; +static uint8_t pios_tim_num_devs; +static struct pios_tim_dev * PIOS_TIM_alloc(void) +{ + struct pios_tim_dev * tim_dev; + + if (pios_tim_num_devs >= PIOS_TIM_MAX_DEVS) { + return (NULL); + } + + tim_dev = &pios_tim_devs[pios_tim_num_devs++]; + tim_dev->magic = PIOS_TIM_DEV_MAGIC; + + return (tim_dev); +} +#endif + + + + +int32_t PIOS_TIM_InitClock(const struct pios_tim_clock_cfg * cfg) +{ + PIOS_DEBUG_Assert(cfg); + + /* Enable appropriate clock to timer module */ + switch((uint32_t) cfg->timer) { + case (uint32_t)TIM1: + RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE); + break; + case (uint32_t)TIM2: + RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); + break; + case (uint32_t)TIM3: + RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE); + break; + case (uint32_t)TIM4: + RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE); + break; +#ifdef STM32F10X_HD + case (uint32_t)TIM5: + RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, ENABLE); + break; + case (uint32_t)TIM6: + RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6, ENABLE); + break; + case (uint32_t)TIM7: + RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM7, ENABLE); + break; + case (uint32_t)TIM8: + RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8, ENABLE); + break; +#endif + } + + /* Configure the dividers for this timer */ + TIM_TimeBaseInit(cfg->timer, cfg->time_base_init); + + /* Configure internal timer clocks */ + TIM_InternalClockConfig(cfg->timer); + + /* Enable timers */ + TIM_Cmd(cfg->timer, ENABLE); + + /* Enable Interrupts */ + NVIC_Init(&cfg->irq.init); + + return 0; +} + +int32_t PIOS_TIM_InitChannels(uint32_t * tim_id, const struct pios_tim_channel * channels, uint8_t num_channels, const struct pios_tim_callbacks * callbacks, uint32_t context) +{ + PIOS_Assert(channels); + PIOS_Assert(num_channels); + + struct pios_tim_dev * tim_dev; + tim_dev = (struct pios_tim_dev *) PIOS_TIM_alloc(); + if (!tim_dev) goto out_fail; + + /* Bind the configuration to the device instance */ + tim_dev->channels = channels; + tim_dev->num_channels = num_channels; + tim_dev->callbacks = callbacks; + tim_dev->context = context; + + /* Configure the pins */ + for (uint8_t i = 0; i < num_channels; i++) { + const struct pios_tim_channel * chan = &(channels[i]); + + /* Enable the peripheral clock for the GPIO */ + switch ((uint32_t)chan->pin.gpio) { + case (uint32_t) GPIOA: + RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); + break; + case (uint32_t) GPIOB: + RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); + break; + case (uint32_t) GPIOC: + RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); + break; + default: + PIOS_Assert(0); + break; + } + GPIO_Init(chan->pin.gpio, &chan->pin.init); + + if (chan->remap) { + GPIO_PinRemapConfig(chan->remap, ENABLE); + } + } + + *tim_id = (uint32_t)tim_dev; + + return(0); + +out_fail: + return(-1); +} + +static void PIOS_TIM_generic_irq_handler(TIM_TypeDef * timer) +{ + /* Iterate over all registered clients of the TIM layer to find channels on this timer */ + for (uint8_t i = 0; i < pios_tim_num_devs; i++) { + const struct pios_tim_dev * tim_dev = &pios_tim_devs[i]; + + if (!tim_dev->channels || tim_dev->num_channels == 0) { + /* No channels to process on this client */ + continue; + } + + /* Check for an overflow event on this timer */ + bool overflow_event; + uint16_t overflow_count; + if (TIM_GetITStatus(timer, TIM_IT_Update) == SET) { + TIM_ClearITPendingBit(timer, TIM_IT_Update); + overflow_count = timer->ARR; + overflow_event = true; + } else { + overflow_count = 0; + overflow_event = false; + } + + for (uint8_t j = 0; j < tim_dev->num_channels; j++) { + const struct pios_tim_channel * chan = &tim_dev->channels[j]; + + if (chan->timer != timer) { + /* channel is not on this timer */ + continue; + } + + /* Figure out which interrupt bit we should be looking at */ + uint16_t timer_it; + switch (chan->timer_chan) { + case TIM_Channel_1: + timer_it = TIM_IT_CC1; + break; + case TIM_Channel_2: + timer_it = TIM_IT_CC2; + break; + case TIM_Channel_3: + timer_it = TIM_IT_CC3; + break; + case TIM_Channel_4: + timer_it = TIM_IT_CC4; + break; + default: + PIOS_Assert(0); + break; + } + + bool edge_event; + uint16_t edge_count; + if (TIM_GetITStatus(chan->timer, timer_it) == SET) { + TIM_ClearITPendingBit(chan->timer, timer_it); + + /* Read the current counter */ + switch(chan->timer_chan) { + case TIM_Channel_1: + edge_count = TIM_GetCapture1(chan->timer); + break; + case TIM_Channel_2: + edge_count = TIM_GetCapture2(chan->timer); + break; + case TIM_Channel_3: + edge_count = TIM_GetCapture3(chan->timer); + break; + case TIM_Channel_4: + edge_count = TIM_GetCapture4(chan->timer); + break; + default: + PIOS_Assert(0); + break; + } + edge_event = true; + } else { + edge_event = false; + edge_count = 0; + } + + if (!tim_dev->callbacks) { + /* No callbacks registered, we're done with this channel */ + continue; + } + + /* Generate the appropriate callbacks */ + if (overflow_event & edge_event) { + /* + * When both edge and overflow happen in the same interrupt, we + * need a heuristic to determine the order of the edge and overflow + * events so that the callbacks happen in the right order. If we + * get the order wrong, our pulse width calculations could be off by up + * to ARR ticks. That could be bad. + * + * Heuristic: If the edge_count is < 16 ticks above zero then we assume the + * edge happened just after the overflow. + */ + + if (edge_count < 16) { + /* Call the overflow callback first */ + if (tim_dev->callbacks->overflow) { + (*tim_dev->callbacks->overflow)((uint32_t)tim_dev, + tim_dev->context, + j, + overflow_count); + } + /* Call the edge callback second */ + if (tim_dev->callbacks->edge) { + (*tim_dev->callbacks->edge)((uint32_t)tim_dev, + tim_dev->context, + j, + edge_count); + } + } else { + /* Call the edge callback first */ + if (tim_dev->callbacks->edge) { + (*tim_dev->callbacks->edge)((uint32_t)tim_dev, + tim_dev->context, + j, + edge_count); + } + /* Call the overflow callback second */ + if (tim_dev->callbacks->overflow) { + (*tim_dev->callbacks->overflow)((uint32_t)tim_dev, + tim_dev->context, + j, + overflow_count); + } + } + } else if (overflow_event && tim_dev->callbacks->overflow) { + (*tim_dev->callbacks->overflow)((uint32_t)tim_dev, + tim_dev->context, + j, + overflow_count); + } else if (edge_event && tim_dev->callbacks->edge) { + (*tim_dev->callbacks->edge)((uint32_t)tim_dev, + tim_dev->context, + j, + edge_count); + } + } + } +} +#if 0 + uint16_t val = 0; + for(uint8_t i = 0; i < pios_pwm_cfg.num_channels; i++) { + struct pios_pwm_channel channel = pios_pwm_cfg.channels[i]; + if ((channel.timer == timer) && (TIM_GetITStatus(channel.timer, channel.ccr) == SET)) { + + TIM_ClearITPendingBit(channel.timer, channel.ccr); + + switch(channel.channel) { + case TIM_Channel_1: + val = TIM_GetCapture1(channel.timer); + break; + case TIM_Channel_2: + val = TIM_GetCapture2(channel.timer); + break; + case TIM_Channel_3: + val = TIM_GetCapture3(channel.timer); + break; + case TIM_Channel_4: + val = TIM_GetCapture4(channel.timer); + break; + } + + if (CaptureState[i] == 0) { + RiseValue[i] = val; + } else { + FallValue[i] = val; + } + + // flip state machine and capture value here + /* Simple rise or fall state machine */ + TIM_ICInitTypeDef TIM_ICInitStructure = pios_pwm_cfg.tim_ic_init; + if (CaptureState[i] == 0) { + /* Switch states */ + CaptureState[i] = 1; + + /* Switch polarity of input capture */ + TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling; + TIM_ICInitStructure.TIM_Channel = channel.channel; + TIM_ICInit(channel.timer, &TIM_ICInitStructure); + } else { + /* Capture computation */ + if (FallValue[i] > RiseValue[i]) { + CaptureValue[i] = (FallValue[i] - RiseValue[i]); + } else { + CaptureValue[i] = ((channel.timer->ARR - RiseValue[i]) + FallValue[i]); + } + + /* Switch states */ + CaptureState[i] = 0; + + /* Increase supervisor counter */ + CapCounter[i]++; + + /* Switch polarity of input capture */ + TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising; + TIM_ICInitStructure.TIM_Channel = channel.channel; + TIM_ICInit(channel.timer, &TIM_ICInitStructure); + } + } + } +#endif + +/* Bind Interrupt Handlers + * + * Map all valid TIM IRQs to the common interrupt handler + * and give it enough context to properly demux the various timers + */ +static void PIOS_TIM_1_irq_handler (void) +{ + PIOS_TIM_generic_irq_handler (TIM1); +} +void TIM1_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_1_irq_handler"))); + +static void PIOS_TIM_2_irq_handler (void) +{ + PIOS_TIM_generic_irq_handler (TIM2); +} +void TIM2_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_2_irq_handler"))); + +void TIM3_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_3_irq_handler"))); +static void PIOS_TIM_3_irq_handler (void) +{ + PIOS_TIM_generic_irq_handler (TIM3); +} + +void TIM4_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_4_irq_handler"))); +static void PIOS_TIM_4_irq_handler (void) +{ + PIOS_TIM_generic_irq_handler (TIM4); +} + diff --git a/flight/PiOS/inc/pios_debug.h b/flight/PiOS/inc/pios_debug.h index e663d224e..4dc5f5b3e 100644 --- a/flight/PiOS/inc/pios_debug.h +++ b/flight/PiOS/inc/pios_debug.h @@ -33,7 +33,9 @@ extern const char *PIOS_DEBUG_AssertMsg; -void PIOS_DEBUG_Init(void); +#include + +void PIOS_DEBUG_Init(const struct pios_tim_channel * channels, uint8_t num_channels); void PIOS_DEBUG_PinHigh(uint8_t pin); void PIOS_DEBUG_PinLow(uint8_t pin); void PIOS_DEBUG_PinValue8Bit(uint8_t value); diff --git a/flight/PiOS/inc/pios_ppm_priv.h b/flight/PiOS/inc/pios_ppm_priv.h index fd863fc50..0facab9d5 100644 --- a/flight/PiOS/inc/pios_ppm_priv.h +++ b/flight/PiOS/inc/pios_ppm_priv.h @@ -35,24 +35,14 @@ #include struct pios_ppm_cfg { - TIM_TimeBaseInitTypeDef tim_base_init; TIM_ICInitTypeDef tim_ic_init; - GPIO_InitTypeDef gpio_init; - uint32_t remap; /* GPIO_Remap_* */ - struct stm32_irq irq; - TIM_TypeDef * timer; - GPIO_TypeDef * port; - uint16_t ccr; + const struct pios_tim_channel * channels; + uint8_t num_channels; }; -extern void PIOS_PPM_irq_handler(); - -extern uint8_t pios_ppm_num_channels; -extern const struct pios_ppm_cfg pios_ppm_cfg; - extern const struct pios_rcvr_driver pios_ppm_rcvr_driver; -extern void PIOS_PPM_Init(void); +extern int32_t PIOS_PPM_Init(uint32_t * ppm_id, const struct pios_ppm_cfg * cfg); #endif /* PIOS_PPM_PRIV_H */ diff --git a/flight/PiOS/inc/pios_pwm_priv.h b/flight/PiOS/inc/pios_pwm_priv.h index 23d7d4818..fe11b3d60 100644 --- a/flight/PiOS/inc/pios_pwm_priv.h +++ b/flight/PiOS/inc/pios_pwm_priv.h @@ -34,32 +34,17 @@ #include #include -struct pios_pwm_channel { - TIM_TypeDef * timer; - GPIO_TypeDef * port; - uint16_t ccr; - uint8_t channel; - uint16_t pin; -}; +#include struct pios_pwm_cfg { - TIM_TimeBaseInitTypeDef tim_base_init; TIM_ICInitTypeDef tim_ic_init; - GPIO_InitTypeDef gpio_init; - uint32_t remap; /* GPIO_Remap_* */ - struct stm32_irq irq; - const struct pios_pwm_channel *const channels; + const struct pios_tim_channel * channels; uint8_t num_channels; }; -extern void PIOS_PWM_irq_handler(TIM_TypeDef * timer); - -extern uint8_t pios_pwm_num_channels; -extern const struct pios_pwm_cfg pios_pwm_cfg; - extern const struct pios_rcvr_driver pios_pwm_rcvr_driver; -extern void PIOS_PWM_Init(void); +extern int32_t PIOS_PWM_Init(uint32_t * pwm_id, const struct pios_pwm_cfg * cfg); #endif /* PIOS_PWM_PRIV_H */ diff --git a/flight/PiOS/inc/pios_servo.h b/flight/PiOS/inc/pios_servo.h index 2d621cb25..043d656df 100644 --- a/flight/PiOS/inc/pios_servo.h +++ b/flight/PiOS/inc/pios_servo.h @@ -31,7 +31,6 @@ #define PIOS_SERVO_H /* Public Functions */ -extern void PIOS_Servo_Init(void); extern void PIOS_Servo_SetHz(uint16_t * update_rates, uint8_t channels); extern void PIOS_Servo_Set(uint8_t Servo, uint16_t Position); diff --git a/flight/PiOS/inc/pios_servo_priv.h b/flight/PiOS/inc/pios_servo_priv.h index a1136c46d..f5ed86e25 100644 --- a/flight/PiOS/inc/pios_servo_priv.h +++ b/flight/PiOS/inc/pios_servo_priv.h @@ -33,25 +33,18 @@ #include #include - -struct pios_servo_channel { - TIM_TypeDef * timer; - GPIO_TypeDef * port; - uint8_t channel; - uint16_t pin; -}; +#include struct pios_servo_cfg { TIM_TimeBaseInitTypeDef tim_base_init; TIM_OCInitTypeDef tim_oc_init; GPIO_InitTypeDef gpio_init; uint32_t remap; - const struct pios_servo_channel *const channels; + const struct pios_tim_channel * channels; uint8_t num_channels; }; - -extern const struct pios_servo_cfg pios_servo_cfg; +extern int32_t PIOS_Servo_Init(const struct pios_servo_cfg * cfg); #endif /* PIOS_SERVO_PRIV_H */ diff --git a/flight/PiOS/inc/pios_tim.h b/flight/PiOS/inc/pios_tim.h new file mode 100644 index 000000000..0bad07f41 --- /dev/null +++ b/flight/PiOS/inc/pios_tim.h @@ -0,0 +1,4 @@ +#ifndef PIOS_TIM_H +#define PIOS_TIM_H + +#endif /* PIOS_TIM_H */ diff --git a/flight/PiOS/inc/pios_tim_priv.h b/flight/PiOS/inc/pios_tim_priv.h new file mode 100644 index 000000000..7f05719f8 --- /dev/null +++ b/flight/PiOS/inc/pios_tim_priv.h @@ -0,0 +1,28 @@ +#ifndef PIOS_TIM_PRIV_H +#define PIOS_TIM_PRIV_H + +#include + +struct pios_tim_clock_cfg { + TIM_TypeDef * timer; + const TIM_TimeBaseInitTypeDef * time_base_init; + struct stm32_irq irq; +}; + +struct pios_tim_channel { + TIM_TypeDef * timer; + uint8_t timer_chan; + + struct stm32_gpio pin; + uint32_t remap; +}; + +struct pios_tim_callbacks { + void (*overflow)(uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count); + void (*edge)(uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count); +}; + +extern int32_t PIOS_TIM_InitClock(const struct pios_tim_clock_cfg * cfg); +extern int32_t PIOS_TIM_InitChannels(uint32_t * tim_id, const struct pios_tim_channel * channels, uint8_t num_channels, const struct pios_tim_callbacks * callbacks, uint32_t context); + +#endif /* PIOS_TIM_PRIV_H */ From 86ce79af1d1395fd1aec6e36a21926931f5c33cc Mon Sep 17 00:00:00 2001 From: Stacey Sheldon Date: Wed, 31 Aug 2011 21:43:57 -0400 Subject: [PATCH 095/145] openpilot: fix compile error on OP --- flight/OpenPilot/System/pios_board.c | 27 ++++++++++++++------------- 1 file changed, 14 insertions(+), 13 deletions(-) diff --git a/flight/OpenPilot/System/pios_board.c b/flight/OpenPilot/System/pios_board.c index e17ad5653..7634f90e1 100644 --- a/flight/OpenPilot/System/pios_board.c +++ b/flight/OpenPilot/System/pios_board.c @@ -1168,19 +1168,20 @@ void PIOS_Board_Init(void) { break; case HWSETTINGS_OP_FLEXIPORT_GPS: #if defined(PIOS_INCLUDE_GPS) - { - uint32_t pios_usart_gps_id; - if (PIOS_USART_Init(&pios_usart_gps_id, &pios_usart_gps_cfg)) { - PIOS_Assert(0); - } - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN); - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id, - rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, - tx_buffer, PIOS_COM_GPS_TX_BUF_LEN)) { - PIOS_Assert(0); + { + uint32_t pios_usart_gps_id; + if (PIOS_USART_Init(&pios_usart_gps_id, &pios_usart_gps_cfg)) { + PIOS_Assert(0); + } + uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN); + uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_TX_BUF_LEN); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id, + rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, + tx_buffer, PIOS_COM_GPS_TX_BUF_LEN)) { + PIOS_Assert(0); + } } #endif /* PIOS_INCLUDE_GPS */ break; From 43b31efb76d16dd7f23eef31926823bcaee75277 Mon Sep 17 00:00:00 2001 From: Stacey Sheldon Date: Wed, 31 Aug 2011 22:35:03 -0400 Subject: [PATCH 096/145] pios: allocate driver instance data from heap Allocate per-instance data for drivers from the heap rather than as static variables from the .data segment. This converts > 800 bytes of RAM from being always consumed as static data into being allocated from the heap only when a particular feature is enabled in the hwsettings object. A minimal config (no receivers, flexi port disabled, main port disabled) leaves 2448 bytes of free heap. That's our new baseline. Approximate RAM (heap) costs of enabling various features: + 632 Serial Telemetry (includes 400 bytes of Rx/Tx buffers) + 108 PWM Rcvr + 152 PPM Rcvr + 112 Spektrum Rcvr + 24 S.Bus (Should be closer to 68 since driver is still using static memory) There are still some drivers that pre-allocate all of their memory as static data. It'll take some work to convert those over to dynamically allocating their instance data. --- flight/PiOS/Common/pios_com.c | 4 ++-- flight/PiOS/Common/pios_rcvr.c | 4 ++-- flight/PiOS/STM32F10x/pios_i2c.c | 8 ++++---- flight/PiOS/STM32F10x/pios_ppm.c | 4 ++-- flight/PiOS/STM32F10x/pios_pwm.c | 4 ++-- flight/PiOS/STM32F10x/pios_spektrum.c | 4 ++-- flight/PiOS/STM32F10x/pios_spi.c | 4 ++-- flight/PiOS/STM32F10x/pios_usart.c | 4 ++-- flight/PiOS/STM32F10x/pios_usb_hid.c | 4 ++-- 9 files changed, 20 insertions(+), 20 deletions(-) diff --git a/flight/PiOS/Common/pios_com.c b/flight/PiOS/Common/pios_com.c index b7a9a67e5..e7fba8867 100644 --- a/flight/PiOS/Common/pios_com.c +++ b/flight/PiOS/Common/pios_com.c @@ -67,12 +67,12 @@ static bool PIOS_COM_validate(struct pios_com_dev * com_dev) return (com_dev && (com_dev->magic == PIOS_COM_DEV_MAGIC)); } -#if defined(PIOS_INCLUDE_FREERTOS) && 0 +#if defined(PIOS_INCLUDE_FREERTOS) static struct pios_com_dev * PIOS_COM_alloc(void) { struct pios_com_dev * com_dev; - com_dev = (struct pios_com_dev *)malloc(sizeof(*com_dev)); + com_dev = (struct pios_com_dev *)pvPortMalloc(sizeof(*com_dev)); if (!com_dev) return (NULL); com_dev->magic = PIOS_COM_DEV_MAGIC; diff --git a/flight/PiOS/Common/pios_rcvr.c b/flight/PiOS/Common/pios_rcvr.c index 29acb2594..43275ba6b 100644 --- a/flight/PiOS/Common/pios_rcvr.c +++ b/flight/PiOS/Common/pios_rcvr.c @@ -20,12 +20,12 @@ static bool PIOS_RCVR_validate(struct pios_rcvr_dev * rcvr_dev) return (rcvr_dev->magic == PIOS_RCVR_DEV_MAGIC); } -#if defined(PIOS_INCLUDE_FREERTOS) && 0 +#if defined(PIOS_INCLUDE_FREERTOS) static struct pios_rcvr_dev * PIOS_RCVR_alloc(void) { struct pios_rcvr_dev * rcvr_dev; - rcvr_dev = (struct pios_rcvr_dev *)malloc(sizeof(*rcvr_dev)); + rcvr_dev = (struct pios_rcvr_dev *)pvPortMalloc(sizeof(*rcvr_dev)); if (!rcvr_dev) return (NULL); rcvr_dev->magic = PIOS_RCVR_DEV_MAGIC; diff --git a/flight/PiOS/STM32F10x/pios_i2c.c b/flight/PiOS/STM32F10x/pios_i2c.c index 0d47fd117..6e623d597 100644 --- a/flight/PiOS/STM32F10x/pios_i2c.c +++ b/flight/PiOS/STM32F10x/pios_i2c.c @@ -823,12 +823,12 @@ static bool PIOS_I2C_validate(struct pios_i2c_adapter * i2c_adapter) return (i2c_adapter->magic == PIOS_I2C_DEV_MAGIC); } -#if defined(PIOS_INCLUDE_FREERTOS) && 0 -static struct pios_i2c_dev * PIOS_I2C_alloc(void) +#if defined(PIOS_INCLUDE_FREERTOS) +static struct pios_i2c_adapter * PIOS_I2C_alloc(void) { - struct pios_i2c_dev * i2c_adapter; + struct pios_i2c_adapter * i2c_adapter; - i2c_adapter = (struct pios_i2c_adapter *)malloc(sizeof(*i2c_adapter)); + i2c_adapter = (struct pios_i2c_adapter *)pvPortMalloc(sizeof(*i2c_adapter)); if (!i2c_adapter) return(NULL); i2c_adapter->magic = PIOS_I2C_DEV_MAGIC; diff --git a/flight/PiOS/STM32F10x/pios_ppm.c b/flight/PiOS/STM32F10x/pios_ppm.c index 0b7a1c5cd..d3ebeebf3 100644 --- a/flight/PiOS/STM32F10x/pios_ppm.c +++ b/flight/PiOS/STM32F10x/pios_ppm.c @@ -83,12 +83,12 @@ static bool PIOS_PPM_validate(struct pios_ppm_dev * ppm_dev) return (ppm_dev->magic == PIOS_PPM_DEV_MAGIC); } -#if defined(PIOS_INCLUDE_FREERTOS) && 0 +#if defined(PIOS_INCLUDE_FREERTOS) static struct pios_ppm_dev * PIOS_PPM_alloc(void) { struct pios_ppm_dev * ppm_dev; - ppm_dev = (struct pios_ppm_dev *)malloc(sizeof(*ppm_dev)); + ppm_dev = (struct pios_ppm_dev *)pvPortMalloc(sizeof(*ppm_dev)); if (!ppm_dev) return(NULL); ppm_dev->magic = PIOS_PPM_DEV_MAGIC; diff --git a/flight/PiOS/STM32F10x/pios_pwm.c b/flight/PiOS/STM32F10x/pios_pwm.c index 9717f66fc..a1276c88c 100644 --- a/flight/PiOS/STM32F10x/pios_pwm.c +++ b/flight/PiOS/STM32F10x/pios_pwm.c @@ -63,12 +63,12 @@ static bool PIOS_PWM_validate(struct pios_pwm_dev * pwm_dev) return (pwm_dev->magic == PIOS_PWM_DEV_MAGIC); } -#if defined(PIOS_INCLUDE_FREERTOS) && 0 +#if defined(PIOS_INCLUDE_FREERTOS) static struct pios_pwm_dev * PIOS_PWM_alloc(void) { struct pios_pwm_dev * pwm_dev; - pwm_dev = (struct pios_pwm_dev *)malloc(sizeof(*pwm_dev)); + pwm_dev = (struct pios_pwm_dev *)pvPortMalloc(sizeof(*pwm_dev)); if (!pwm_dev) return(NULL); pwm_dev->magic = PIOS_PWM_DEV_MAGIC; diff --git a/flight/PiOS/STM32F10x/pios_spektrum.c b/flight/PiOS/STM32F10x/pios_spektrum.c index dab724ef8..f0a611671 100644 --- a/flight/PiOS/STM32F10x/pios_spektrum.c +++ b/flight/PiOS/STM32F10x/pios_spektrum.c @@ -82,12 +82,12 @@ static bool PIOS_SPEKTRUM_validate(struct pios_spektrum_dev * spektrum_dev) return (spektrum_dev->magic == PIOS_SPEKTRUM_DEV_MAGIC); } -#if defined(PIOS_INCLUDE_FREERTOS) && 0 +#if defined(PIOS_INCLUDE_FREERTOS) static struct pios_spektrum_dev * PIOS_SPEKTRUM_alloc(void) { struct pios_spektrum_dev * spektrum_dev; - spektrum_dev = (struct pios_spektrum_dev *)malloc(sizeof(*spektrum_dev)); + spektrum_dev = (struct pios_spektrum_dev *)pvPortMalloc(sizeof(*spektrum_dev)); if (!spektrum_dev) return(NULL); spektrum_dev->magic = PIOS_SPEKTRUM_DEV_MAGIC; diff --git a/flight/PiOS/STM32F10x/pios_spi.c b/flight/PiOS/STM32F10x/pios_spi.c index 74a14afc0..a5e57335f 100644 --- a/flight/PiOS/STM32F10x/pios_spi.c +++ b/flight/PiOS/STM32F10x/pios_spi.c @@ -46,10 +46,10 @@ static bool PIOS_SPI_validate(struct pios_spi_dev * com_dev) return(true); } -#if defined(PIOS_INCLUDE_FREERTOS) && 0 +#if defined(PIOS_INCLUDE_FREERTOS) static struct pios_spi_dev * PIOS_SPI_alloc(void) { - return (malloc(sizeof(struct pios_spi_dev))); + return (pvPortMalloc(sizeof(struct pios_spi_dev))); } #else static struct pios_spi_dev pios_spi_devs[PIOS_SPI_MAX_DEVS]; diff --git a/flight/PiOS/STM32F10x/pios_usart.c b/flight/PiOS/STM32F10x/pios_usart.c index 8582344b5..68a9265ee 100644 --- a/flight/PiOS/STM32F10x/pios_usart.c +++ b/flight/PiOS/STM32F10x/pios_usart.c @@ -70,12 +70,12 @@ static bool PIOS_USART_validate(struct pios_usart_dev * usart_dev) return (usart_dev->magic == PIOS_USART_DEV_MAGIC); } -#if defined(PIOS_INCLUDE_FREERTOS) && 0 +#if defined(PIOS_INCLUDE_FREERTOS) static struct pios_usart_dev * PIOS_USART_alloc(void) { struct pios_usart_dev * usart_dev; - usart_dev = (struct pios_usart_dev *)malloc(sizeof(*usart_dev)); + usart_dev = (struct pios_usart_dev *)pvPortMalloc(sizeof(*usart_dev)); if (!usart_dev) return(NULL); usart_dev->magic = PIOS_USART_DEV_MAGIC; diff --git a/flight/PiOS/STM32F10x/pios_usb_hid.c b/flight/PiOS/STM32F10x/pios_usb_hid.c index 0ebca1320..8644dd517 100644 --- a/flight/PiOS/STM32F10x/pios_usb_hid.c +++ b/flight/PiOS/STM32F10x/pios_usb_hid.c @@ -75,12 +75,12 @@ static bool PIOS_USB_HID_validate(struct pios_usb_hid_dev * usb_hid_dev) return (usb_hid_dev->magic == PIOS_USB_HID_DEV_MAGIC); } -#if defined(PIOS_INCLUDE_FREERTOS) && 0 +#if defined(PIOS_INCLUDE_FREERTOS) static struct pios_usb_hid_dev * PIOS_USB_HID_alloc(void) { struct pios_usb_hid_dev * usb_hid_dev; - usb_hid_dev = (struct pios_usb_hid_dev *)malloc(sizeof(*usb_hid_dev)); + usb_hid_dev = (struct pios_usb_hid_dev *)pvPortMalloc(sizeof(*usb_hid_dev)); if (!usb_hid_dev) return(NULL); usb_hid_dev->magic = PIOS_USB_HID_DEV_MAGIC; From ae1dd945b131c344e23acd1f5b4e9c1e3afd7ad8 Mon Sep 17 00:00:00 2001 From: Mathieu Rondonneau Date: Thu, 1 Sep 2011 21:50:13 -0400 Subject: [PATCH 097/145] Fix win32 sim macro problem This is to get the macro in pios.win32 in sync with sim posix and real flight. --- flight/PiOS.win32/inc/pios_initcall.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/PiOS.win32/inc/pios_initcall.h b/flight/PiOS.win32/inc/pios_initcall.h index edd9ddc08..bdb47cc8e 100644 --- a/flight/PiOS.win32/inc/pios_initcall.h +++ b/flight/PiOS.win32/inc/pios_initcall.h @@ -38,7 +38,7 @@ * and we cannot define a linker script for each of them atm */ -#define MODULE_INITCALL(ifn, iparam, sfn, sparam, flags) +#define MODULE_INITCALL(ifn, sfn) #define MODULE_TASKCREATE_ALL From 1de58ebb875d13713df24a348cbda9c339efa829 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Mon, 29 Aug 2011 11:37:22 -0500 Subject: [PATCH 098/145] Make scope gadget check if object exists before using it. Avoids segfaults when scoped objects disappear. --- .../openpilotgcs/src/plugins/scope/scopegadgetwidget.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/ground/openpilotgcs/src/plugins/scope/scopegadgetwidget.cpp b/ground/openpilotgcs/src/plugins/scope/scopegadgetwidget.cpp index fb2bbf796..dfbd9d614 100644 --- a/ground/openpilotgcs/src/plugins/scope/scopegadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/scope/scopegadgetwidget.cpp @@ -385,8 +385,15 @@ void ScopeGadgetWidget::addCurvePlot(QString uavObject, QString uavFieldSubField ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); UAVObjectManager *objManager = pm->getObject(); UAVDataObject* obj = dynamic_cast(objManager->getObject((plotData->uavObject))); - + if(!obj) { + qDebug() << "Object " << plotData->uavObject << " is missing"; + return; + } UAVObjectField* field = obj->getField(plotData->uavField); + if(!field) { + qDebug() << "Field " << plotData->uavField << " of object " << plotData->uavObject << " is missing"; + return; + } QString units = field->getUnits(); if(units == 0) From e18288085c7911d4394135b7913138d7595c0bd0 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Fri, 2 Sep 2011 13:26:28 -0500 Subject: [PATCH 099/145] OP-390 Make the object saving throw an error message on save failures. --- .../uavobjectutil/uavobjectutilmanager.cpp | 24 +++++++++++++++---- 1 file changed, 19 insertions(+), 5 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp index b5070c269..8caafca48 100644 --- a/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp +++ b/ground/openpilotgcs/src/plugins/uavobjectutil/uavobjectutilmanager.cpp @@ -34,6 +34,8 @@ #include #include #include +#include +#include // ****************************** // constructor/destructor @@ -168,18 +170,30 @@ void UAVObjectUtilManager::objectPersistenceTransactionCompleted(UAVObject* obj, */ void UAVObjectUtilManager::objectPersistenceOperationFailed() { - qDebug() << "objectPersistenceOperationFailed"; if(saveState == AWAITING_COMPLETED) { //TODO: some warning that this operation failed somehow // We have to disconnect the object persistence 'updated' signal // and ask to save the next object: - UAVObject *obj = getObjectManager()->getObject(ObjectPersistence::NAME); - obj->disconnect(this); - queue.dequeue(); // We can now remove the object, it failed anyway. + + ObjectPersistence * objectPersistence = ObjectPersistence::GetInstance(getObjectManager()); + Q_ASSERT(objectPersistence); + + UAVObject* obj = queue.dequeue(); // We can now remove the object, it failed anyway. + Q_ASSERT(obj); + + objectPersistence->disconnect(this); + saveState = IDLE; - emit saveCompleted(obj->getField("ObjectID")->getValue().toInt(), false); + emit saveCompleted(obj->getObjID(), false); + + // For now cause error message here to make sure user knows + QErrorMessage err; + err.showMessage("Saving object " + obj->getName() + " failed. Please try again"); + err.exec(); + saveNextObject(); } + } From d28f0c4c7871a700af01730b7a965b655113a806 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sat, 3 Sep 2011 21:22:48 -0500 Subject: [PATCH 100/145] GCS Config Stabilization: Increase maximum rate that can be entered to 500 deg/s from 300 deg/s for heli people. --- .../openpilotgcs/src/plugins/config/stabilization.ui | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/stabilization.ui b/ground/openpilotgcs/src/plugins/config/stabilization.ui index 799c50880..5c2de727e 100644 --- a/ground/openpilotgcs/src/plugins/config/stabilization.ui +++ b/ground/openpilotgcs/src/plugins/config/stabilization.ui @@ -601,21 +601,21 @@ When you change one, the other is updated. - 300 + 500 - 300 + 500 - 300 + 500 @@ -644,21 +644,21 @@ When you change one, the other is updated. - 300 + 500 - 300 + 500 - 300 + 500 From 697dbf4f5f1bb860c693d5519bb12f7b364a36b5 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sat, 3 Sep 2011 23:47:56 -0500 Subject: [PATCH 101/145] OP-568 PIOS_RCVR: Standardize the values that are returned from the PIOS_RCVR and make them symbolic constants. - A timeout is 0 - A missing driver is 65534 - An invalid channel is 65535 ManualControl: Make it deal with the values explicitly. A timed out value should not be treated like a minimum duration signal. Instead it does not updated the scaled value but marks the data window as invalid to trigger the failsafe. --- flight/Modules/ManualControl/manualcontrol.c | 33 +++++++++++++++---- flight/PiOS/Common/pios_rcvr.c | 3 ++ flight/PiOS/STM32F10x/pios_ppm.c | 9 +++-- flight/PiOS/STM32F10x/pios_spektrum.c | 4 +-- flight/PiOS/inc/pios_rcvr.h | 4 +++ .../OpenPilotOSX.xcodeproj/project.pbxproj | 12 +++++++ 6 files changed, 52 insertions(+), 13 deletions(-) diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c index 9165f06f8..3e2662bce 100644 --- a/flight/Modules/ManualControl/manualcontrol.c +++ b/flight/Modules/ManualControl/manualcontrol.c @@ -202,6 +202,8 @@ static void manualControlTask(void *parameters) if (!ManualControlCommandReadOnly(&cmd)) { + bool valid_input_detected = true; + // Read channel values in us for (uint8_t n = 0; n < MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM && n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; @@ -209,14 +211,20 @@ static void manualControlTask(void *parameters) extern uint32_t pios_rcvr_group_map[]; if (settings.ChannelGroups[n] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { - cmd.Channel[n] = -1; - } else if (!pios_rcvr_group_map[settings.ChannelGroups[n]]) { - cmd.Channel[n] = -2; + cmd.Channel[n] = PIOS_RCVR_INVALID; } else { cmd.Channel[n] = PIOS_RCVR_Read(pios_rcvr_group_map[settings.ChannelGroups[n]], settings.ChannelNumber[n]); } - scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n], settings.ChannelMin[n], settings.ChannelNeutral[n]); + + // If a channel has timed out this is not valid data and we shouldn't update anything + // until we decide to go to failsafe + if(cmd.Channel[n] == PIOS_RCVR_TIMEOUT) + valid_input_detected = false; + else if (cmd.Channel[n] == PIOS_RCVR_INVALID) + scaledChannel[n] = 0; + else + scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n], settings.ChannelMin[n], settings.ChannelNeutral[n]); } // Check settings, if error raise alarm @@ -224,7 +232,20 @@ static void manualControlTask(void *parameters) settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE || settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE || settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE || - settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE || + // Check all channel mappings are valid + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == PIOS_RCVR_INVALID || + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == PIOS_RCVR_INVALID || + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == PIOS_RCVR_INVALID || + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == PIOS_RCVR_INVALID || + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == PIOS_RCVR_INVALID || + // Check the driver is exists + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == PIOS_RCVR_NODRIVER || + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == PIOS_RCVR_NODRIVER || + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == PIOS_RCVR_NODRIVER || + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == PIOS_RCVR_NODRIVER || + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == PIOS_RCVR_NODRIVER) { + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; ManualControlCommandSet(&cmd); @@ -232,7 +253,7 @@ static void manualControlTask(void *parameters) } // decide if we have valid manual input or not - bool valid_input_detected = validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]) && + valid_input_detected &= validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]) && validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]) && validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]) && validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]); diff --git a/flight/PiOS/Common/pios_rcvr.c b/flight/PiOS/Common/pios_rcvr.c index 29acb2594..b76e04a44 100644 --- a/flight/PiOS/Common/pios_rcvr.c +++ b/flight/PiOS/Common/pios_rcvr.c @@ -78,6 +78,9 @@ out_fail: int32_t PIOS_RCVR_Read(uint32_t rcvr_id, uint8_t channel) { + if (rcvr_id == 0) + return PIOS_RCVR_NODRIVER; + struct pios_rcvr_dev * rcvr_dev = (struct pios_rcvr_dev *)rcvr_id; if (!PIOS_RCVR_validate(rcvr_dev)) { diff --git a/flight/PiOS/STM32F10x/pios_ppm.c b/flight/PiOS/STM32F10x/pios_ppm.c index 0b7a1c5cd..bee955861 100644 --- a/flight/PiOS/STM32F10x/pios_ppm.c +++ b/flight/PiOS/STM32F10x/pios_ppm.c @@ -47,7 +47,6 @@ const struct pios_rcvr_driver pios_ppm_rcvr_driver = { #define PIOS_PPM_IN_MIN_SYNC_PULSE_US 3800 // microseconds #define PIOS_PPM_IN_MIN_CHANNEL_PULSE_US 750 // microseconds #define PIOS_PPM_IN_MAX_CHANNEL_PULSE_US 2250 // microseconds -#define PIOS_PPM_INPUT_INVALID 0 /* Local Variables */ static TIM_ICInitTypeDef TIM_ICInitStructure; @@ -290,7 +289,7 @@ static void PIOS_PPM_tim_edge_cb (uint32_t tim_id, uint32_t context, uint8_t cha } for (uint32_t i = ppm_dev->NumChannels; i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) { - ppm_dev->CaptureValue[i] = PIOS_PPM_INPUT_INVALID; + ppm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; } } @@ -314,7 +313,7 @@ static void PIOS_PPM_tim_edge_cb (uint32_t tim_id, uint32_t context, uint8_t cha /* Not a valid pulse duration */ ppm_dev->Tracking = FALSE; for (uint32_t i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS ; i++) { - ppm_dev->CaptureValueNewFrame[i] = PIOS_PPM_INPUT_INVALID; + ppm_dev->CaptureValueNewFrame[i] = PIOS_RCVR_TIMEOUT; } } } @@ -342,8 +341,8 @@ static void PIOS_PPM_Supervisor(uint32_t ppm_id) { ppm_dev->Tracking = FALSE; for (int32_t i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS ; i++) { - ppm_dev->CaptureValue[i] = PIOS_PPM_INPUT_INVALID; - ppm_dev->CaptureValueNewFrame[i] = PIOS_PPM_INPUT_INVALID; + ppm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; + ppm_dev->CaptureValueNewFrame[i] = PIOS_RCVR_TIMEOUT; } } diff --git a/flight/PiOS/STM32F10x/pios_spektrum.c b/flight/PiOS/STM32F10x/pios_spektrum.c index dab724ef8..53142069a 100644 --- a/flight/PiOS/STM32F10x/pios_spektrum.c +++ b/flight/PiOS/STM32F10x/pios_spektrum.c @@ -338,8 +338,8 @@ static void PIOS_SPEKTRUM_Supervisor(uint32_t spektrum_id) /* signal lost */ fsm->sync_of = 0; for (int i = 0; i < PIOS_SPEKTRUM_NUM_INPUTS; i++) { - fsm->CaptureValue[i] = 0; - fsm->CaptureValueTemp[i] = 0; + fsm->CaptureValue[i] = PIOS_RCVR_TIMEOUT; + fsm->CaptureValueTemp[i] = PIOS_RCVR_TIMEOUT; } } spektrum_dev->supv_timer = 0; diff --git a/flight/PiOS/inc/pios_rcvr.h b/flight/PiOS/inc/pios_rcvr.h index 0f4e6a973..ce28d37af 100644 --- a/flight/PiOS/inc/pios_rcvr.h +++ b/flight/PiOS/inc/pios_rcvr.h @@ -39,6 +39,10 @@ struct pios_rcvr_driver { /* Public Functions */ extern int32_t PIOS_RCVR_Read(uint32_t rcvr_id, uint8_t channel); +#define PIOS_RCVR_TIMEOUT 0 +#define PIOS_RCVR_NODRIVER 65534 +#define PIOS_RCVR_INVALID 65535 + #endif /* PIOS_RCVR_H */ /** diff --git a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj index ceca7eafb..da0793b51 100644 --- a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj +++ b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj @@ -91,6 +91,12 @@ 65632DF51251650300469B77 /* pios_board.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_board.h; sourceTree = ""; }; 65632DF61251650300469B77 /* STM32103CB_AHRS.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = STM32103CB_AHRS.h; sourceTree = ""; }; 65632DF71251650300469B77 /* STM3210E_OP.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = STM3210E_OP.h; sourceTree = ""; }; + 65643CAB1413322000A32F59 /* pios_rcvr_priv.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_rcvr_priv.h; sourceTree = ""; }; + 65643CAC1413322000A32F59 /* pios_rcvr.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_rcvr.h; sourceTree = ""; }; + 65643CAD1413322000A32F59 /* pios_rtc_priv.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_rtc_priv.h; sourceTree = ""; }; + 65643CAE1413322000A32F59 /* pios_sbus_priv.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_sbus_priv.h; sourceTree = ""; }; + 65643CAF1413322000A32F59 /* pios_sbus.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_sbus.h; sourceTree = ""; }; + 65643CB01413322000A32F59 /* pios_spektrum_priv.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_spektrum_priv.h; sourceTree = ""; }; 6572CB1613D0F2B200FC2972 /* link_STM32103CB_CC_Rev1_memory.ld */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; name = link_STM32103CB_CC_Rev1_memory.ld; path = ../../PiOS/STM32F10x/link_STM32103CB_CC_Rev1_memory.ld; sourceTree = SOURCE_ROOT; }; 6572CB1713D0F2B200FC2972 /* link_STM32103CB_CC_Rev1_sections.ld */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; name = link_STM32103CB_CC_Rev1_sections.ld; path = ../../PiOS/STM32F10x/link_STM32103CB_CC_Rev1_sections.ld; sourceTree = SOURCE_ROOT; }; 657CEEAD121DB6C8007A1FBE /* homelocation.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = homelocation.xml; sourceTree = ""; }; @@ -7724,11 +7730,17 @@ 65E8F04811EFF25C00BBF654 /* pios_ppm.h */, 65E8F04911EFF25C00BBF654 /* pios_pwm.h */, 657FF86A12EA8BFB00801617 /* pios_pwm_priv.h */, + 65643CAC1413322000A32F59 /* pios_rcvr.h */, + 65643CAB1413322000A32F59 /* pios_rcvr_priv.h */, 6589A9E2131DF1C7006BD67C /* pios_rtc.h */, + 65643CAD1413322000A32F59 /* pios_rtc_priv.h */, + 65643CAE1413322000A32F59 /* pios_sbus_priv.h */, + 65643CAF1413322000A32F59 /* pios_sbus.h */, 65E8F04A11EFF25C00BBF654 /* pios_sdcard.h */, 65E8F04B11EFF25C00BBF654 /* pios_servo.h */, 65FBE14412E7C98100176B5A /* pios_servo_priv.h */, 65E8F04C11EFF25C00BBF654 /* pios_spektrum.h */, + 65643CB01413322000A32F59 /* pios_spektrum_priv.h */, 65E8F04D11EFF25C00BBF654 /* pios_spi.h */, 65E8F04E11EFF25C00BBF654 /* pios_spi_priv.h */, 65E8F04F11EFF25C00BBF654 /* pios_stm32.h */, From d3de8ff0effe2616e837adc1ca4586ed91f2d085 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sat, 3 Sep 2011 23:57:51 -0500 Subject: [PATCH 102/145] OP-559: Process the arm status when disconnect and allow it to timeout and disarm --- flight/Modules/ManualControl/manualcontrol.c | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c index 3e2662bce..db9e75dc4 100644 --- a/flight/Modules/ManualControl/manualcontrol.c +++ b/flight/Modules/ManualControl/manualcontrol.c @@ -278,7 +278,6 @@ static void manualControlTask(void *parameters) // Important: Throttle < 0 will reset Stabilization coefficients among other things. Either change this, // or leave throttle at IDLE speed or above when going into AUTO-failsafe. AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); - ManualControlCommandSet(&cmd); } else { AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL); @@ -314,13 +313,15 @@ static void manualControlTask(void *parameters) processFlightMode(&settings, flightMode); - processArm(&cmd, &settings); - - // Update cmd object - ManualControlCommandSet(&cmd); } + // Process arming outside conditional so system will disarm when disconnected + processArm(&cmd, &settings); + + // Update cmd object + ManualControlCommandSet(&cmd); + } else { ManualControlCommandGet(&cmd); /* Under GCS control */ } @@ -645,7 +646,7 @@ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData } else { // Not really needed since this function not called when disconnected if (cmd->Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) - return; + lowThrottle = true; // The throttle is not low, in case we where arming or disarming, abort if (!lowThrottle) { From 20de0462924eb6c4c8ef22091a31ed663c816df4 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sun, 4 Sep 2011 00:07:34 -0500 Subject: [PATCH 103/145] Force system to be disarmed when a bad configuration is present --- flight/Modules/ManualControl/manualcontrol.c | 6 ++++++ .../OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj | 2 ++ 2 files changed, 8 insertions(+) diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c index db9e75dc4..de8937f92 100644 --- a/flight/Modules/ManualControl/manualcontrol.c +++ b/flight/Modules/ManualControl/manualcontrol.c @@ -81,6 +81,7 @@ static void updateActuatorDesired(ManualControlCommandData * cmd); static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings); static void processFlightMode(ManualControlSettingsData * settings, float flightMode); static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings); +static void setArmedIfChanged(uint8_t val); static void manualControlTask(void *parameters); static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral); @@ -249,6 +250,11 @@ static void manualControlTask(void *parameters) AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; ManualControlCommandSet(&cmd); + + // Need to do this here since we don't process armed status. Since this shouldn't happen in flight (changed config) + // immediately disarm + setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); + continue; } diff --git a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj index da0793b51..0a0834048 100644 --- a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj +++ b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj @@ -97,6 +97,7 @@ 65643CAE1413322000A32F59 /* pios_sbus_priv.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_sbus_priv.h; sourceTree = ""; }; 65643CAF1413322000A32F59 /* pios_sbus.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_sbus.h; sourceTree = ""; }; 65643CB01413322000A32F59 /* pios_spektrum_priv.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_spektrum_priv.h; sourceTree = ""; }; + 65643CB91413456D00A32F59 /* pios_tim.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_tim.c; sourceTree = ""; }; 6572CB1613D0F2B200FC2972 /* link_STM32103CB_CC_Rev1_memory.ld */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; name = link_STM32103CB_CC_Rev1_memory.ld; path = ../../PiOS/STM32F10x/link_STM32103CB_CC_Rev1_memory.ld; sourceTree = SOURCE_ROOT; }; 6572CB1713D0F2B200FC2972 /* link_STM32103CB_CC_Rev1_sections.ld */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; name = link_STM32103CB_CC_Rev1_sections.ld; path = ../../PiOS/STM32F10x/link_STM32103CB_CC_Rev1_sections.ld; sourceTree = SOURCE_ROOT; }; 657CEEAD121DB6C8007A1FBE /* homelocation.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = homelocation.xml; sourceTree = ""; }; @@ -7782,6 +7783,7 @@ 65E8F0E711EFF25C00BBF654 /* pios_spektrum.c */, 65E8F0E811EFF25C00BBF654 /* pios_spi.c */, 65E8F0E911EFF25C00BBF654 /* pios_sys.c */, + 65643CB91413456D00A32F59 /* pios_tim.c */, 65E8F0EA11EFF25C00BBF654 /* pios_usart.c */, 65E8F0ED11EFF25C00BBF654 /* pios_usb_hid.c */, 651CF9E5120B5D8300EEFD70 /* pios_usb_hid_desc.c */, From 51967ae63f3c768f3afc56f5037d4cada88c5430 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sun, 4 Sep 2011 01:14:54 -0500 Subject: [PATCH 104/145] OP-571 PIOS_PWM: Add back the PWM supervisor --- flight/PiOS/STM32F10x/pios_pwm.c | 24 +++++++++++++++++++++++- 1 file changed, 23 insertions(+), 1 deletion(-) diff --git a/flight/PiOS/STM32F10x/pios_pwm.c b/flight/PiOS/STM32F10x/pios_pwm.c index 9717f66fc..392ab7f32 100644 --- a/flight/PiOS/STM32F10x/pios_pwm.c +++ b/flight/PiOS/STM32F10x/pios_pwm.c @@ -42,6 +42,8 @@ const struct pios_rcvr_driver pios_pwm_rcvr_driver = { }; /* Local Variables */ +/* 100 ms timeout without updates on channels */ +const static uint32_t PWM_SUPERVISOR_TIMEOUT = 100000; enum pios_pwm_dev_magic { PIOS_PWM_DEV_MAGIC = 0xab30293c, @@ -56,6 +58,7 @@ struct pios_pwm_dev { uint16_t FallValue[PIOS_PWM_NUM_INPUTS]; uint32_t CaptureValue[PIOS_PWM_NUM_INPUTS]; uint32_t CapCounter[PIOS_PWM_NUM_INPUTS]; + uint32_t us_since_update[PIOS_PWM_NUM_INPUTS]; }; static bool PIOS_PWM_validate(struct pios_pwm_dev * pwm_dev) @@ -120,7 +123,7 @@ int32_t PIOS_PWM_Init(uint32_t * pwm_id, const struct pios_pwm_cfg * cfg) pwm_dev->CaptureState[i] = 0; pwm_dev->RiseValue[i] = 0; pwm_dev->FallValue[i] = 0; - pwm_dev->CaptureValue[i] = 0; + pwm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; } uint32_t tim_id; @@ -152,6 +155,10 @@ int32_t PIOS_PWM_Init(uint32_t * pwm_id, const struct pios_pwm_cfg * cfg) TIM_ITConfig(chan->timer, TIM_IT_CC4, ENABLE); break; } + + // Need the update event for that timer to detect timeouts + TIM_ITConfig(chan->timer, TIM_IT_Update, ENABLE); + } *pwm_id = (uint32_t) pwm_dev; @@ -193,6 +200,20 @@ static void PIOS_PWM_tim_overflow_cb (uint32_t tim_id, uint32_t context, uint8_t return; } + if (channel >= pwm_dev->cfg->num_channels) { + /* Channel out of range */ + return; + } + + pwm_dev->us_since_update[channel] += count; + if(pwm_dev->us_since_update[channel] >= PWM_SUPERVISOR_TIMEOUT) { + pwm_dev->CaptureState[channel] = 0; + pwm_dev->RiseValue[channel] = 0; + pwm_dev->FallValue[channel] = 0; + pwm_dev->CaptureValue[channel] = PIOS_RCVR_TIMEOUT; + pwm_dev->us_since_update[channel] = 0; + } + return; } @@ -215,6 +236,7 @@ static void PIOS_PWM_tim_edge_cb (uint32_t tim_id, uint32_t context, uint8_t cha if (pwm_dev->CaptureState[chan_idx] == 0) { pwm_dev->RiseValue[chan_idx] = count; + pwm_dev->us_since_update[chan_idx] = 0; } else { pwm_dev->FallValue[chan_idx] = count; } From 89e640ae7fcb5fb9ef22f51cde2accbecc7c8f83 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sun, 4 Sep 2011 01:23:59 -0500 Subject: [PATCH 105/145] Make sure all receiver drivers return correct constants for invalid channels. --- flight/Modules/ManualControl/manualcontrol.c | 22 +++++++++---------- flight/PiOS/STM32F10x/pios_ppm.c | 4 ++-- flight/PiOS/STM32F10x/pios_pwm.c | 4 ++-- flight/PiOS/STM32F10x/pios_sbus.c | 2 +- flight/PiOS/STM32F10x/pios_spektrum.c | 6 ++--- flight/PiOS/inc/pios_rcvr.h | 4 ++-- .../OpenPilotOSX.xcodeproj/project.pbxproj | 2 ++ 7 files changed, 22 insertions(+), 22 deletions(-) diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c index de8937f92..ef0719510 100644 --- a/flight/Modules/ManualControl/manualcontrol.c +++ b/flight/Modules/ManualControl/manualcontrol.c @@ -222,8 +222,6 @@ static void manualControlTask(void *parameters) // until we decide to go to failsafe if(cmd.Channel[n] == PIOS_RCVR_TIMEOUT) valid_input_detected = false; - else if (cmd.Channel[n] == PIOS_RCVR_INVALID) - scaledChannel[n] = 0; else scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n], settings.ChannelMin[n], settings.ChannelNeutral[n]); } @@ -235,17 +233,17 @@ static void manualControlTask(void *parameters) settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE || settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE || // Check all channel mappings are valid - cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == PIOS_RCVR_INVALID || - cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == PIOS_RCVR_INVALID || - cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == PIOS_RCVR_INVALID || - cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == PIOS_RCVR_INVALID || - cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == PIOS_RCVR_INVALID || + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t) PIOS_RCVR_INVALID || + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t) PIOS_RCVR_INVALID || + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t) PIOS_RCVR_INVALID || + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t) PIOS_RCVR_INVALID || + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t) PIOS_RCVR_INVALID || // Check the driver is exists - cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == PIOS_RCVR_NODRIVER || - cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == PIOS_RCVR_NODRIVER || - cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == PIOS_RCVR_NODRIVER || - cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == PIOS_RCVR_NODRIVER || - cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == PIOS_RCVR_NODRIVER) { + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t) PIOS_RCVR_NODRIVER || + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t) PIOS_RCVR_NODRIVER || + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t) PIOS_RCVR_NODRIVER || + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t) PIOS_RCVR_NODRIVER || + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t) PIOS_RCVR_NODRIVER) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; diff --git a/flight/PiOS/STM32F10x/pios_ppm.c b/flight/PiOS/STM32F10x/pios_ppm.c index bee955861..0dd9aabba 100644 --- a/flight/PiOS/STM32F10x/pios_ppm.c +++ b/flight/PiOS/STM32F10x/pios_ppm.c @@ -211,12 +211,12 @@ static int32_t PIOS_PPM_Get(uint32_t rcvr_id, uint8_t channel) if (!PIOS_PPM_validate(ppm_dev)) { /* Invalid device specified */ - return -1; + return PIOS_RCVR_INVALID; } if (channel >= PIOS_PPM_IN_MAX_NUM_CHANNELS) { /* Channel out of range */ - return -1; + return PIOS_RCVR_INVALID; } return ppm_dev->CaptureValue[channel]; } diff --git a/flight/PiOS/STM32F10x/pios_pwm.c b/flight/PiOS/STM32F10x/pios_pwm.c index 392ab7f32..b49ce1abf 100644 --- a/flight/PiOS/STM32F10x/pios_pwm.c +++ b/flight/PiOS/STM32F10x/pios_pwm.c @@ -181,12 +181,12 @@ static int32_t PIOS_PWM_Get(uint32_t rcvr_id, uint8_t channel) if (!PIOS_PWM_validate(pwm_dev)) { /* Invalid device specified */ - return -1; + return PIOS_RCVR_INVALID; } if (channel >= PIOS_PWM_NUM_INPUTS) { /* Channel out of range */ - return -1; + return PIOS_RCVR_INVALID; } return pwm_dev->CaptureValue[channel]; } diff --git a/flight/PiOS/STM32F10x/pios_sbus.c b/flight/PiOS/STM32F10x/pios_sbus.c index a80fe8fe3..4c1c90618 100644 --- a/flight/PiOS/STM32F10x/pios_sbus.c +++ b/flight/PiOS/STM32F10x/pios_sbus.c @@ -183,7 +183,7 @@ static int32_t PIOS_SBUS_Get(uint32_t rcvr_id, uint8_t channel) { /* return error if channel is not available */ if (channel >= SBUS_NUMBER_OF_CHANNELS) { - return -1; + return PIOS_RCVR_INVALID; } return channel_data[channel]; } diff --git a/flight/PiOS/STM32F10x/pios_spektrum.c b/flight/PiOS/STM32F10x/pios_spektrum.c index 53142069a..24c034e88 100644 --- a/flight/PiOS/STM32F10x/pios_spektrum.c +++ b/flight/PiOS/STM32F10x/pios_spektrum.c @@ -264,12 +264,12 @@ static int32_t PIOS_SPEKTRUM_Get(uint32_t rcvr_id, uint8_t channel) { struct pios_spektrum_dev * spektrum_dev = (struct pios_spektrum_dev *)rcvr_id; - bool valid = PIOS_SPEKTRUM_validate(spektrum_dev); - PIOS_Assert(valid); + if(!PIOS_SPEKTRUM_validate(spektrum_dev)) + return PIOS_RCVR_INVALID; /* Return error if channel not available */ if (channel >= PIOS_SPEKTRUM_NUM_INPUTS) { - return -1; + return PIOS_RCVR_INVALID; } return spektrum_dev->fsm.CaptureValue[channel]; } diff --git a/flight/PiOS/inc/pios_rcvr.h b/flight/PiOS/inc/pios_rcvr.h index ce28d37af..a61798d8c 100644 --- a/flight/PiOS/inc/pios_rcvr.h +++ b/flight/PiOS/inc/pios_rcvr.h @@ -40,8 +40,8 @@ struct pios_rcvr_driver { extern int32_t PIOS_RCVR_Read(uint32_t rcvr_id, uint8_t channel); #define PIOS_RCVR_TIMEOUT 0 -#define PIOS_RCVR_NODRIVER 65534 -#define PIOS_RCVR_INVALID 65535 +#define PIOS_RCVR_NODRIVER -2 +#define PIOS_RCVR_INVALID -1 #endif /* PIOS_RCVR_H */ diff --git a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj index 0a0834048..a51d5a08d 100644 --- a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj +++ b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj @@ -98,6 +98,7 @@ 65643CAF1413322000A32F59 /* pios_sbus.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_sbus.h; sourceTree = ""; }; 65643CB01413322000A32F59 /* pios_spektrum_priv.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_spektrum_priv.h; sourceTree = ""; }; 65643CB91413456D00A32F59 /* pios_tim.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_tim.c; sourceTree = ""; }; + 65643CBA141350C200A32F59 /* pios_sbus.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_sbus.c; sourceTree = ""; }; 6572CB1613D0F2B200FC2972 /* link_STM32103CB_CC_Rev1_memory.ld */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; name = link_STM32103CB_CC_Rev1_memory.ld; path = ../../PiOS/STM32F10x/link_STM32103CB_CC_Rev1_memory.ld; sourceTree = SOURCE_ROOT; }; 6572CB1713D0F2B200FC2972 /* link_STM32103CB_CC_Rev1_sections.ld */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; name = link_STM32103CB_CC_Rev1_sections.ld; path = ../../PiOS/STM32F10x/link_STM32103CB_CC_Rev1_sections.ld; sourceTree = SOURCE_ROOT; }; 657CEEAD121DB6C8007A1FBE /* homelocation.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = homelocation.xml; sourceTree = ""; }; @@ -7779,6 +7780,7 @@ 65E8F0E411EFF25C00BBF654 /* pios_ppm.c */, 65E8F0E511EFF25C00BBF654 /* pios_pwm.c */, 6589A9DB131DEE76006BD67C /* pios_rtc.c */, + 65643CBA141350C200A32F59 /* pios_sbus.c */, 65E8F0E611EFF25C00BBF654 /* pios_servo.c */, 65E8F0E711EFF25C00BBF654 /* pios_spektrum.c */, 65E8F0E811EFF25C00BBF654 /* pios_spi.c */, From 510a1760caf0f348a79974b66176ba05133113de Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sun, 4 Sep 2011 03:45:36 -0500 Subject: [PATCH 106/145] When disconnected set any accessory channels to neutral. Otherwise the actuator module could keep the pitch at high. However the "right" thing to do seems very specific. For example negative pitch is probably preferable. --- flight/Modules/ManualControl/manualcontrol.c | 24 ++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c index ef0719510..eab3605be 100644 --- a/flight/Modules/ManualControl/manualcontrol.c +++ b/flight/Modules/ManualControl/manualcontrol.c @@ -282,6 +282,30 @@ static void manualControlTask(void *parameters) // Important: Throttle < 0 will reset Stabilization coefficients among other things. Either change this, // or leave throttle at IDLE speed or above when going into AUTO-failsafe. AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + + AccessoryDesiredData accessory; + // Set Accessory 0 + if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] != + MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + accessory.AccessoryVal = 0; + if(AccessoryDesiredInstSet(0, &accessory) != 0) + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } + // Set Accessory 1 + if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] != + MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + accessory.AccessoryVal = 0; + if(AccessoryDesiredInstSet(1, &accessory) != 0) + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } + // Set Accessory 2 + if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] != + MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + accessory.AccessoryVal = 0; + if(AccessoryDesiredInstSet(2, &accessory) != 0) + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } + } else { AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL); From 533ae9bb41600eff87396abaaff4baa6f30cd785 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sun, 4 Sep 2011 12:15:34 -0500 Subject: [PATCH 107/145] SBUS: Missed handling the S.Bus failsafe. Now returns PIOS_RVCR_TIMEOUT for either SBus failsafe mode or when no data for 100 ms. --- flight/PiOS/STM32F10x/pios_sbus.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/PiOS/STM32F10x/pios_sbus.c b/flight/PiOS/STM32F10x/pios_sbus.c index 4c1c90618..543da364b 100644 --- a/flight/PiOS/STM32F10x/pios_sbus.c +++ b/flight/PiOS/STM32F10x/pios_sbus.c @@ -59,7 +59,7 @@ static void PIOS_SBUS_Supervisor(uint32_t sbus_id); static void reset_channels(void) { for (int i = 0; i < SBUS_NUMBER_OF_CHANNELS; i++) { - channel_data[i] = 0; + channel_data[i] = PIOS_RCVR_TIMEOUT; } } From 82c5f9f0f4f3af0a0709b9022f48e0e760f60c15 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sun, 4 Sep 2011 12:37:39 -0500 Subject: [PATCH 108/145] PIOS_RCVR: Document return values better and use enum for them --- flight/PiOS/Common/pios_rcvr.c | 9 +++++++++ flight/PiOS/inc/pios_rcvr.h | 12 +++++++++--- 2 files changed, 18 insertions(+), 3 deletions(-) diff --git a/flight/PiOS/Common/pios_rcvr.c b/flight/PiOS/Common/pios_rcvr.c index b76e04a44..bd8cce3cd 100644 --- a/flight/PiOS/Common/pios_rcvr.c +++ b/flight/PiOS/Common/pios_rcvr.c @@ -76,6 +76,15 @@ out_fail: return(-1); } +/** + * @brief Reads an input channel from the appropriate driver + * @param[in] rcvr_id driver to read from + * @param[in] channel channel to read + * @returns Unitless input value + * @retval PIOS_RCVR_TIMEOUT indicates a failsafe or timeout from that channel + * @retval PIOS_RCVR_INVALID invalid channel for this driver (usually out of range supported) + * @retval PIOS_RCVR_NODRIVER driver was not initialized + */ int32_t PIOS_RCVR_Read(uint32_t rcvr_id, uint8_t channel) { if (rcvr_id == 0) diff --git a/flight/PiOS/inc/pios_rcvr.h b/flight/PiOS/inc/pios_rcvr.h index a61798d8c..ab493cd35 100644 --- a/flight/PiOS/inc/pios_rcvr.h +++ b/flight/PiOS/inc/pios_rcvr.h @@ -39,9 +39,15 @@ struct pios_rcvr_driver { /* Public Functions */ extern int32_t PIOS_RCVR_Read(uint32_t rcvr_id, uint8_t channel); -#define PIOS_RCVR_TIMEOUT 0 -#define PIOS_RCVR_NODRIVER -2 -#define PIOS_RCVR_INVALID -1 +/*! Define error codes for PIOS_RCVR_Get */ +enum PIOS_RCVR_errors { + /*! Indicates that a failsafe condition or missing receiver detected for that channel */ + PIOS_RCVR_TIMEOUT = 0, + /*! Channel is invalid for this driver (usually out of range supported) */ + PIOS_RCVR_INVALID = -1, + /*! Indicates that the driver for this channel has not been initialized */ + PIOS_RCVR_NODRIVER = -2 +}; #endif /* PIOS_RCVR_H */ From fa33c66f237ced555def3f1905cc39a02abfb6cb Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sun, 4 Sep 2011 13:34:49 -0500 Subject: [PATCH 109/145] OP-553 StabilizationSettings: Change default max roll/pitch to 55 degrees from 35 degrees. I hope this doesn't make it too sensitive for any of the beginngers. --- shared/uavobjectdefinition/stabilizationsettings.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/shared/uavobjectdefinition/stabilizationsettings.xml b/shared/uavobjectdefinition/stabilizationsettings.xml index 653c7cce8..e42f468bf 100644 --- a/shared/uavobjectdefinition/stabilizationsettings.xml +++ b/shared/uavobjectdefinition/stabilizationsettings.xml @@ -1,8 +1,8 @@ PID settings used by the Stabilization module to combine the @ref AttitudeActual and @ref AttitudeDesired to compute @ref ActuatorDesired - - + + From 1e9bcf8426edc3e7fbf4d6657671b3fc6667f9a8 Mon Sep 17 00:00:00 2001 From: Stacey Sheldon Date: Sun, 4 Sep 2011 23:22:29 -0400 Subject: [PATCH 110/145] openpilot: fix merge of unidirectional gps feature branch --- flight/OpenPilot/System/pios_board.c | 14 +------------- 1 file changed, 1 insertion(+), 13 deletions(-) diff --git a/flight/OpenPilot/System/pios_board.c b/flight/OpenPilot/System/pios_board.c index 2334f7a33..22bf12cad 100644 --- a/flight/OpenPilot/System/pios_board.c +++ b/flight/OpenPilot/System/pios_board.c @@ -1176,21 +1176,9 @@ void PIOS_Board_Init(void) { PIOS_Assert(rx_buffer); if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id, rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, - 0, 0)) { + NULL, 0)) { PIOS_Assert(0); } - { - uint32_t pios_usart_gps_id; - if (PIOS_USART_Init(&pios_usart_gps_id, &pios_usart_gps_cfg)) { - PIOS_Assert(0); - } - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN); - PIOS_Assert(rx_buffer); - if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id, - rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, - NULL, 0)) { - PIOS_Assert(0); ->>>>>>> origin/CorvusCorax_unidirectional-GPS-com } #endif /* PIOS_INCLUDE_GPS */ break; From 6344bc8f83adb305f11641d9e490ec59fb37a60e Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sun, 4 Sep 2011 01:36:57 -0500 Subject: [PATCH 111/145] Fix encoding no configccpmwidget.cpp --- .../src/plugins/config/configccpmwidget.cpp | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/configccpmwidget.cpp b/ground/openpilotgcs/src/plugins/config/configccpmwidget.cpp index e53cbb70c..9df53e420 100644 --- a/ground/openpilotgcs/src/plugins/config/configccpmwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configccpmwidget.cpp @@ -218,17 +218,17 @@ ConfigccpmWidget::ConfigccpmWidget(QWidget *parent) : ConfigTaskWidget(parent) m_ccpm->ccpmServoZChannel->setCurrentIndex(8); QStringList Types; - Types << "CCPM 2 Servo 90º" << "CCPM 3 Servo 90º" << "CCPM 4 Servo 90º" << "CCPM 3 Servo 120º" << "CCPM 3 Servo 140º" << "FP 2 Servo 90º" << "Custom - User Angles" << "Custom - Advanced Settings" ; + Types << QString::fromUtf8("CCPM 2 Servo 90º") << QString::fromUtf8("CCPM 3 Servo 90º") << + QString::fromUtf8("CCPM 4 Servo 90º") << QString::fromUtf8("CCPM 3 Servo 120º") << + QString::fromUtf8("CCPM 3 Servo 140º") << QString::fromUtf8("FP 2 Servo 90º") << + QString::fromUtf8("Custom - User Angles") << QString::fromUtf8("Custom - Advanced Settings"); m_ccpm->ccpmType->addItems(Types); m_ccpm->ccpmType->setCurrentIndex(m_ccpm->ccpmType->count() - 1); requestccpmUpdate(); UpdateCurveSettings(); - //disable changing number of points in curves until UAVObjects have more than 5 m_ccpm->NumCurvePoints->setEnabled(0); - - UpdateType(); @@ -332,7 +332,7 @@ void ConfigccpmWidget::UpdateType() NumServosDefined=4; //set values for pre defined heli types - if (TypeText.compare(QString("CCPM 2 Servo 90º"), Qt::CaseInsensitive)==0) + if (TypeText.compare(QString::fromUtf8("CCPM 2 Servo 90º"), Qt::CaseInsensitive)==0) { m_ccpm->ccpmAngleW->setValue(AdjustmentAngle + 0); m_ccpm->ccpmAngleX->setValue(fmod(AdjustmentAngle + 90,360)); @@ -348,7 +348,7 @@ void ConfigccpmWidget::UpdateType() NumServosDefined=2; } - if (TypeText.compare(QString("CCPM 3 Servo 90º"), Qt::CaseInsensitive)==0) + if (TypeText.compare(QString::fromUtf8("CCPM 3 Servo 90º"), Qt::CaseInsensitive)==0) { m_ccpm->ccpmAngleW->setValue(AdjustmentAngle + 0); m_ccpm->ccpmAngleX->setValue(fmod(AdjustmentAngle + 90,360)); @@ -361,7 +361,7 @@ void ConfigccpmWidget::UpdateType() NumServosDefined=3; } - if (TypeText.compare(QString("CCPM 4 Servo 90º"), Qt::CaseInsensitive)==0) + if (TypeText.compare(QString::fromUtf8("CCPM 4 Servo 90º"), Qt::CaseInsensitive)==0) { m_ccpm->ccpmAngleW->setValue(AdjustmentAngle + 0); m_ccpm->ccpmAngleX->setValue(fmod(AdjustmentAngle + 90,360)); @@ -373,7 +373,7 @@ void ConfigccpmWidget::UpdateType() NumServosDefined=4; } - if (TypeText.compare(QString("CCPM 3 Servo 120º"), Qt::CaseInsensitive)==0) + if (TypeText.compare(QString::fromUtf8("CCPM 3 Servo 120º"), Qt::CaseInsensitive)==0) { m_ccpm->ccpmAngleW->setValue(AdjustmentAngle + 0); m_ccpm->ccpmAngleX->setValue(fmod(AdjustmentAngle + 120,360)); @@ -386,7 +386,7 @@ void ConfigccpmWidget::UpdateType() NumServosDefined=3; } - if (TypeText.compare(QString("CCPM 3 Servo 140º"), Qt::CaseInsensitive)==0) + if (TypeText.compare(QString::fromUtf8("CCPM 3 Servo 140º"), Qt::CaseInsensitive)==0) { m_ccpm->ccpmAngleW->setValue(AdjustmentAngle + 0); m_ccpm->ccpmAngleX->setValue(fmod(AdjustmentAngle + 140,360)); @@ -399,7 +399,7 @@ void ConfigccpmWidget::UpdateType() NumServosDefined=3; } - if (TypeText.compare(QString("FP 2 Servo 90º"), Qt::CaseInsensitive)==0) + if (TypeText.compare(QString::fromUtf8("FP 2 Servo 90º"), Qt::CaseInsensitive)==0) { m_ccpm->ccpmAngleW->setValue(AdjustmentAngle + 0); m_ccpm->ccpmAngleX->setValue(fmod(AdjustmentAngle + 90,360)); From a81f14a5755c956962f25e2b729aa87c84128c5b Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sun, 4 Sep 2011 03:09:48 -0500 Subject: [PATCH 112/145] CCPM config: Fix the collective pass through mode for multi receiver support. Now the user must configure Accessory# in the input configuration section though. --- .../openpilotgcs/src/plugins/config/ccpm.ui | 2 +- .../src/plugins/config/configccpmwidget.cpp | 238 ++++++------------ .../src/plugins/config/configccpmwidget.h | 4 +- 3 files changed, 74 insertions(+), 170 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/ccpm.ui b/ground/openpilotgcs/src/plugins/config/ccpm.ui index 12d501f60..7730a7a29 100644 --- a/ground/openpilotgcs/src/plugins/config/ccpm.ui +++ b/ground/openpilotgcs/src/plugins/config/ccpm.ui @@ -70,7 +70,7 @@ - 2 + 0 diff --git a/ground/openpilotgcs/src/plugins/config/configccpmwidget.cpp b/ground/openpilotgcs/src/plugins/config/configccpmwidget.cpp index 9df53e420..a6b343bea 100644 --- a/ground/openpilotgcs/src/plugins/config/configccpmwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configccpmwidget.cpp @@ -70,8 +70,6 @@ ConfigccpmWidget::ConfigccpmWidget(QWidget *parent) : ConfigTaskWidget(parent) m_ccpm->SwashplateImage->setSceneRect(-50,-30,500,500); //m_ccpm->SwashplateImage->scale(.85,.85); - - QSvgRenderer *renderer = new QSvgRenderer(); renderer->load(QString(":/configgadget/images/ccpm_setup.svg")); @@ -142,68 +140,22 @@ ConfigccpmWidget::ConfigccpmWidget(QWidget *parent) : ConfigTaskWidget(parent) } - -/* - Servos[0] = new QGraphicsSvgItem(); - Servos[0]->setSharedRenderer(renderer); - Servos[0]->setElementId("ServoW"); - m_ccpm->SwashplateImage->scene()->addItem(Servos[0]); - - Servos[1] = new QGraphicsSvgItem(); - Servos[1]->setSharedRenderer(renderer); - Servos[1]->setElementId("ServoX"); - m_ccpm->SwashplateImage->scene()->addItem(Servos[1]); - - Servos[2] = new QGraphicsSvgItem(); - Servos[2]->setSharedRenderer(renderer); - Servos[2]->setElementId("ServoY"); - m_ccpm->SwashplateImage->scene()->addItem(Servos[2]); - - Servos[3] = new QGraphicsSvgItem(); - Servos[3]->setSharedRenderer(renderer); - Servos[3]->setElementId("ServoZ"); - m_ccpm->SwashplateImage->scene()->addItem(Servos[3]); - - - ServosText[0] = new QGraphicsTextItem(); - ServosText[0]->setDefaultTextColor(Qt::red); - ServosText[0]->setPlainText(QString("-")); - ServosText[0]->setFont(serifFont); - m_ccpm->SwashplateImage->scene()->addItem(ServosText[0]); - - ServosText[1] = new QGraphicsTextItem(); - ServosText[1]->setDefaultTextColor(Qt::red); - ServosText[1]->setPlainText(QString("-")); - ServosText[1]->setFont(serifFont); - m_ccpm->SwashplateImage->scene()->addItem(ServosText[1]); - - ServosText[2] = new QGraphicsTextItem(); - ServosText[2]->setDefaultTextColor(Qt::red); - ServosText[2]->setPlainText(QString("-")); - ServosText[2]->setFont(serifFont); - m_ccpm->SwashplateImage->scene()->addItem(ServosText[2]); - - ServosText[3] = new QGraphicsTextItem(); - ServosText[3]->setDefaultTextColor(Qt::red); - ServosText[3]->setPlainText(QString("-")); - ServosText[3]->setFont(serifFont); - m_ccpm->SwashplateImage->scene()->addItem(ServosText[3]); -*/ m_ccpm->PitchCurve->setMin(-1); resetMixer(m_ccpm->PitchCurve, 5); resetMixer(m_ccpm->ThrottleCurve, 5); + MixerSettings * mixerSettings = MixerSettings::GetInstance(getObjectManager()); + Q_ASSERT(mixerSettings); + UAVObjectField * curve2source = mixerSettings->getField("Curve2Source"); + Q_ASSERT(curve2source); - - + m_ccpm->ccpmCollectiveChannel->addItems(curve2source->getOptions()); + m_ccpm->ccpmCollectiveChannel->setCurrentIndex(0); QStringList channels; - channels << "Channel1" << "Channel2" << - "Channel3" << "Channel4" << "Channel5" << "Channel6" << "Channel7" << "Channel8" ; - m_ccpm->ccpmCollectiveChannel->addItems(channels); - m_ccpm->ccpmCollectiveChannel->setCurrentIndex(8); - channels << "None" ; + channels << "Channel1" << "Channel2" << "Channel3" << "Channel4" << + "Channel5" << "Channel6" << "Channel7" << "Channel8" << "None"; m_ccpm->ccpmEngineChannel->addItems(channels); m_ccpm->ccpmEngineChannel->setCurrentIndex(8); m_ccpm->ccpmTailChannel->addItems(channels); @@ -277,8 +229,6 @@ ConfigccpmWidget::ConfigccpmWidget(QWidget *parent) : ConfigTaskWidget(parent) ccpmSwashplateRedraw(); - // connect(parent, SIGNAL(autopilotConnected()),this, SLOT(requestccpmUpdate())); - } ConfigccpmWidget::~ConfigccpmWidget() @@ -292,7 +242,7 @@ void ConfigccpmWidget::UpdateType() QString TypeText; double AdjustmentAngle=0; - UpdatCCPMOptionsFromUI(); + UpdateCCPMOptionsFromUI(); SetUIComponentVisibilities(); TypeInt = m_ccpm->ccpmType->count() - m_ccpm->ccpmType->currentIndex()-1; @@ -915,14 +865,10 @@ void ConfigccpmWidget::UpdateMixer() bool useCyclic; int i,j,ThisEnable[6]; float CollectiveConstant,PitchConstant,RollConstant,ThisAngle[6]; - //QTableWidgetItem *newItem;// = new QTableWidgetItem(); QString Channel; ccpmChannelCheck(); - //Type = m_ccpm->ccpmType->count() - m_ccpm->ccpmType->currentIndex()-1; - //CollectiveConstant=m_ccpm->ccpmCollectiveSlider->value()/100.0; - //CorrectionAngle=m_ccpm->ccpmCorrectionAngle->value(); - UpdatCCPMOptionsFromUI(); + UpdateCCPMOptionsFromUI(); useCCPM = !(GUIConfigData.heli.ccpmCollectivePassthroughState || !GUIConfigData.heli.ccpmLinkCyclicState); useCyclic = GUIConfigData.heli.ccpmLinkRollState; @@ -978,15 +924,6 @@ void ConfigccpmWidget::UpdateMixer() //go through the user data and update the mixer matrix for (i=0;i<6;i++) { - /* - data.Mixer0Type = 0;//Disabled,Motor,Servo - data.Mixer0Vector[0] = 0;//ThrottleCurve1 - data.Mixer0Vector[1] = 0;//ThrottleCurve2 - data.Mixer0Vector[2] = 0;//Roll - data.Mixer0Vector[3] = 0;//Pitch - data.Mixer0Vector[4] = 0;//Yaw - - */ if ((MixerChannelData[i]<8)&&((ThisEnable[i])||(i<2))) { m_ccpm->ccpmAdvancedSettingsTable->item(i,0)->setText(QString("%1").arg( MixerChannelData[i]+1 )); @@ -1055,7 +992,7 @@ void ConfigccpmWidget::UpdateMixer() } __attribute__((packed)) heliGUISettingsStruct; */ -void ConfigccpmWidget::UpdatCCPMOptionsFromUI() +void ConfigccpmWidget::UpdateCCPMOptionsFromUI() { bool useCCPM; bool useCyclic; @@ -1097,7 +1034,6 @@ void ConfigccpmWidget::UpdatCCPMOptionsFromUI() GUIConfigData.heli.SliderValue1 = m_ccpm->ccpmPitchScale->value(); } GUIConfigData.heli.SliderValue2 = m_ccpm->ccpmRollScale->value(); - //GUIConfigData.heli.RevoSlider = m_ccpm->ccpmREVOScale->value(); //servo assignments GUIConfigData.heli.ServoIndexW = m_ccpm->ccpmServoWChannel->currentIndex(); @@ -1106,7 +1042,7 @@ void ConfigccpmWidget::UpdatCCPMOptionsFromUI() GUIConfigData.heli.ServoIndexZ = m_ccpm->ccpmServoZChannel->currentIndex(); } -void ConfigccpmWidget::UpdatCCPMUIFromOptions() +void ConfigccpmWidget::UpdateCCPMUIFromOptions() { //swashplate config m_ccpm->ccpmType->setCurrentIndex(m_ccpm->ccpmType->count() - (GUIConfigData.heli.SwasplateType +1)); @@ -1134,7 +1070,6 @@ void ConfigccpmWidget::UpdatCCPMUIFromOptions() m_ccpm->ccpmRollScaleBox->setValue(GUIConfigData.heli.SliderValue2); m_ccpm->ccpmCollectiveSlider->setValue(GUIConfigData.heli.SliderValue0); m_ccpm->ccpmCollectivespinBox->setValue(GUIConfigData.heli.SliderValue0); - //m_ccpm->ccpmREVOScale->setValue(GUIConfigData.heli.RevoSlider); //servo assignments m_ccpm->ccpmServoWChannel->setCurrentIndex(GUIConfigData.heli.ServoIndexW); @@ -1147,7 +1082,7 @@ void ConfigccpmWidget::UpdatCCPMUIFromOptions() void ConfigccpmWidget::SetUIComponentVisibilities() { - UpdatCCPMOptionsFromUI(); + UpdateCCPMOptionsFromUI(); //set which sliders are user... m_ccpm->ccpmRevoMixingBox->setVisible(0); @@ -1205,7 +1140,7 @@ void ConfigccpmWidget::requestccpmUpdate() for(i = 0; i < SystemSettings::GUICONFIGDATA_NUMELEM; i++) GUIConfigData.UAVObject[i]=systemSettingsData.GUIConfigData[i]; - UpdatCCPMUIFromOptions(); + UpdateCCPMUIFromOptions(); // Get existing mixer settings MixerSettings * mixerSettings = MixerSettings::GetInstance(getObjectManager()); @@ -1299,7 +1234,7 @@ void ConfigccpmWidget::requestccpmUpdate() } updatingFromHardware=FALSE; - UpdatCCPMUIFromOptions(); + UpdateCCPMUIFromOptions(); ccpmSwashplateUpdate(); } @@ -1317,109 +1252,78 @@ void ConfigccpmWidget::sendccpmUpdate() updatingToHardware=TRUE; //ShowDisclaimer(1); + UpdateCCPMOptionsFromUI(); + + // Store the data required to reconstruct + SystemSettings * systemSettings = SystemSettings::GetInstance(getObjectManager()); + Q_ASSERT(systemSettings); + SystemSettings::DataFields systemSettingsData = systemSettings->getData(); + systemSettingsData.GUIConfigData[0] = GUIConfigData.UAVObject[0]; + systemSettingsData.GUIConfigData[1] = GUIConfigData.UAVObject[1]; + systemSettings->setData(systemSettingsData); + systemSettings->updated(); - ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); - UAVObjectManager *objManager = pm->getObject(); + MixerSettings * mixerSettings = MixerSettings::GetInstance(getObjectManager()); + Q_ASSERT(mixerSettings); + MixerSettings::DataFields mixerSettingsData = mixerSettings->getData(); - UpdatCCPMOptionsFromUI(); - obj = dynamic_cast(getObjectManager()->getObject(QString("SystemSettings"))); - field = obj->getField(QString("GUIConfigData")); - field->setValue(GUIConfigData.UAVObject[0],0); - field->setValue(GUIConfigData.UAVObject[1],1); - obj->updated(); - + UpdateMixer(); - - obj = dynamic_cast(objManager->getObject(QString("MixerSettings"))); - Q_ASSERT(obj); + // Set up some helper pointers + qint8 * mixers[8] = {mixerSettingsData.Mixer1Vector, + mixerSettingsData.Mixer2Vector, + mixerSettingsData.Mixer3Vector, + mixerSettingsData.Mixer4Vector, + mixerSettingsData.Mixer5Vector, + mixerSettingsData.Mixer6Vector, + mixerSettingsData.Mixer7Vector, + mixerSettingsData.Mixer8Vector + }; - UpdateMixer(); + quint8 * mixerTypes[8] = { + &mixerSettingsData.Mixer1Type, + &mixerSettingsData.Mixer2Type, + &mixerSettingsData.Mixer3Type, + &mixerSettingsData.Mixer4Type, + &mixerSettingsData.Mixer5Type, + &mixerSettingsData.Mixer6Type, + &mixerSettingsData.Mixer7Type, + &mixerSettingsData.Mixer8Type + }; - //clear the output types - for (i=0;i<8;i++) + //go through the user data and update the mixer matrix + for (i=0;i<6;i++) + { + if (MixerChannelData[i]<8) { - field = obj->getField( QString( "Mixer%1Type" ).arg( i+1 )); - //clear the mixer type - field->setValue("Disabled"); + //set the mixer type + *(mixerTypes[MixerChannelData[i]]) = i==0 ? + MixerSettings::MIXER1TYPE_MOTOR : + MixerSettings::MIXER1TYPE_SERVO; + + //config the vector + for (j=0;j<5;j++) + mixers[MixerChannelData[i]][j] = m_ccpm->ccpmAdvancedSettingsTable->item(i,j+1)->text().toInt(); } + } + //get the user data for the curve into the mixer settings + for (i=0;i<5;i++) + mixerSettingsData.ThrottleCurve1[i] = m_ccpm->CurveSettings->item(i, 0)->text().toDouble(); - //go through the user data and update the mixer matrix - for (i=0;i<6;i++) - { - /* - data.Mixer0Type = 0;//Disabled,Motor,Servo - data.Mixer0Vector[0] = 0;//ThrottleCurve1 - data.Mixer0Vector[1] = 0;//ThrottleCurve2 - data.Mixer0Vector[2] = 0;//Roll - data.Mixer0Vector[3] = 0;//Pitch - data.Mixer0Vector[4] = 0;//Yaw - - */ - if (MixerChannelData[i]<8) - { - //select the correct mixer for this config element - field = obj->getField(QString( "Mixer%1Type" ).arg( MixerChannelData[i]+1 )); - //set the mixer type - if (i==0) - { - field->setValue("Motor"); - } - else - { - field->setValue("Servo"); - } - - //select the correct mixer for this config element - field = obj->getField(QString( "Mixer%1Vector" ).arg( MixerChannelData[i]+1 )); - //config the vector - for (j=0;j<5;j++) - { - field->setValue(m_ccpm->ccpmAdvancedSettingsTable->item(i,j+1)->text().toInt(),j); - } - - } - - } - - - //get the user data for the curve into the mixer settings - field = obj->getField(QString("ThrottleCurve1")); - for (i=0;i<5;i++) - { - field->setValue(m_ccpm->CurveSettings->item(i, 0)->text().toDouble(),i); - } - field = obj->getField(QString("ThrottleCurve2")); - for (i=0;i<5;i++) - { - field->setValue(m_ccpm->CurveSettings->item(i, 1)->text().toDouble(),i); - } - - obj->updated(); - - field = obj->getField(QString("Curve2Source")); + for (i=0;i<5;i++) + mixerSettingsData.ThrottleCurve2[i] = m_ccpm->CurveSettings->item(i, 1)->text().toDouble(); //mapping of collective input to curve 2... //MixerSettings.Curve2Source = Throttle,Roll,Pitch,Yaw,Accessory0,Accessory1,Accessory2,Accessory3,Accessory4,Accessory5 //check if we are using throttle or directly from a channel... if (GUIConfigData.heli.ccpmCollectivePassthroughState) - {// input channel - field->setValue("Accessory0"); - obj->updated(); - - obj = dynamic_cast(objManager->getObject(QString("ManualControlSettings"))); - Q_ASSERT(obj); - field = obj->getField(QString("Accessory0")); - field->setValue(tr( "Channel%1" ).arg(GUIConfigData.heli.CollectiveChannel+1)); - - } + mixerSettingsData.Curve2Source = GUIConfigData.heli.CollectiveChannel; else - {// throttle - - field->setValue("Throttle"); - } + mixerSettingsData.Curve2Source = MixerSettings::CURVE2SOURCE_THROTTLE; - obj->updated(); + mixerSettings->setData(mixerSettingsData); + mixerSettings->updated(); updatingToHardware=FALSE; } diff --git a/ground/openpilotgcs/src/plugins/config/configccpmwidget.h b/ground/openpilotgcs/src/plugins/config/configccpmwidget.h index ab5425229..a3a4313f7 100644 --- a/ground/openpilotgcs/src/plugins/config/configccpmwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configccpmwidget.h @@ -136,8 +136,8 @@ private: void SwashLvlCancelButtonPressed(); void SwashLvlFinishButtonPressed(); - void UpdatCCPMOptionsFromUI(); - void UpdatCCPMUIFromOptions(); + void UpdateCCPMOptionsFromUI(); + void UpdateCCPMUIFromOptions(); void SetUIComponentVisibilities(); void ccpmChannelCheck(); From acfb28b04bfa28f54183110764fd90505d7dbb02 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sun, 4 Sep 2011 14:51:02 -0500 Subject: [PATCH 113/145] Heli configuration: Make swashplate leveling routine using static accessor methods. Also got rid of incredibly annoying message when you tried to alt-tab out of GCS. --- .../src/plugins/config/configccpmwidget.cpp | 162 ++++++------------ .../src/plugins/config/configccpmwidget.h | 2 - 2 files changed, 53 insertions(+), 111 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/configccpmwidget.cpp b/ground/openpilotgcs/src/plugins/config/configccpmwidget.cpp index a6b343bea..4a103987a 100644 --- a/ground/openpilotgcs/src/plugins/config/configccpmwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configccpmwidget.cpp @@ -39,6 +39,7 @@ #include "mixersettings.h" #include "systemsettings.h" +#include "actuatorcommand.h" #define Pi 3.14159265358979323846 @@ -130,10 +131,6 @@ ConfigccpmWidget::ConfigccpmWidget(QWidget *parent) : ConfigTaskWidget(parent) SwashLvlSpinBoxes[i] = new QSpinBox(m_ccpm->SwashLvlSwashplateImage); // use QGraphicsView m_ccpm->SwashLvlSwashplateImage->scene()->addWidget(SwashLvlSpinBoxes[i]); - //SwashLvlSpinBoxes[i]->move(i*50+50,20); - //SwashLvlSpinBoxes[i]->resize(40,20); - //SwashLvlSpinBoxes[i]->heightForWidth() - SwashLvlSpinBoxes[i]->setFixedSize(50,20); SwashLvlSpinBoxes[i]->setMaximum(10000); SwashLvlSpinBoxes[i]->setMinimum(0); SwashLvlSpinBoxes[i]->setValue(0); @@ -455,6 +452,9 @@ void ConfigccpmWidget::UpdateCurveWidgets() void ConfigccpmWidget::updatePitchCurveValue(QList curveValues0,double Value0) { + Q_UNUSED(curveValues0); + Q_UNUSED(Value0); + int NumCurvePoints,i; double CurrentValue; QList internalCurveValues; @@ -476,6 +476,9 @@ void ConfigccpmWidget::updatePitchCurveValue(QList curveValues0,double V void ConfigccpmWidget::updateThrottleCurveValue(QList curveValues0,double Value0) { + Q_UNUSED(curveValues0); + Q_UNUSED(Value0); + int NumCurvePoints,i; double CurrentValue; QList internalCurveValues; @@ -1118,20 +1121,17 @@ void ConfigccpmWidget::requestccpmUpdate() #define MaxAngleError 2 int MixerDataFromHeli[8][5]; quint8 MixerOutputType[8]; - int EngineChannel,TailRotorChannel,ServoChannels[4],ServoAngles[4],SortAngles[4],CalcAngles[4],ServoCurve2[4]; + int EngineChannel,TailRotorChannel,ServoChannels[4],ServoAngles[4],SortAngles[4],ServoCurve2[4]; int NumServos=0; - double Collective=0.0; - double a1,a2; - int HeadRotation,temp; - int isCCPM=0; if (SwashLvlConfigurationInProgress)return; if (updatingToHardware)return; updatingFromHardware=TRUE; - int i,j; + unsigned int i,j; SystemSettings * systemSettings = SystemSettings::GetInstance(getObjectManager()); + Q_ASSERT(systemSettings); SystemSettings::DataFields systemSettingsData = systemSettings->getData(); Q_ASSERT(SystemSettings::GUICONFIGDATA_NUMELEM == @@ -1245,8 +1245,6 @@ void ConfigccpmWidget::requestccpmUpdate() void ConfigccpmWidget::sendccpmUpdate() { int i,j; - UAVObjectField *field; - UAVDataObject* obj; if (SwashLvlConfigurationInProgress)return; updatingToHardware=TRUE; @@ -1724,7 +1722,6 @@ void ConfigccpmWidget::enableSwashplateLevellingControl(bool state) mdata.gcsTelemetryUpdateMode = UAVObject::UPDATEMODE_ONCHANGE; mdata.gcsTelemetryUpdatePeriod = 100; SwashLvlConfigurationInProgress=1; - connect(qApp, SIGNAL(focusChanged(QWidget*,QWidget*)),this, SLOT(FocusChanged(QWidget*,QWidget*))); m_ccpm->TabObject->setTabEnabled(0,0); m_ccpm->TabObject->setTabEnabled(2,0); m_ccpm->TabObject->setTabEnabled(3,0); @@ -1735,7 +1732,6 @@ void ConfigccpmWidget::enableSwashplateLevellingControl(bool state) mdata = SwashLvlaccInitialData; // Restore metadata SwashLvlConfigurationInProgress=0; - disconnect(qApp, SIGNAL(focusChanged(QWidget*,QWidget*)),this, SLOT(FocusChanged(QWidget*,QWidget*))); m_ccpm->TabObject->setTabEnabled(0,1); m_ccpm->TabObject->setTabEnabled(2,1); m_ccpm->TabObject->setTabEnabled(3,1); @@ -1761,41 +1757,24 @@ void ConfigccpmWidget::setSwashplateLevel(int percent) SwashLvlServoInterlock=1; - UAVDataObject* obj = dynamic_cast(getObjectManager()->getObject(QString("ActuatorCommand"))); - UAVObjectField * channel = obj->getField("Channel"); + ActuatorCommand * actuatorCommand = ActuatorCommand::GetInstance(getObjectManager()); + ActuatorCommand::DataFields actuatorCommandData = actuatorCommand->getData(); - - - if (level==0) - { - for (i=0;isetValue(newSwashLvlConfiguration.Neutral[i],newSwashLvlConfiguration.ServoChannels[i]); - SwashLvlSpinBoxes[i]->setValue(newSwashLvlConfiguration.Neutral[i]); - } - - } - else if (level>0) - { - for (i=0;i 0) value = (newSwashLvlConfiguration.Max[i] - newSwashLvlConfiguration.Neutral[i])*level + newSwashLvlConfiguration.Neutral[i]; - channel->setValue(value,newSwashLvlConfiguration.ServoChannels[i]); - SwashLvlSpinBoxes[i]->setValue(value); - } - - } - else if (level<0) - { - for (i=0;isetValue(value,newSwashLvlConfiguration.ServoChannels[i]); - SwashLvlSpinBoxes[i]->setValue(value); - } + actuatorCommandData.Channel[newSwashLvlConfiguration.ServoChannels[i]] = value; + SwashLvlSpinBoxes[i]->setValue(value); } - obj->updated(); + + actuatorCommand->setData(actuatorCommandData); + actuatorCommand->updated(); + SwashLvlServoInterlock=0; return; @@ -1804,76 +1783,41 @@ return; void ConfigccpmWidget::SwashLvlSpinBoxChanged(int value) { + Q_UNUSED(value); int i; if (SwashLvlServoInterlock==1)return; - UAVDataObject* obj = dynamic_cast(getObjectManager()->getObject(QString("ActuatorCommand"))); - UAVObjectField * channel = obj->getField("Channel"); - switch (SwashLvlState) - { - case 0: - break; - case 1: //Neutral levelling - for (i=0;igetData(); + + for (i = 0; i < CCPM_MAX_SWASH_SERVOS; i++) { + value = SwashLvlSpinBoxes[i]->value(); + + switch (SwashLvlState) { - newSwashLvlConfiguration.Neutral[i]=SwashLvlSpinBoxes[i]->value(); - channel->setValue(newSwashLvlConfiguration.Neutral[i],newSwashLvlConfiguration.ServoChannels[i]); + case 1: //Neutral levelling + newSwashLvlConfiguration.Neutral[i]=value; + break; + case 2: //Max levelling + newSwashLvlConfiguration.Max[i] = value; + break; + case 3: //Min levelling + newSwashLvlConfiguration.Min[i]= value; + break; + case 4: //levelling verification + break; + case 5: //levelling complete + break; + default: + break; } - obj->updated(); - break; - case 2: //Max levelling - for (i=0;ivalue(); - channel->setValue(newSwashLvlConfiguration.Max[i],newSwashLvlConfiguration.ServoChannels[i]); - } - obj->updated(); - break; - case 3: //Min levelling - for (i=0;ivalue(); - channel->setValue(newSwashLvlConfiguration.Min[i],newSwashLvlConfiguration.ServoChannels[i]); - } - obj->updated(); - break; - case 4: //levelling verification - break; - case 5: //levelling complete - break; - default: - break; + + actuatorCommandData.Channel[newSwashLvlConfiguration.ServoChannels[i]] = value; } + + + actuatorCommand->setData(actuatorCommandData); + actuatorCommand->updated(); + return; } - - -void ConfigccpmWidget::FocusChanged(QWidget *oldFocus, QWidget *newFocus) -{ - if (SwashLvlConfigurationInProgress!=1) return; - QMessageBox msgBox; - int ret; - msgBox.setText("

Warning!!!

"); - - if ((this->isAncestorOf(oldFocus))&&(!this->isAncestorOf(newFocus))) - { - msgBox.setInformativeText("You are in the middle of the levelling routine
Changing focus will cancel all levelling and return the OP hardware to the state it was in before levelling began.

Do you want to continue the levelling routine?"); - msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::No); - msgBox.setDefaultButton(QMessageBox::Yes); - msgBox.setIcon(QMessageBox::Information); - ret = msgBox.exec(); - - if (ret == QMessageBox::Yes) - { - - //m_ccpm->TabObject->setCurrentIndex(1); - //m_ccpm->SwashPlateLevel->setFocus(Qt::MouseFocusReason); - //m_ccpm->SwashLvlInstructionsBox->setFocus(Qt::MouseFocusReason); - oldFocus->setFocus(Qt::MouseFocusReason); - } - if (ret == QMessageBox::No) - { - SwashLvlCancelButtonPressed(); - } - } -} diff --git a/ground/openpilotgcs/src/plugins/config/configccpmwidget.h b/ground/openpilotgcs/src/plugins/config/configccpmwidget.h index a3a4313f7..56898f0fb 100644 --- a/ground/openpilotgcs/src/plugins/config/configccpmwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configccpmwidget.h @@ -145,8 +145,6 @@ private: void enableSwashplateLevellingControl(bool state); void setSwashplateLevel(int percent); void SwashLvlSpinBoxChanged(int value); - void FocusChanged(QWidget *oldFocus, QWidget *newFocus); - virtual void refreshValues() {}; // Not used public slots: From a8ef57c6d667711dc4e041450d012e2e895e3711 Mon Sep 17 00:00:00 2001 From: Sambas Date: Tue, 6 Sep 2011 08:31:02 +0300 Subject: [PATCH 114/145] Redo DX8 changes --- flight/PiOS/STM32F10x/pios_spektrum.c | 18 +++++++----------- 1 file changed, 7 insertions(+), 11 deletions(-) diff --git a/flight/PiOS/STM32F10x/pios_spektrum.c b/flight/PiOS/STM32F10x/pios_spektrum.c index f0a611671..b2e26ad30 100644 --- a/flight/PiOS/STM32F10x/pios_spektrum.c +++ b/flight/PiOS/STM32F10x/pios_spektrum.c @@ -162,14 +162,10 @@ static int32_t PIOS_SPEKTRUM_UpdateFSM(struct pios_spektrum_fsm * fsm, uint8_t b { fsm->bytecount++; if (fsm->sync == 0) { - /* Known sync bytes, 0x01, 0x02, 0x12 */ + /* Known sync bytes, 0x01, 0x02, 0x12, 0xb2 */ + /* 0xb2 DX8 3bind pulses only */ if (fsm->bytecount == 2) { - if (b == 0x01) { - fsm->datalength=0; // 10bit - fsm->sync = 1; - fsm->bytecount = 2; - } - else if(b == 0x02) { + if ((b == 0x01) || (b == 0x02) || (b == 0xb2)) { fsm->datalength=0; // 10bit fsm->sync = 1; fsm->bytecount = 2; @@ -321,9 +317,9 @@ static void PIOS_SPEKTRUM_Supervisor(uint32_t spektrum_id) bool valid = PIOS_SPEKTRUM_validate(spektrum_dev); PIOS_Assert(valid); - /* 125hz */ + /* 625hz */ spektrum_dev->supv_timer++; - if(spektrum_dev->supv_timer > 5) { + if(spektrum_dev->supv_timer > 4) { /* sync between frames */ struct pios_spektrum_fsm * fsm = &(spektrum_dev->fsm); @@ -332,8 +328,8 @@ static void PIOS_SPEKTRUM_Supervisor(uint32_t spektrum_id) fsm->prev_byte = 0xFF; fsm->frame_error = 0; fsm->sync_of++; - /* watchdog activated after 100ms silence */ - if (fsm->sync_of > 12) { + /* watchdog activated after 200ms silence */ + if (fsm->sync_of > 30) { /* signal lost */ fsm->sync_of = 0; From a4dfe9ac4e46f2c516ba87f60de369161aea908b Mon Sep 17 00:00:00 2001 From: Oleg Semyonov Date: Tue, 6 Sep 2011 15:46:21 +0300 Subject: [PATCH 115/145] packaging: do not build not yet supported targets for package (save time) --- package/Makefile | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/package/Makefile b/package/Makefile index 0a197844a..5598be8c3 100644 --- a/package/Makefile +++ b/package/Makefile @@ -29,11 +29,11 @@ CLEAN_GROUND := YES CLEAN_FLIGHT := YES endif -# Set up targets (PPM target seems to be broken at the moment) -FW_TARGETS := $(addprefix fw_, ahrs pipxtreme coptercontrol openpilot) +# Set up targets +FW_TARGETS := $(addprefix fw_, coptercontrol) FW_TARGETS_TOOLS := $(addprefix fw_, coptercontrol) -BL_TARGETS := $(addprefix bl_, coptercontrol openpilot ahrs pipxtreme) -BU_TARGETS := $(addprefix bu_, coptercontrol openpilot ahrs pipxtreme) +BL_TARGETS := $(addprefix bl_, coptercontrol) +BU_TARGETS := $(addprefix bu_, coptercontrol) help: @echo From 72625d99710e28ffc9b356bf4b83429db41413e8 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Tue, 6 Sep 2011 16:35:28 -0500 Subject: [PATCH 116/145] PiOS RCVR: Make the public API use a 1 based indexing for channel numbers. This may or may not get into next, but if so anyone following it MUST reconfigure their inputs. --- flight/Modules/ManualControl/manualcontrol.c | 13 +++++++------ flight/PiOS/Common/pios_rcvr.c | 6 ++++++ 2 files changed, 13 insertions(+), 6 deletions(-) diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c index eab3605be..95b519037 100644 --- a/flight/Modules/ManualControl/manualcontrol.c +++ b/flight/Modules/ManualControl/manualcontrol.c @@ -398,8 +398,9 @@ static void resetRcvrActivity(struct rcvr_activity_fsm * fsm) } static void updateRcvrActivitySample(uint32_t rcvr_id, uint16_t samples[], uint8_t max_channels) { - for (uint8_t channel = 0; channel < max_channels; channel++) { - samples[channel] = PIOS_RCVR_Read(rcvr_id, channel); + for (uint8_t channel = 1; channel <= max_channels; channel++) { + // Subtract 1 because channels are 1 indexed + samples[channel - 1] = PIOS_RCVR_Read(rcvr_id, channel); } } @@ -408,12 +409,12 @@ static bool updateRcvrActivityCompare(uint32_t rcvr_id, struct rcvr_activity_fsm bool activity_updated = false; /* Compare the current value to the previous sampled value */ - for (uint8_t channel = 0; - channel < RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP; + for (uint8_t channel = 1; + channel <= RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP; channel++) { uint16_t delta; - uint16_t prev = fsm->prev[channel]; - uint16_t curr = PIOS_RCVR_Read(rcvr_id, channel); + uint16_t prev = fsm->prev[channel - 1]; // Subtract 1 because channels are 1 indexed + uint16_t curr = PIOS_RCVR_Read(rcvr_id, channel); if (curr > prev) { delta = curr - prev; } else { diff --git a/flight/PiOS/Common/pios_rcvr.c b/flight/PiOS/Common/pios_rcvr.c index 02778bad4..0f3988d7f 100644 --- a/flight/PiOS/Common/pios_rcvr.c +++ b/flight/PiOS/Common/pios_rcvr.c @@ -87,6 +87,12 @@ out_fail: */ int32_t PIOS_RCVR_Read(uint32_t rcvr_id, uint8_t channel) { + // Publicly facing API uses channel 1 for first channel + if(channel == 0) + return PIOS_RCVR_INVALID; + else + channel--; + if (rcvr_id == 0) return PIOS_RCVR_NODRIVER; From 8c91522e5417a591fc68057179e41f93f1f7d7bb Mon Sep 17 00:00:00 2001 From: James Cotton Date: Tue, 6 Sep 2011 16:42:03 -0500 Subject: [PATCH 117/145] UAVO: Reorder field names of ManualControlSettings which affects the order of channels listed in the UI configuration. This keeps it consistent with the previous releases. --- shared/uavobjectdefinition/manualcontrolsettings.xml | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/shared/uavobjectdefinition/manualcontrolsettings.xml b/shared/uavobjectdefinition/manualcontrolsettings.xml index d6d7b90a3..025ff642d 100644 --- a/shared/uavobjectdefinition/manualcontrolsettings.xml +++ b/shared/uavobjectdefinition/manualcontrolsettings.xml @@ -2,16 +2,16 @@ Settings to indicate how to decode receiver input by @ref ManualControlModule. + elementnames="Throttle,Roll,Pitch,Yaw,FlightMode,Accessory0,Accessory1,Accessory2"/> + elementnames="Throttle,Roll,Pitch,Yaw,FlightMode,Accessory0,Accessory1,Accessory2"/> + elementnames="Throttle,Roll,Pitch,Yaw,FlightMode,Accessory0,Accessory1,Accessory2"/> + elementnames="Throttle,Roll,Pitch,Yaw,FlightMode,Accessory0,Accessory1,Accessory2"/> From d496f81c68df56a22dcb557577338bfd1e4c0e52 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Tue, 6 Sep 2011 17:48:45 -0500 Subject: [PATCH 118/145] GCS Input config: Change the layout a bit to make the spacing between rows regular, and move some of the layout to the .ui file. --- .../src/plugins/config/configinputwidget.cpp | 22 ++-- .../src/plugins/config/configinputwidget.h | 2 - .../openpilotgcs/src/plugins/config/input.ui | 35 +++++- .../src/plugins/config/inputchannelform.ui | 116 +++++++++--------- 4 files changed, 98 insertions(+), 77 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp index 1590f52f0..3519e8350 100644 --- a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp @@ -46,7 +46,7 @@ #define STICK_MIN_MOVE -8 #define STICK_MAX_MOVE 8 -ConfigInputWidget::ConfigInputWidget(QWidget *parent) : ConfigTaskWidget(parent),wizardStep(wizardWelcome),loop(NULL),skipflag(false),goWizard(NULL) +ConfigInputWidget::ConfigInputWidget(QWidget *parent) : ConfigTaskWidget(parent),wizardStep(wizardWelcome),loop(NULL),skipflag(false) { manualCommandObj = ManualControlCommand::GetInstance(getObjectManager()); manualSettingsObj = ManualControlSettings::GetInstance(getObjectManager()); @@ -60,7 +60,7 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) : ConfigTaskWidget(parent) foreach(QString name,manualSettingsObj->getFields().at(0)->getElementNames()) { inputChannelForm * inp=new inputChannelForm(this,index==0); - m_config->advancedPage->layout()->addWidget(inp); + m_config->channelSettings->layout()->addWidget(inp); inp->ui->channelName->setText(name); addUAVObjectToWidgetRelation("ManualControlSettings","ChannelGroups",inp->ui->channelGroup,index); addUAVObjectToWidgetRelation("ManualControlSettings","ChannelNumber",inp->ui->channelNumber,index); @@ -69,12 +69,8 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) : ConfigTaskWidget(parent) addUAVObjectToWidgetRelation("ManualControlSettings","ChannelMax",inp->ui->channelMax,index); ++index; } - goWizard=new QPushButton(tr("Start Wizard"),this); - m_config->advancedPage->layout()->addWidget(goWizard); - connect(goWizard,SIGNAL(clicked()),this,SLOT(goToNormalWizard())); - goSimpleWizard=new QPushButton(tr("Start Simple Wizard"),this); - m_config->advancedPage->layout()->addWidget(goSimpleWizard); - connect(goSimpleWizard,SIGNAL(clicked()),this,SLOT(goToSimpleWizard())); + + connect(m_config->configurationWizard,SIGNAL(clicked()),this,SLOT(goToNormalWizard())); connect(m_config->wzNext,SIGNAL(clicked()),this,SLOT(wzNext())); connect(m_config->wzCancel,SIGNAL(clicked()),this,SLOT(wzCancel())); @@ -98,8 +94,6 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) : ConfigTaskWidget(parent) addUAVObjectToWidgetRelation("ManualControlSettings","Arming",m_config->armControl); addUAVObjectToWidgetRelation("ManualControlSettings","ArmedTimeout",m_config->armTimeout,0,1000); connect( ManualControlCommand::GetInstance(getObjectManager()),SIGNAL(objectUpdated(UAVObject*)),this,SLOT(moveFMSlider())); - addWidget(goWizard); - addWidget(goSimpleWizard); enableControls(false); populateWidgets(); @@ -912,11 +906,9 @@ void ConfigInputWidget::dimOtherControls(bool value) void ConfigInputWidget::enableControls(bool enable) { - if(goWizard) - { - goWizard->setEnabled(enable); - goSimpleWizard->setEnabled(enable); - } + m_config->configurationWizard->setEnabled(enable); + m_config->runCalibration->setEnabled(enable); + ConfigTaskWidget::enableControls(enable); } diff --git a/ground/openpilotgcs/src/plugins/config/configinputwidget.h b/ground/openpilotgcs/src/plugins/config/configinputwidget.h index 2249528cb..4cdf1b0a2 100644 --- a/ground/openpilotgcs/src/plugins/config/configinputwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configinputwidget.h @@ -116,8 +116,6 @@ private: QTimer * animate; void resetTxControls(); void setMoveFromCommand(int command); - QPushButton * goWizard; - QPushButton * goSimpleWizard; bool isSimple; void goToWizard(); private slots: diff --git a/ground/openpilotgcs/src/plugins/config/input.ui b/ground/openpilotgcs/src/plugins/config/input.ui index 0ec15f063..636ca9385 100644 --- a/ground/openpilotgcs/src/plugins/config/input.ui +++ b/ground/openpilotgcs/src/plugins/config/input.ui @@ -27,10 +27,41 @@ - 1 + 0 - + + + + + + + + Start Configuration Wizard + + + + + + + Run Calibration + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + diff --git a/ground/openpilotgcs/src/plugins/config/inputchannelform.ui b/ground/openpilotgcs/src/plugins/config/inputchannelform.ui index ac4c9e015..dcd469485 100644 --- a/ground/openpilotgcs/src/plugins/config/inputchannelform.ui +++ b/ground/openpilotgcs/src/plugins/config/inputchannelform.ui @@ -6,8 +6,8 @@ 0 0 - 404 - 43 + 449 + 48 @@ -26,46 +26,6 @@ 0 - - - - true - - - Number - - - - - - - true - - - Min - - - - - - - true - - - Function - - - - - - - true - - - Group - - - @@ -95,16 +55,6 @@ - - - - true - - - Neutral - - - @@ -112,13 +62,43 @@ - - + + 9999 + + + + true + + + Function + + + + + + + true + + + Number + + + + + + + true + + + Receiver type + + + @@ -129,10 +109,30 @@ - - - - 9999 + + + + true + + + Min + + + + + + + true + + + Neutral + + + + + + + Qt::Horizontal From cabfbc229dce02ff636ce3193d3915e3c879555e Mon Sep 17 00:00:00 2001 From: James Cotton Date: Tue, 6 Sep 2011 18:26:47 -0500 Subject: [PATCH 119/145] Input config: Bring back old style calibration. I can't seem to make the input widget list a class variable which isn't very safe (multipe instances of the config gadget will act funny). --- .../src/plugins/config/configinputwidget.cpp | 93 +++++++++++++++++++ .../src/plugins/config/configinputwidget.h | 3 + 2 files changed, 96 insertions(+) diff --git a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp index 3519e8350..f12ae4dd3 100644 --- a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp @@ -46,6 +46,8 @@ #define STICK_MIN_MOVE -8 #define STICK_MAX_MOVE 8 +QList inputList; + ConfigInputWidget::ConfigInputWidget(QWidget *parent) : ConfigTaskWidget(parent),wizardStep(wizardWelcome),loop(NULL),skipflag(false) { manualCommandObj = ManualControlCommand::GetInstance(getObjectManager()); @@ -56,9 +58,12 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) : ConfigTaskWidget(parent) setupButtons(m_config->saveRCInputToRAM,m_config->saveRCInputToSD); + inputList.clear(); + int index=0; foreach(QString name,manualSettingsObj->getFields().at(0)->getElementNames()) { + Q_ASSERT(index < ManualControlSettings::CHANNELGROUPS_NUMELEM); inputChannelForm * inp=new inputChannelForm(this,index==0); m_config->channelSettings->layout()->addWidget(inp); inp->ui->channelName->setText(name); @@ -67,10 +72,13 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) : ConfigTaskWidget(parent) addUAVObjectToWidgetRelation("ManualControlSettings","ChannelMin",inp->ui->channelMin,index); addUAVObjectToWidgetRelation("ManualControlSettings","ChannelNeutral",inp->ui->channelNeutral,index); addUAVObjectToWidgetRelation("ManualControlSettings","ChannelMax",inp->ui->channelMax,index); + inputList.append(inp); ++index; } connect(m_config->configurationWizard,SIGNAL(clicked()),this,SLOT(goToNormalWizard())); + connect(m_config->runCalibration,SIGNAL(toggled(bool)),this, SLOT(simpleCalibration(bool))); + connect(manualSettingsObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(settingsUpdated())); connect(m_config->wzNext,SIGNAL(clicked()),this,SLOT(wzNext())); connect(m_config->wzCancel,SIGNAL(clicked()),this,SLOT(wzCancel())); @@ -970,3 +978,88 @@ void ConfigInputWidget::moveFMSlider() m_config->fmsSlider->setValue(0); } } + +void ConfigInputWidget::updateCalibration() +{ + bool changed = false; + + manualCommandData=manualCommandObj->getData(); + for(uint i=0;imanualCommandData.Channel[i]) + manualSettingsData.ChannelMin[i]=manualCommandData.Channel[i]; + if(manualSettingsData.ChannelMax[i]setData(manualSettingsData); + manualSettingsObj->updated(); + settingsUpdated(); +} + +void ConfigInputWidget::settingsUpdated() +{ + manualSettingsData=manualSettingsObj->getData(); + Q_ASSERT(inputList.length() <= ManualControlSettings::CHANNELGROUPS_NUMELEM); + + for(int i = 0; i < inputList.length(); i++) { + inputList[i]->ui->channelNeutral->setMaximum(manualSettingsData.ChannelMax[i]); + inputList[i]->ui->channelNeutral->setMinimum(manualSettingsData.ChannelMin[i]); + inputList[i]->ui->channelNeutral->setValue(manualSettingsData.ChannelNeutral[i]); + } +} + +void ConfigInputWidget::simpleCalibration(bool enable) +{ + if (enable) { + QMessageBox msgBox; + msgBox.setText(tr("Arming Settings are now set to Always Disarmed for your safety.")); + msgBox.setDetailedText(tr("You will have to reconfigure arming settings yourself afterwards.")); + msgBox.setStandardButtons(QMessageBox::Ok); + msgBox.setDefaultButton(QMessageBox::Ok); + msgBox.exec(); + + manualCommandData = manualCommandObj->getData(); + + manualSettingsData=manualSettingsObj->getData(); + manualSettingsData.Arming=ManualControlSettings::ARMING_ALWAYSDISARMED; + manualSettingsObj->setData(manualSettingsData); + + for (int i = 0; i < ManualControlCommand::CHANNEL_NUMELEM; i++) { + manualSettingsData.ChannelMin[i] = manualCommandData.Channel[i]; + manualSettingsData.ChannelNeutral[i] = manualCommandData.Channel[i]; + manualSettingsData.ChannelMax[i] = manualCommandData.Channel[i]; + } + + UAVObject::Metadata mdata= manualCommandObj->getMetadata(); + mdata.flightTelemetryUpdateMode = UAVObject::UPDATEMODE_PERIODIC; + mdata.flightTelemetryUpdatePeriod = 150; + manualCommandObj->setMetadata(mdata); + + connect(manualCommandObj, SIGNAL(objectUnpacked(UAVObject*)), this, SLOT(updateCalibration())); + } else { + manualCommandData = manualCommandObj->getData(); + manualSettingsData = manualSettingsObj->getData(); + + manualCommandObj->setMetadata(manualCommandObj->getDefaultMetadata()); + + for (int i = 0; i < ManualControlCommand::CHANNEL_NUMELEM; i++) + manualSettingsData.ChannelNeutral[i] = manualCommandData.Channel[i]; + + // Force flight mode neutral to middle + manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNUMBER_FLIGHTMODE] = + (manualSettingsData.ChannelMax[ManualControlSettings::CHANNELNUMBER_FLIGHTMODE] + + manualSettingsData.ChannelMin[ManualControlSettings::CHANNELNUMBER_FLIGHTMODE]) / 2; + + // Force throttle to be near min + manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_THROTTLE]= + manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_THROTTLE]+ + ((manualSettingsData.ChannelMax[ManualControlSettings::CHANNELMAX_THROTTLE]- + manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_THROTTLE])*0.02); + + manualSettingsObj->setData(manualSettingsData); + + disconnect(manualCommandObj, SIGNAL(objectUnpacked(UAVObject*)), this, SLOT(updateCalibration())); + } +} diff --git a/ground/openpilotgcs/src/plugins/config/configinputwidget.h b/ground/openpilotgcs/src/plugins/config/configinputwidget.h index 4cdf1b0a2..912867cf6 100644 --- a/ground/openpilotgcs/src/plugins/config/configinputwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configinputwidget.h @@ -132,6 +132,9 @@ private slots: void dimOtherControls(bool value); void moveFMSlider(); void invertControls(); + void simpleCalibration(bool state); + void updateCalibration(); + void settingsUpdated(); protected: void resizeEvent(QResizeEvent *event); virtual void enableControls(bool enable); From 1a5fd9f30e081bc20e3ba14a21142fab19b36f6f Mon Sep 17 00:00:00 2001 From: James Cotton Date: Tue, 6 Sep 2011 23:16:34 -0500 Subject: [PATCH 120/145] Heli: Added an explicit collective channel. Also make the default channel number 0 since that is not invalid. --- flight/Modules/Actuator/actuator.c | 6 +++++- flight/Modules/ManualControl/manualcontrol.c | 6 ++++++ shared/uavobjectdefinition/manualcontrolcommand.xml | 5 +++-- shared/uavobjectdefinition/manualcontrolsettings.xml | 12 ++++++------ shared/uavobjectdefinition/mixersettings.xml | 2 +- 5 files changed, 21 insertions(+), 10 deletions(-) diff --git a/flight/Modules/Actuator/actuator.c b/flight/Modules/Actuator/actuator.c index 6b353d20f..2f5765016 100644 --- a/flight/Modules/Actuator/actuator.c +++ b/flight/Modules/Actuator/actuator.c @@ -42,7 +42,7 @@ #include "mixersettings.h" #include "mixerstatus.h" #include "cameradesired.h" - +#include "manualcontrolcommand.h" // Private constants #define MAX_QUEUE_SIZE 2 @@ -236,6 +236,10 @@ static void actuatorTask(void* parameters) case MIXERSETTINGS_CURVE2SOURCE_YAW: curve2 = MixerCurve(desired.Yaw,mixerSettings.ThrottleCurve2); break; + case MIXERSETTINGS_CURVE2SOURCE_COLLECTIVE: + ManualControlCommandCollectiveGet(&curve2); + curve2 = MixerCurve(curve2,mixerSettings.ThrottleCurve2); + break; case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY0: case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY1: case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY2: diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c index 95b519037..bfbc626f6 100644 --- a/flight/Modules/ManualControl/manualcontrol.c +++ b/flight/Modules/ManualControl/manualcontrol.c @@ -278,6 +278,7 @@ static void manualControlTask(void *parameters) cmd.Roll = 0; cmd.Yaw = 0; cmd.Pitch = 0; + cmd.Collective = 0; //cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO; // don't do until AUTO implemented and functioning // Important: Throttle < 0 will reset Stabilization coefficients among other things. Either change this, // or leave throttle at IDLE speed or above when going into AUTO-failsafe. @@ -316,6 +317,11 @@ static void manualControlTask(void *parameters) cmd.Throttle = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]; flightMode = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE]; + if(cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != PIOS_RCVR_INVALID && + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != PIOS_RCVR_NODRIVER && + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != PIOS_RCVR_TIMEOUT) + cmd.Collective = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE]; + AccessoryDesiredData accessory; // Set Accessory 0 if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] != diff --git a/shared/uavobjectdefinition/manualcontrolcommand.xml b/shared/uavobjectdefinition/manualcontrolcommand.xml index ef74c2576..626115233 100644 --- a/shared/uavobjectdefinition/manualcontrolcommand.xml +++ b/shared/uavobjectdefinition/manualcontrolcommand.xml @@ -2,10 +2,11 @@ The output from the @ref ManualControlModule which descodes the receiver inputs. Overriden by GCS for fly-by-wire control. - + + - + diff --git a/shared/uavobjectdefinition/manualcontrolsettings.xml b/shared/uavobjectdefinition/manualcontrolsettings.xml index 025ff642d..ae7c9b24c 100644 --- a/shared/uavobjectdefinition/manualcontrolsettings.xml +++ b/shared/uavobjectdefinition/manualcontrolsettings.xml @@ -2,16 +2,16 @@ Settings to indicate how to decode receiver input by @ref ManualControlModule. - + + elementnames="Throttle,Roll,Pitch,Yaw,FlightMode,Collective,Accessory0,Accessory1,Accessory2"/> + elementnames="Throttle,Roll,Pitch,Yaw,FlightMode,Collective,Accessory0,Accessory1,Accessory2"/> + elementnames="Throttle,Roll,Pitch,Yaw,FlightMode,Collective,Accessory0,Accessory1,Accessory2"/> diff --git a/shared/uavobjectdefinition/mixersettings.xml b/shared/uavobjectdefinition/mixersettings.xml index e070814c5..64aa7604f 100644 --- a/shared/uavobjectdefinition/mixersettings.xml +++ b/shared/uavobjectdefinition/mixersettings.xml @@ -6,7 +6,7 @@ - + From bbdb176409f39c0d20aab3519af94754c26650ce Mon Sep 17 00:00:00 2001 From: James Cotton Date: Wed, 7 Sep 2011 01:47:10 -0500 Subject: [PATCH 121/145] Input configuration: Make input channel into a dropdown box and make the neutral position into a slider. During calibration the slider moves and the min and max values are updated appropriately. Also make the collective channel skippable in the configuration wizard. --- .../src/plugins/config/configinputwidget.cpp | 21 +---- .../src/plugins/config/configinputwidget.h | 1 - .../src/plugins/config/configtaskwidget.cpp | 15 ++++ .../src/plugins/config/inputchannelform.cpp | 80 +++++++++++++++++++ .../src/plugins/config/inputchannelform.h | 7 ++ .../src/plugins/config/inputchannelform.ui | 64 ++++++++++++--- 6 files changed, 156 insertions(+), 32 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp index f12ae4dd3..a1051f3d4 100644 --- a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp @@ -78,7 +78,6 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) : ConfigTaskWidget(parent) connect(m_config->configurationWizard,SIGNAL(clicked()),this,SLOT(goToNormalWizard())); connect(m_config->runCalibration,SIGNAL(toggled(bool)),this, SLOT(simpleCalibration(bool))); - connect(manualSettingsObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(settingsUpdated())); connect(m_config->wzNext,SIGNAL(clicked()),this,SLOT(wzNext())); connect(m_config->wzCancel,SIGNAL(clicked()),this,SLOT(wzCancel())); @@ -565,7 +564,8 @@ void ConfigInputWidget::identifyControls() } m_config->wzText->setText(QString(tr("Please move each control once at a time according to the instructions and picture below.\n\n" "Move the %1 stick")).arg(manualSettingsObj->getFields().at(0)->getElementNames().at(currentCommand))); - if(manualSettingsObj->getField("ChannelGroups")->getElementNames().at(currentCommand).contains("Accessory")) + if(manualSettingsObj->getField("ChannelGroups")->getElementNames().at(currentCommand).contains("Accessory") || + manualSettingsObj->getField("ChannelGroups")->getElementNames().at(currentCommand).contains("Collective")) { m_config->wzNext->setEnabled(true); } @@ -981,8 +981,6 @@ void ConfigInputWidget::moveFMSlider() void ConfigInputWidget::updateCalibration() { - bool changed = false; - manualCommandData=manualCommandObj->getData(); for(uint i=0;isetData(manualSettingsData); manualSettingsObj->updated(); - settingsUpdated(); -} - -void ConfigInputWidget::settingsUpdated() -{ - manualSettingsData=manualSettingsObj->getData(); - Q_ASSERT(inputList.length() <= ManualControlSettings::CHANNELGROUPS_NUMELEM); - - for(int i = 0; i < inputList.length(); i++) { - inputList[i]->ui->channelNeutral->setMaximum(manualSettingsData.ChannelMax[i]); - inputList[i]->ui->channelNeutral->setMinimum(manualSettingsData.ChannelMin[i]); - inputList[i]->ui->channelNeutral->setValue(manualSettingsData.ChannelNeutral[i]); - } } void ConfigInputWidget::simpleCalibration(bool enable) @@ -1026,7 +1011,7 @@ void ConfigInputWidget::simpleCalibration(bool enable) manualSettingsData.Arming=ManualControlSettings::ARMING_ALWAYSDISARMED; manualSettingsObj->setData(manualSettingsData); - for (int i = 0; i < ManualControlCommand::CHANNEL_NUMELEM; i++) { + for (unsigned int i = 0; i < ManualControlCommand::CHANNEL_NUMELEM; i++) { manualSettingsData.ChannelMin[i] = manualCommandData.Channel[i]; manualSettingsData.ChannelNeutral[i] = manualCommandData.Channel[i]; manualSettingsData.ChannelMax[i] = manualCommandData.Channel[i]; diff --git a/ground/openpilotgcs/src/plugins/config/configinputwidget.h b/ground/openpilotgcs/src/plugins/config/configinputwidget.h index 912867cf6..0b95371c5 100644 --- a/ground/openpilotgcs/src/plugins/config/configinputwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configinputwidget.h @@ -134,7 +134,6 @@ private slots: void invertControls(); void simpleCalibration(bool state); void updateCalibration(); - void settingsUpdated(); protected: void resizeEvent(QResizeEvent *event); virtual void enableControls(bool enable); diff --git a/ground/openpilotgcs/src/plugins/config/configtaskwidget.cpp b/ground/openpilotgcs/src/plugins/config/configtaskwidget.cpp index eeb6c08aa..98b10683c 100644 --- a/ground/openpilotgcs/src/plugins/config/configtaskwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configtaskwidget.cpp @@ -48,7 +48,9 @@ void ConfigTaskWidget::addUAVObjectToWidgetRelation(QString object, QString fiel UAVObject *obj=NULL; UAVObjectField *_field=NULL; obj = objManager->getObject(QString(object)); + Q_ASSERT(obj); _field = obj->getField(QString(field)); + Q_ASSERT(_field); addUAVObjectToWidgetRelation(object,field,widget,_field->getElementNames().indexOf(index)); } @@ -59,6 +61,7 @@ void ConfigTaskWidget::addUAVObjectToWidgetRelation(QString object, QString fiel if(!object.isEmpty()) { obj = objManager->getObject(QString(object)); + Q_ASSERT(obj); connect(obj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(refreshWidgetsValues())); } //smartsave->addObject(obj); @@ -182,6 +185,10 @@ void ConfigTaskWidget::populateWidgets() { cb->setValue(ow->field->getValue(ow->index).toInt()/ow->scale); } + else if(QSlider * cb=qobject_cast(ow->widget)) + { + cb->setValue(ow->field->getValue(ow->index).toInt()/ow->scale); + } } setDirty(dirtyBack); } @@ -207,6 +214,10 @@ void ConfigTaskWidget::refreshWidgetsValues() { cb->setValue(ow->field->getValue(ow->index).toInt()/ow->scale); } + else if(QSlider * cb=qobject_cast(ow->widget)) + { + cb->setValue(ow->field->getValue(ow->index).toInt()/ow->scale); + } } setDirty(dirtyBack); } @@ -231,6 +242,10 @@ void ConfigTaskWidget::updateObjectsFromWidgets() { ow->field->setValue(cb->value()* ow->scale,ow->index); } + else if(QSlider * cb=qobject_cast(ow->widget)) + { + ow->field->setValue(cb->value()* ow->scale,ow->index); + } } } diff --git a/ground/openpilotgcs/src/plugins/config/inputchannelform.cpp b/ground/openpilotgcs/src/plugins/config/inputchannelform.cpp index 904918a8e..72c2cfd5e 100644 --- a/ground/openpilotgcs/src/plugins/config/inputchannelform.cpp +++ b/ground/openpilotgcs/src/plugins/config/inputchannelform.cpp @@ -1,6 +1,8 @@ #include "inputchannelform.h" #include "ui_inputchannelform.h" +#include "manualcontrolsettings.h" + inputChannelForm::inputChannelForm(QWidget *parent,bool showlegend) : QWidget(parent), ui(new Ui::inputChannelForm) @@ -21,9 +23,87 @@ inputChannelForm::inputChannelForm(QWidget *parent,bool showlegend) : delete ui->legend4; delete ui->legend5; } + + connect(ui->channelMin,SIGNAL(valueChanged(int)),this,SLOT(minUpdated(int))); + connect(ui->channelMax,SIGNAL(valueChanged(int)),this,SLOT(maxUpdated(int))); + connect(ui->channelGroup,SIGNAL(currentIndexChanged(int)),this,SLOT(groupUpdated())); + + // This is awkward but since we want the UI to be a dropdown but the field is not an enum + // it breaks the UAUVObject widget relation of the task gadget. Running the data through + // a spin box fixes this + connect(ui->channelNumberDropdown,SIGNAL(currentIndexChanged(int)),this,SLOT(channelDropdownUpdated(int))); + connect(ui->channelNumber,SIGNAL(valueChanged(int)),this,SLOT(channelNumberUpdated(int))); } inputChannelForm::~inputChannelForm() { delete ui; } + +void inputChannelForm::minUpdated(int newval) +{ + ui->channelNeutral->setMinimum(newval); +} + +void inputChannelForm::maxUpdated(int newval) +{ + ui->channelNeutral->setMaximum(newval); +} + +/** + * Update the channel options based on the selected receiver type + * + * I fully admit this is terrible practice to embed data within UI + * like this. Open to suggestions. JC 2011-09-07 + */ +void inputChannelForm::groupUpdated() +{ + ui->channelNumberDropdown->clear(); + ui->channelNumberDropdown->addItem("Disabled"); + + quint8 count = 0; + + switch(ui->channelGroup->currentIndex()) { + case -1: // Nothing selected + count = 0; + break; + case ManualControlSettings::CHANNELGROUPS_PWM: + count = 8; // Need to make this 6 for CC + break; + case ManualControlSettings::CHANNELGROUPS_PPM: + case ManualControlSettings::CHANNELGROUPS_SBUS: + case ManualControlSettings::CHANNELGROUPS_SPEKTRUM1: + case ManualControlSettings::CHANNELGROUPS_SPEKTRUM2: + count = 12; + break; + case ManualControlSettings::CHANNELGROUPS_GCS: + count = 5; + case ManualControlSettings::CHANNELGROUPS_NONE: + count = 0; + break; + default: + Q_ASSERT(0); + } + + for (int i = 0; i < count; i++) + ui->channelNumberDropdown->addItem(QString(tr("Chan %1").arg(i+1))); + + ui->channelNumber->setMaximum(count); + ui->channelNumber->setMinimum(0); +} + +/** + * Update the dropdown from the hidden control + */ +void inputChannelForm::channelDropdownUpdated(int newval) +{ + ui->channelNumber->setValue(newval); +} + +/** + * Update the hidden control from the dropdown + */ +void inputChannelForm::channelNumberUpdated(int newval) +{ + ui->channelNumberDropdown->setCurrentIndex(newval); +} diff --git a/ground/openpilotgcs/src/plugins/config/inputchannelform.h b/ground/openpilotgcs/src/plugins/config/inputchannelform.h index 50615cb2c..2567ba275 100644 --- a/ground/openpilotgcs/src/plugins/config/inputchannelform.h +++ b/ground/openpilotgcs/src/plugins/config/inputchannelform.h @@ -16,6 +16,13 @@ public: ~inputChannelForm(); friend class ConfigInputWidget; +private slots: + void minUpdated(int); + void maxUpdated(int); + void groupUpdated(); + void channelDropdownUpdated(int); + void channelNumberUpdated(int); + private: Ui::inputChannelForm *ui; }; diff --git a/ground/openpilotgcs/src/plugins/config/inputchannelform.ui b/ground/openpilotgcs/src/plugins/config/inputchannelform.ui index dcd469485..1e8b53c7c 100644 --- a/ground/openpilotgcs/src/plugins/config/inputchannelform.ui +++ b/ground/openpilotgcs/src/plugins/config/inputchannelform.ui @@ -6,7 +6,7 @@ 0 0 - 449 + 482 48 @@ -26,7 +26,7 @@ 0 - + @@ -45,24 +45,30 @@ - - - - - - - 255 + + + + + 5 + 0 + + + + + 100 + 16777215 + - + 9999 - + 9999 @@ -95,7 +101,7 @@ true - Receiver type + Type @@ -129,13 +135,45 @@ - + Qt::Horizontal + + + + + 4 + 0 + + + + + 80 + 16777215 + + + + 7 + + + + + + + true + + + + 0 + 0 + + + + From 1a2750051a793d2cf6eae4b5423bdfcb5d6e51f4 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Wed, 7 Sep 2011 09:09:41 -0500 Subject: [PATCH 122/145] Input wizard: Make collective pitch come first for helicopter mode transmitters and tell person to use throttle hold so it's detected correctly. --- .../src/plugins/config/configinputwidget.cpp | 106 ++++++++++++++---- .../src/plugins/config/configinputwidget.h | 5 +- .../openpilotgcs/src/plugins/config/input.ui | 9 +- 3 files changed, 88 insertions(+), 32 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp index a1051f3d4..09ac656de 100644 --- a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp @@ -48,7 +48,7 @@ QList inputList; -ConfigInputWidget::ConfigInputWidget(QWidget *parent) : ConfigTaskWidget(parent),wizardStep(wizardWelcome),loop(NULL),skipflag(false) +ConfigInputWidget::ConfigInputWidget(QWidget *parent) : ConfigTaskWidget(parent),wizardStep(wizardWelcome),loop(NULL),skipflag(false),transmitterType(heli) { manualCommandObj = ManualControlCommand::GetInstance(getObjectManager()); manualSettingsObj = ManualControlSettings::GetInstance(getObjectManager()); @@ -296,6 +296,7 @@ void ConfigInputWidget::wzCancel() case wizardWelcome: break; case wizardChooseMode: + case wizardChooseType: break; case wizardIdentifySticks: disconnect(receiverActivityObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(identifyControls())); @@ -323,7 +324,12 @@ void ConfigInputWidget::wzNext() void ConfigInputWidget::wzBack() { - setupWizardWidget(wizardStep-1); + if(wizardStep == wizardIdentifySticks && currentCommand != 0) { + currentCommand --; + usedChannels.removeLast(); + getChannelFromStep(currentCommand); + } else + setupWizardWidget(wizardStep-1); } void ConfigInputWidget::setupWizardWidget(int step) @@ -372,9 +378,8 @@ void ConfigInputWidget::setupWizardWidget(int step) m_config->checkBoxesLayout->layout()->addWidget(mode2); wizardStep=wizardChooseMode; } - else if(step==wizardIdentifySticks && !isSimple) + else if(step==wizardChooseType) { - usedChannels.clear(); if(wizardStep==wizardChooseMode) { QRadioButton * mode=qobject_cast(extraWidgets.at(0)); @@ -386,28 +391,45 @@ void ConfigInputWidget::setupWizardWidget(int step) delete extraWidgets.at(1); extraWidgets.clear(); } + + m_config->wzText->setText(tr("Please choose your transmiter mode.\n" + "Acro means normal transmitter\n" + "Heli means there is a collective pitch and throttle input\n")); + m_config->wzBack->setEnabled(true); + QRadioButton * typeAcro=new QRadioButton(tr("Acro"),this); + QRadioButton * typeHeli=new QRadioButton(tr("Heli"),this); + typeAcro->setChecked(true); + typeHeli->setChecked(false); + extraWidgets.clear(); + extraWidgets.append(typeAcro); + extraWidgets.append(typeHeli); + m_config->checkBoxesLayout->layout()->addWidget(typeAcro); + m_config->checkBoxesLayout->layout()->addWidget(typeHeli); + wizardStep=wizardChooseType; + } else if(step==wizardIdentifySticks && !isSimple) { + usedChannels.clear(); + if(wizardStep==wizardChooseType) + { + qDebug() << "Chosing type"; + QRadioButton * type=qobject_cast(extraWidgets.at(0)); + if(type->isChecked()) + transmitterType=acro; + else + transmitterType=heli; + qDebug() << "Checked: " << type->isChecked() << " " << "type is" << transmitterType; + delete extraWidgets.at(0); + delete extraWidgets.at(1); + extraWidgets.clear(); + } wizardStep=wizardIdentifySticks; currentCommand=0; - setMoveFromCommand(currentCommand); - m_config->wzText->setText(QString(tr("Please move each control once at a time according to the instructions and picture below.\n\n" - "Move the %1 stick")).arg(manualSettingsObj->getField("ChannelGroups")->getElementNames().at(currentCommand))); + getChannelFromStep(currentCommand); manualSettingsData=manualSettingsObj->getData(); connect(receiverActivityObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(identifyControls())); m_config->wzNext->setEnabled(false); } else if(step==wizardIdentifyCenter || (isSimple && step==wizardIdentifySticks)) { - if(wizardStep==wizardChooseMode) - { - QRadioButton * mode=qobject_cast(extraWidgets.at(0)); - if(mode->isChecked()) - transmitterMode=mode1; - else - transmitterMode=mode2; - delete extraWidgets.at(0); - delete extraWidgets.at(1); - extraWidgets.clear(); - } setTxMovement(centerAll); if(wizardStep==wizardIdentifySticks) disconnect(receiverActivityObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(identifyControls())); @@ -528,9 +550,41 @@ void ConfigInputWidget::setupWizardWidget(int step) } +/** + * Unfortunately order of channel should be different in different conditions + */ +int ConfigInputWidget::getChannelFromStep(int currentStep) +{ + int channelToSet; + if(transmitterType == heli) { + // For heli swap order of collective to beginning + qDebug() << "Transmitter type: " << heli << " channelToSet: " << currentStep; + if(currentStep == 0) + channelToSet = ManualControlSettings::CHANNELGROUPS_COLLECTIVE; + else if(currentStep <= ManualControlSettings::CHANNELGROUPS_COLLECTIVE) + channelToSet = currentStep - 1; + else channelToSet = currentStep; + qDebug() << "Channel to set: " << channelToSet; + } else + channelToSet = currentStep; + + Q_ASSERT(channelToSet >= 0 && channelToSet < ManualControlSettings::CHANNELGROUPS_NUMELEM); + + if(channelToSet == ManualControlSettings::CHANNELGROUPS_COLLECTIVE) + m_config->wzText->setText(QString(tr("Please enable throttle hold mode and move the collective pitch stick"))); + else + m_config->wzText->setText(QString(tr("Please move each control once at a time according to the instructions and picture below.\n\n" + "Move the %1 stick")).arg(manualSettingsObj->getField("ChannelGroups")->getElementNames().at(channelToSet))); + + return channelToSet; +} + void ConfigInputWidget::identifyControls() { static int debounce=0; + + int channelToSet = getChannelFromStep(currentCommand); + receiverActivityData=receiverActivityObj->getData(); if(receiverActivityData.ActiveChannel==255) return; @@ -548,22 +602,21 @@ void ConfigInputWidget::identifyControls() debounce=0; usedChannels.append(lastChannel); manualSettingsData=manualSettingsObj->getData(); - manualSettingsData.ChannelGroups[currentCommand]=currentChannel.group; - manualSettingsData.ChannelNumber[currentCommand]=currentChannel.number; + manualSettingsData.ChannelGroups[channelToSet]=currentChannel.group; + manualSettingsData.ChannelNumber[channelToSet]=currentChannel.number; manualSettingsObj->setData(manualSettingsData); } else return; } ++currentCommand; - setMoveFromCommand(currentCommand); + channelToSet = getChannelFromStep(currentCommand); + setMoveFromCommand(channelToSet); if(currentCommand>ManualControlSettings::CHANNELGROUPS_NUMELEM-1) { disconnect(receiverActivityObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(identifyControls())); m_config->wzNext->setEnabled(true); } - m_config->wzText->setText(QString(tr("Please move each control once at a time according to the instructions and picture below.\n\n" - "Move the %1 stick")).arg(manualSettingsObj->getFields().at(0)->getElementNames().at(currentCommand))); if(manualSettingsObj->getField("ChannelGroups")->getElementNames().at(currentCommand).contains("Accessory") || manualSettingsObj->getField("ChannelGroups")->getElementNames().at(currentCommand).contains("Collective")) { @@ -607,6 +660,13 @@ void ConfigInputWidget::setMoveFromCommand(int command) else setTxMovement(moveRightVerticalStick); } + else if(command==ManualControlSettings::CHANNELNUMBER_COLLECTIVE) + { + if(transmitterMode==mode2) + setTxMovement(moveLeftVerticalStick); + else + setTxMovement(moveRightVerticalStick); + } else if(command==ManualControlSettings::CHANNELNUMBER_FLIGHTMODE) { setTxMovement(moveFlightMode); diff --git a/ground/openpilotgcs/src/plugins/config/configinputwidget.h b/ground/openpilotgcs/src/plugins/config/configinputwidget.h index 0b95371c5..1bc1701e7 100644 --- a/ground/openpilotgcs/src/plugins/config/configinputwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configinputwidget.h @@ -52,10 +52,11 @@ class ConfigInputWidget: public ConfigTaskWidget public: ConfigInputWidget(QWidget *parent = 0); ~ConfigInputWidget(); - enum wizardSteps{wizardWelcome,wizardChooseMode,wizardIdentifySticks,wizardIdentifyCenter,wizardIdentifyLimits,wizardIdentifyInverted,wizardFinish}; + enum wizardSteps{wizardWelcome,wizardChooseMode,wizardChooseType,wizardIdentifySticks,wizardIdentifyCenter,wizardIdentifyLimits,wizardIdentifyInverted,wizardFinish}; enum txMode{mode1,mode2}; enum txMovements{moveLeftVerticalStick,moveRightVerticalStick,moveLeftHorizontalStick,moveRightHorizontalStick,moveAccess0,moveAccess1,moveAccess2,moveFlightMode,centerAll,moveAll,nothing}; enum txMovementType{vertical,horizontal,jump,mix}; + enum txType {acro, heli}; public slots: private: @@ -68,6 +69,7 @@ private: void setupWizardWidget(int step); QList extraWidgets; txMode transmitterMode; + txType transmitterType; struct channelsStruct { bool operator ==(const channelsStruct& rhs) const @@ -118,6 +120,7 @@ private: void setMoveFromCommand(int command); bool isSimple; void goToWizard(); + int getChannelFromStep(int); private slots: void wzNext(); void wzBack(); diff --git a/ground/openpilotgcs/src/plugins/config/input.ui b/ground/openpilotgcs/src/plugins/config/input.ui index 636ca9385..2fac45ec9 100644 --- a/ground/openpilotgcs/src/plugins/config/input.ui +++ b/ground/openpilotgcs/src/plugins/config/input.ui @@ -27,7 +27,7 @@ - 0 + 1 @@ -79,13 +79,6 @@ 70 - - - 10 - 75 - true - - TextLabel From a5509965b2263968c9fb35837f6d7be8faf82660 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Fri, 9 Sep 2011 00:54:47 -0500 Subject: [PATCH 123/145] Input configuration: add reverse checkbox and label for current neutral value --- .../src/plugins/config/configinputwidget.cpp | 7 +++-- .../src/plugins/config/configinputwidget.h | 1 + .../src/plugins/config/inputchannelform.cpp | 26 ++++++++++++++----- .../src/plugins/config/inputchannelform.h | 4 +-- .../src/plugins/config/inputchannelform.ui | 17 ++++++++++++ 5 files changed, 45 insertions(+), 10 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp index 09ac656de..038c411ed 100644 --- a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp @@ -1044,9 +1044,11 @@ void ConfigInputWidget::updateCalibration() manualCommandData=manualCommandObj->getData(); for(uint i=0;imanualCommandData.Channel[i]) + if((!reverse[i] && manualSettingsData.ChannelMin[i]>manualCommandData.Channel[i]) || + (reverse[i] && manualSettingsData.ChannelMin[i]manualCommandData.Channel[i])) manualSettingsData.ChannelMax[i]=manualCommandData.Channel[i]; manualSettingsData.ChannelNeutral[i] = manualCommandData.Channel[i]; } @@ -1072,6 +1074,7 @@ void ConfigInputWidget::simpleCalibration(bool enable) manualSettingsObj->setData(manualSettingsData); for (unsigned int i = 0; i < ManualControlCommand::CHANNEL_NUMELEM; i++) { + reverse[i] = manualSettingsData.ChannelMax[i] < manualSettingsData.ChannelMin[i]; manualSettingsData.ChannelMin[i] = manualCommandData.Channel[i]; manualSettingsData.ChannelNeutral[i] = manualCommandData.Channel[i]; manualSettingsData.ChannelMax[i] = manualCommandData.Channel[i]; diff --git a/ground/openpilotgcs/src/plugins/config/configinputwidget.h b/ground/openpilotgcs/src/plugins/config/configinputwidget.h index 1bc1701e7..fd6b14001 100644 --- a/ground/openpilotgcs/src/plugins/config/configinputwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configinputwidget.h @@ -61,6 +61,7 @@ public slots: private: bool growing; + bool reverse[ManualControlSettings::CHANNELNEUTRAL_NUMELEM]; txMovements currentMovement; int movePos; void setTxMovement(txMovements movement); diff --git a/ground/openpilotgcs/src/plugins/config/inputchannelform.cpp b/ground/openpilotgcs/src/plugins/config/inputchannelform.cpp index 72c2cfd5e..c30de6d31 100644 --- a/ground/openpilotgcs/src/plugins/config/inputchannelform.cpp +++ b/ground/openpilotgcs/src/plugins/config/inputchannelform.cpp @@ -24,9 +24,10 @@ inputChannelForm::inputChannelForm(QWidget *parent,bool showlegend) : delete ui->legend5; } - connect(ui->channelMin,SIGNAL(valueChanged(int)),this,SLOT(minUpdated(int))); - connect(ui->channelMax,SIGNAL(valueChanged(int)),this,SLOT(maxUpdated(int))); + connect(ui->channelMin,SIGNAL(valueChanged(int)),this,SLOT(minMaxUpdated())); + connect(ui->channelMax,SIGNAL(valueChanged(int)),this,SLOT(minMaxUpdated())); connect(ui->channelGroup,SIGNAL(currentIndexChanged(int)),this,SLOT(groupUpdated())); + connect(ui->channelNeutral,SIGNAL(valueChanged(int)), this, SLOT(neutralUpdated(int))); // This is awkward but since we want the UI to be a dropdown but the field is not an enum // it breaks the UAUVObject widget relation of the task gadget. Running the data through @@ -40,14 +41,27 @@ inputChannelForm::~inputChannelForm() delete ui; } -void inputChannelForm::minUpdated(int newval) +/** + * Update the direction of the slider and boundaries + */ +void inputChannelForm::minMaxUpdated() { - ui->channelNeutral->setMinimum(newval); + bool reverse = ui->channelMin->value() > ui->channelMax->value(); + if(reverse) { + ui->channelNeutral->setMinimum(ui->channelMax->value()); + ui->channelNeutral->setMaximum(ui->channelMin->value()); + } else { + ui->channelNeutral->setMinimum(ui->channelMin->value()); + ui->channelNeutral->setMaximum(ui->channelMax->value()); + } + ui->channelRev->setChecked(reverse); + ui->channelNeutral->setInvertedAppearance(reverse); + ui->channelNeutral->setInvertedControls(reverse); } -void inputChannelForm::maxUpdated(int newval) +void inputChannelForm::neutralUpdated(int newval) { - ui->channelNeutral->setMaximum(newval); + ui->neutral->setText(QString::number(newval)); } /** diff --git a/ground/openpilotgcs/src/plugins/config/inputchannelform.h b/ground/openpilotgcs/src/plugins/config/inputchannelform.h index 2567ba275..eb8ad7d18 100644 --- a/ground/openpilotgcs/src/plugins/config/inputchannelform.h +++ b/ground/openpilotgcs/src/plugins/config/inputchannelform.h @@ -17,8 +17,8 @@ public: friend class ConfigInputWidget; private slots: - void minUpdated(int); - void maxUpdated(int); + void minMaxUpdated(); + void neutralUpdated(int); void groupUpdated(); void channelDropdownUpdated(int); void channelNumberUpdated(int); diff --git a/ground/openpilotgcs/src/plugins/config/inputchannelform.ui b/ground/openpilotgcs/src/plugins/config/inputchannelform.ui index 1e8b53c7c..4ebaa12eb 100644 --- a/ground/openpilotgcs/src/plugins/config/inputchannelform.ui +++ b/ground/openpilotgcs/src/plugins/config/inputchannelform.ui @@ -174,6 +174,23 @@ + + + + false + + + Rev + + + + + + + TextLabel + + + From dfe91af68611fdd5f2467f80a0b3f37b649e8308 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Fri, 9 Sep 2011 01:17:13 -0500 Subject: [PATCH 124/145] UAVObjects: Space allocated in manualcontrolcommand did not match the number of channels in settings. Fixed this and added assertion to catch in future. --- flight/Modules/ManualControl/inc/manualcontrol.h | 7 +++++++ flight/Modules/ManualControl/manualcontrol.c | 2 +- shared/uavobjectdefinition/manualcontrolcommand.xml | 2 +- 3 files changed, 9 insertions(+), 2 deletions(-) diff --git a/flight/Modules/ManualControl/inc/manualcontrol.h b/flight/Modules/ManualControl/inc/manualcontrol.h index 5cca1a623..1215ca27d 100644 --- a/flight/Modules/ManualControl/inc/manualcontrol.h +++ b/flight/Modules/ManualControl/inc/manualcontrol.h @@ -105,4 +105,11 @@ int32_t ManualControlInitialize(); ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD == (int) FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) \ ) +#define assumptions_channelcount ( \ +( (int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM ) && \ +( (int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELNUMBER_NUMELEM ) && \ +( (int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELMIN_NUMELEM ) && \ +( (int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELMAX_NUMELEM ) && \ +( (int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELNEUTRAL_NUMELEM ) ) + #endif // MANUALCONTROL_H diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c index bfbc626f6..e8b18fefb 100644 --- a/flight/Modules/ManualControl/manualcontrol.c +++ b/flight/Modules/ManualControl/manualcontrol.c @@ -101,7 +101,7 @@ static struct rcvr_activity_fsm activity_fsm; static void resetRcvrActivity(struct rcvr_activity_fsm * fsm); static bool updateRcvrActivity(struct rcvr_activity_fsm * fsm); -#define assumptions (assumptions1 && assumptions3 && assumptions5 && assumptions7 && assumptions8 && assumptions_flightmode) +#define assumptions (assumptions1 && assumptions3 && assumptions5 && assumptions7 && assumptions8 && assumptions_flightmode && assumptions_channelcount) /** * Module starting diff --git a/shared/uavobjectdefinition/manualcontrolcommand.xml b/shared/uavobjectdefinition/manualcontrolcommand.xml index 626115233..d8dba78d6 100644 --- a/shared/uavobjectdefinition/manualcontrolcommand.xml +++ b/shared/uavobjectdefinition/manualcontrolcommand.xml @@ -7,7 +7,7 @@ - + From 199db1362d2a3853a9c42efce76c1e6541a650ea Mon Sep 17 00:00:00 2001 From: James Cotton Date: Fri, 9 Sep 2011 19:40:58 -0500 Subject: [PATCH 125/145] Actuator: Found a bug in actuator that affects how throttle and pitch curves were used. Basically 80% was always 100%. This should make hovering better and fix heli swash pitch. --- flight/Modules/Actuator/actuator.c | 31 +++++++++++++++--------------- 1 file changed, 15 insertions(+), 16 deletions(-) diff --git a/flight/Modules/Actuator/actuator.c b/flight/Modules/Actuator/actuator.c index 6b353d20f..b457d07c5 100644 --- a/flight/Modules/Actuator/actuator.c +++ b/flight/Modules/Actuator/actuator.c @@ -74,7 +74,7 @@ static void actuatorTask(void* parameters); static void actuator_update_rate(UAVObjEvent *); static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutral); static void setFailsafe(); -static float MixerCurve(const float throttle, const float* curve); +static float MixerCurve(const float throttle, const float* curve, uint8_t elements); static bool set_channel(uint8_t mixer_channel, uint16_t value); float ProcessMixer(const int index, const float curve1, const float curve2, MixerSettingsData* mixerSettings, ActuatorDesiredData* desired, @@ -219,22 +219,24 @@ static void actuatorTask(void* parameters) bool positiveThrottle = desired.Throttle >= 0.00; bool spinWhileArmed = MotorsSpinWhileArmed == ACTUATORSETTINGS_MOTORSSPINWHILEARMED_TRUE; - float curve1 = MixerCurve(desired.Throttle,mixerSettings.ThrottleCurve1); + float curve1 = MixerCurve(desired.Throttle,mixerSettings.ThrottleCurve1,MIXERSETTINGS_THROTTLECURVE1_NUMELEM); + //The source for the secondary curve is selectable float curve2 = 0; AccessoryDesiredData accessory; switch(mixerSettings.Curve2Source) { case MIXERSETTINGS_CURVE2SOURCE_THROTTLE: - curve2 = MixerCurve(desired.Throttle,mixerSettings.ThrottleCurve2); + curve2 = MixerCurve(desired.Throttle,mixerSettings.ThrottleCurve2,MIXERSETTINGS_THROTTLECURVE2_NUMELEM); break; case MIXERSETTINGS_CURVE2SOURCE_ROLL: - curve2 = MixerCurve(desired.Roll,mixerSettings.ThrottleCurve2); + curve2 = MixerCurve(desired.Roll,mixerSettings.ThrottleCurve2,MIXERSETTINGS_THROTTLECURVE2_NUMELEM); break; case MIXERSETTINGS_CURVE2SOURCE_PITCH: - curve2 = MixerCurve(desired.Pitch,mixerSettings.ThrottleCurve2); + curve2 = MixerCurve(desired.Pitch,mixerSettings.ThrottleCurve2, + MIXERSETTINGS_THROTTLECURVE2_NUMELEM); break; case MIXERSETTINGS_CURVE2SOURCE_YAW: - curve2 = MixerCurve(desired.Yaw,mixerSettings.ThrottleCurve2); + curve2 = MixerCurve(desired.Yaw,mixerSettings.ThrottleCurve2,MIXERSETTINGS_THROTTLECURVE2_NUMELEM); break; case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY0: case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY1: @@ -243,7 +245,7 @@ static void actuatorTask(void* parameters) case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY4: case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY5: if(AccessoryDesiredInstGet(mixerSettings.Curve2Source - MIXERSETTINGS_CURVE2SOURCE_ACCESSORY0,&accessory) == 0) - curve2 = MixerCurve(accessory.AccessoryVal,mixerSettings.ThrottleCurve2); + curve2 = MixerCurve(accessory.AccessoryVal,mixerSettings.ThrottleCurve2,MIXERSETTINGS_THROTTLECURVE2_NUMELEM); else curve2 = 0; break; @@ -423,12 +425,9 @@ float ProcessMixer(const int index, const float curve1, const float curve2, *Interpolate a throttle curve. Throttle input should be in the range 0 to 1. *Output is in the range 0 to 1. */ - -#define MIXER_CURVE_ENTRIES 5 - -static float MixerCurve(const float throttle, const float* curve) +static float MixerCurve(const float throttle, const float* curve, uint8_t elements) { - float scale = throttle * MIXER_CURVE_ENTRIES; + float scale = throttle * (elements - 1); int idx1 = scale; scale -= (float)idx1; //remainder if(curve[0] < -1) @@ -441,12 +440,12 @@ static float MixerCurve(const float throttle, const float* curve) scale = 0; } int idx2 = idx1 + 1; - if(idx2 >= MIXER_CURVE_ENTRIES) + if(idx2 >= elements) { - idx2 = MIXER_CURVE_ENTRIES -1; //clamp to highest entry in table - if(idx1 >= MIXER_CURVE_ENTRIES) + idx2 = elements -1; //clamp to highest entry in table + if(idx1 >= elements) { - idx1 = MIXER_CURVE_ENTRIES -1; + idx1 = elements -1; } } return((curve[idx1] * (1 - scale)) + (curve[idx2] * scale)); From eb191d869a2bffa474d9fc2ccaac6cb19675fe71 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Fri, 9 Sep 2011 09:42:13 -0500 Subject: [PATCH 126/145] Mainboard: Get spektrum working again. --- flight/CopterControl/System/pios_board.c | 1 - flight/OpenPilot/System/inc/pios_config.h | 4 ++-- flight/OpenPilot/System/pios_board.c | 2 ++ flight/PiOS/STM32F10x/link_STM3210E_OP_sections.ld | 4 ++-- 4 files changed, 6 insertions(+), 5 deletions(-) diff --git a/flight/CopterControl/System/pios_board.c b/flight/CopterControl/System/pios_board.c index d9023ffb9..a417ae7b8 100644 --- a/flight/CopterControl/System/pios_board.c +++ b/flight/CopterControl/System/pios_board.c @@ -969,7 +969,6 @@ void PIOS_Board_Init(void) { UAVObjInitialize(); HwSettingsInitialize(); - ManualControlSettingsInitialize(); #if defined(PIOS_INCLUDE_RTC) /* Initialize the real-time clock and its associated tick */ diff --git a/flight/OpenPilot/System/inc/pios_config.h b/flight/OpenPilot/System/inc/pios_config.h index 8e37dc95a..fce461483 100644 --- a/flight/OpenPilot/System/inc/pios_config.h +++ b/flight/OpenPilot/System/inc/pios_config.h @@ -46,8 +46,8 @@ #define PIOS_INCLUDE_SPEKTRUM //#define PIOS_INCLUDE_SBUS -#define PIOS_INCLUDE_PWM -#define PIOS_INCLUDE_PPM +//#define PIOS_INCLUDE_PWM +//#define PIOS_INCLUDE_PPM #define PIOS_INCLUDE_TELEMETRY_RF diff --git a/flight/OpenPilot/System/pios_board.c b/flight/OpenPilot/System/pios_board.c index 22bf12cad..520f542fd 100644 --- a/flight/OpenPilot/System/pios_board.c +++ b/flight/OpenPilot/System/pios_board.c @@ -1099,6 +1099,8 @@ void PIOS_Board_Init(void) { EventDispatcherInitialize(); UAVObjInitialize(); + HwSettingsInitialize(); + #if defined(PIOS_INCLUDE_RTC) /* Initialize the real-time clock and its associated tick */ PIOS_RTC_Init(&pios_rtc_main_cfg); diff --git a/flight/PiOS/STM32F10x/link_STM3210E_OP_sections.ld b/flight/PiOS/STM32F10x/link_STM3210E_OP_sections.ld index 5d504bfdd..066fa01ee 100644 --- a/flight/PiOS/STM32F10x/link_STM3210E_OP_sections.ld +++ b/flight/PiOS/STM32F10x/link_STM3210E_OP_sections.ld @@ -1,7 +1,7 @@ /* This is the size of the stack for early init and for all FreeRTOS IRQs */ -_irq_stack_size = 0x400; +_irq_stack_size = 0x800; /* This is the size of the stack for early init: life span is until scheduler starts */ -_init_stack_size = 0x400; +_init_stack_size = 0x800; /* Check valid alignment for VTOR */ ASSERT(ORIGIN(FLASH) == ALIGN(ORIGIN(FLASH), 0x80), "Start of memory region flash not aligned for startup vector table"); From 4847a04db811ac6ec5396f74d3fdeb3a4e2d2841 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sat, 10 Sep 2011 13:11:21 -0500 Subject: [PATCH 127/145] Mainboard: Get PWM working again. Had to add extra timer IRQ handlers for TIM5-8. Also TIM1 was not handled probably (for CC either) as the name of the IRQ is TIM1_CC and TIM1_UP for the capture compare versus update. I haven't checked the downstream code that the registers it uses to map from a context to event is invariant under timer channel. --- flight/OpenPilot/System/inc/pios_config.h | 4 +- flight/OpenPilot/System/pios_board.c | 2 +- flight/PiOS/STM32F10x/pios_tim.c | 42 +++++++++++++++++-- .../OpenPilotOSX.xcodeproj/project.pbxproj | 22 +++++++++- 4 files changed, 62 insertions(+), 8 deletions(-) diff --git a/flight/OpenPilot/System/inc/pios_config.h b/flight/OpenPilot/System/inc/pios_config.h index fce461483..8e37dc95a 100644 --- a/flight/OpenPilot/System/inc/pios_config.h +++ b/flight/OpenPilot/System/inc/pios_config.h @@ -46,8 +46,8 @@ #define PIOS_INCLUDE_SPEKTRUM //#define PIOS_INCLUDE_SBUS -//#define PIOS_INCLUDE_PWM -//#define PIOS_INCLUDE_PPM +#define PIOS_INCLUDE_PWM +#define PIOS_INCLUDE_PPM #define PIOS_INCLUDE_TELEMETRY_RF diff --git a/flight/OpenPilot/System/pios_board.c b/flight/OpenPilot/System/pios_board.c index 520f542fd..b424cf5e7 100644 --- a/flight/OpenPilot/System/pios_board.c +++ b/flight/OpenPilot/System/pios_board.c @@ -736,7 +736,7 @@ const struct pios_servo_cfg pios_servo_cfg = { /* * PWM Inputs */ -#if defined(PIOS_INCLUDE_PWM) +#if defined(PIOS_INCLUDE_PWM) || defined(PIOS_INCLUDE_PPM) #include static const struct pios_tim_channel pios_tim_rcvrport_all_channels[] = { { diff --git a/flight/PiOS/STM32F10x/pios_tim.c b/flight/PiOS/STM32F10x/pios_tim.c index 345c9e36e..dcdd07670 100644 --- a/flight/PiOS/STM32F10x/pios_tim.c +++ b/flight/PiOS/STM32F10x/pios_tim.c @@ -365,17 +365,23 @@ static void PIOS_TIM_generic_irq_handler(TIM_TypeDef * timer) * Map all valid TIM IRQs to the common interrupt handler * and give it enough context to properly demux the various timers */ -static void PIOS_TIM_1_irq_handler (void) +void TIM1_UP_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_1_UP_irq_handler"))); +static void PIOS_TIM_1_UP_irq_handler (void) { PIOS_TIM_generic_irq_handler (TIM1); } -void TIM1_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_1_irq_handler"))); +void TIM1_CC_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_1_CC_irq_handler"))); +static void PIOS_TIM_1_CC_irq_handler (void) +{ + PIOS_TIM_generic_irq_handler (TIM1); +} + +void TIM2_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_2_irq_handler"))); static void PIOS_TIM_2_irq_handler (void) { PIOS_TIM_generic_irq_handler (TIM2); } -void TIM2_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_2_irq_handler"))); void TIM3_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_3_irq_handler"))); static void PIOS_TIM_3_irq_handler (void) @@ -389,3 +395,33 @@ static void PIOS_TIM_4_irq_handler (void) PIOS_TIM_generic_irq_handler (TIM4); } +void TIM5_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_5_irq_handler"))); +static void PIOS_TIM_5_irq_handler (void) +{ + PIOS_TIM_generic_irq_handler (TIM5); +} + +void TIM6_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_6_irq_handler"))); +static void PIOS_TIM_6_irq_handler (void) +{ + PIOS_TIM_generic_irq_handler (TIM6); +} + +void TIM7_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_7_irq_handler"))); +static void PIOS_TIM_7_irq_handler (void) +{ + PIOS_TIM_generic_irq_handler (TIM7); +} + +void TIM8_UP_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_8_UP_irq_handler"))); +static void PIOS_TIM_8_UP_irq_handler (void) +{ + PIOS_TIM_generic_irq_handler (TIM8); +} + +void TIM8_CC_IRQHandler(void) __attribute__ ((alias ("PIOS_TIM_8_CC_irq_handler"))); +static void PIOS_TIM_8_CC_irq_handler (void) +{ + PIOS_TIM_generic_irq_handler (TIM8); +} + diff --git a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj index a51d5a08d..b860e748c 100644 --- a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj +++ b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj @@ -99,6 +99,8 @@ 65643CB01413322000A32F59 /* pios_spektrum_priv.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_spektrum_priv.h; sourceTree = ""; }; 65643CB91413456D00A32F59 /* pios_tim.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_tim.c; sourceTree = ""; }; 65643CBA141350C200A32F59 /* pios_sbus.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = pios_sbus.c; sourceTree = ""; }; + 65643CEC141429A100A32F59 /* NMEA.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = NMEA.c; sourceTree = ""; }; + 65643CEE141429AF00A32F59 /* NMEA.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = NMEA.h; sourceTree = ""; }; 6572CB1613D0F2B200FC2972 /* link_STM32103CB_CC_Rev1_memory.ld */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; name = link_STM32103CB_CC_Rev1_memory.ld; path = ../../PiOS/STM32F10x/link_STM32103CB_CC_Rev1_memory.ld; sourceTree = SOURCE_ROOT; }; 6572CB1713D0F2B200FC2972 /* link_STM32103CB_CC_Rev1_sections.ld */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text; name = link_STM32103CB_CC_Rev1_sections.ld; path = ../../PiOS/STM32F10x/link_STM32103CB_CC_Rev1_sections.ld; sourceTree = SOURCE_ROOT; }; 657CEEAD121DB6C8007A1FBE /* homelocation.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = homelocation.xml; sourceTree = ""; }; @@ -7766,6 +7768,24 @@ 65E8F05811EFF25C00BBF654 /* STM32F10x */ = { isa = PBXGroup; children = ( + 65D1FBD813F51865006374A6 /* pios_bmp085.c */, + 6560A39D13EE277E00105DA5 /* pios_iap.c */, + 6560A39E13EE277E00105DA5 /* pios_sbus.c */, + 6560A38E13EE270C00105DA5 /* link_STM3210E_INS_BL_sections.ld */, + 6560A38F13EE270C00105DA5 /* link_STM3210E_INS_memory.ld */, + 6560A39013EE270C00105DA5 /* link_STM3210E_INS_sections.ld */, + 6560A39113EE270C00105DA5 /* link_STM3210E_OP_BL_sections.ld */, + 6560A39213EE270C00105DA5 /* link_STM3210E_OP_memory.ld */, + 6560A39313EE270C00105DA5 /* link_STM3210E_OP_sections.ld */, + 6560A39413EE270C00105DA5 /* link_STM32103CB_AHRS_BL_sections.ld */, + 6560A39513EE270C00105DA5 /* link_STM32103CB_AHRS_memory.ld */, + 6560A39613EE270C00105DA5 /* link_STM32103CB_AHRS_sections.ld */, + 6560A39713EE270C00105DA5 /* link_STM32103CB_CC_Rev1_BL_sections.ld */, + 6572CB1613D0F2B200FC2972 /* link_STM32103CB_CC_Rev1_memory.ld */, + 6572CB1713D0F2B200FC2972 /* link_STM32103CB_CC_Rev1_sections.ld */, + 6560A39813EE270C00105DA5 /* link_STM32103CB_PIPXTREME_BL_sections.ld */, + 6560A39913EE270C00105DA5 /* link_STM32103CB_PIPXTREME_memory.ld */, + 6560A39A13EE270C00105DA5 /* link_STM32103CB_PIPXTREME_sections.ld */, 65E8F05911EFF25C00BBF654 /* Libraries */, 65E8F0D811EFF25C00BBF654 /* link_stm32f10x_HD.ld */, 65E8F0DB11EFF25C00BBF654 /* link_stm32f10x_MD.ld */, @@ -7803,8 +7823,6 @@ 65E8F05911EFF25C00BBF654 /* Libraries */ = { isa = PBXGroup; children = ( - 6572CB1613D0F2B200FC2972 /* link_STM32103CB_CC_Rev1_memory.ld */, - 6572CB1713D0F2B200FC2972 /* link_STM32103CB_CC_Rev1_sections.ld */, 65E8F05A11EFF25C00BBF654 /* CMSIS */, 65E8F06B11EFF25C00BBF654 /* dosfs */, 65E8F07111EFF25C00BBF654 /* FreeRTOS */, From 01cd2ded57147f79019bfa70dcdc641a5abfea6f Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sat, 10 Sep 2011 14:19:12 -0500 Subject: [PATCH 128/145] Mainboard config: The rcvr port is used for PPM/PWM/Spektrum so make these one setting that is mutually exclusive. --- flight/CopterControl/System/pios_board.c | 8 +++--- flight/OpenPilot/System/pios_board.c | 28 +++++++------------ .../plugins/config/config_cc_hw_widget.cpp | 2 +- shared/uavobjectdefinition/hwsettings.xml | 5 ++-- 4 files changed, 17 insertions(+), 26 deletions(-) diff --git a/flight/CopterControl/System/pios_board.c b/flight/CopterControl/System/pios_board.c index a417ae7b8..0aafea4fa 100644 --- a/flight/CopterControl/System/pios_board.c +++ b/flight/CopterControl/System/pios_board.c @@ -1163,12 +1163,12 @@ void PIOS_Board_Init(void) { /* Configure the rcvr port */ uint8_t hwsettings_rcvrport; - HwSettingsRcvrPortGet(&hwsettings_rcvrport); + HwSettingsCC_RcvrPortGet(&hwsettings_rcvrport); switch (hwsettings_rcvrport) { - case HWSETTINGS_RCVRPORT_DISABLED: + case HWSETTINGS_CC_RCVRPORT_DISABLED: break; - case HWSETTINGS_RCVRPORT_PWM: + case HWSETTINGS_CC_RCVRPORT_PWM: #if defined(PIOS_INCLUDE_PWM) { uint32_t pios_pwm_id; @@ -1182,7 +1182,7 @@ void PIOS_Board_Init(void) { } #endif /* PIOS_INCLUDE_PWM */ break; - case HWSETTINGS_RCVRPORT_PPM: + case HWSETTINGS_CC_RCVRPORT_PPM: #if defined(PIOS_INCLUDE_PPM) { uint32_t pios_ppm_id; diff --git a/flight/OpenPilot/System/pios_board.c b/flight/OpenPilot/System/pios_board.c index b424cf5e7..d7c43f547 100644 --- a/flight/OpenPilot/System/pios_board.c +++ b/flight/OpenPilot/System/pios_board.c @@ -1193,17 +1193,18 @@ void PIOS_Board_Init(void) { PIOS_ADC_Init(); PIOS_GPIO_Init(); - /* Configure the aux port */ - uint8_t hwsettings_op_auxport; - HwSettingsOP_AuxPortGet(&hwsettings_op_auxport); + /* Configure the rcvr port */ + uint8_t hwsettings_rcvrport; + HwSettingsOP_RcvrPortGet(&hwsettings_rcvrport); - switch (hwsettings_op_auxport) { - case HWSETTINGS_OP_AUXPORT_DISABLED: + + switch (hwsettings_rcvrport) { + case HWSETTINGS_OP_RCVRPORT_DISABLED: break; - case HWSETTINGS_OP_AUXPORT_DEBUG: + case HWSETTINGS_OP_RCVRPORT_DEBUG: /* Not supported yet */ break; - case HWSETTINGS_OP_AUXPORT_SPEKTRUM1: + case HWSETTINGS_OP_RCVRPORT_SPEKTRUM1: #if defined(PIOS_INCLUDE_SPEKTRUM) { uint32_t pios_usart_spektrum_id; @@ -1224,16 +1225,7 @@ void PIOS_Board_Init(void) { } #endif break; - } - - /* Configure the rcvr port */ - uint8_t hwsettings_rcvrport; - HwSettingsRcvrPortGet(&hwsettings_rcvrport); - - switch (hwsettings_rcvrport) { - case HWSETTINGS_RCVRPORT_DISABLED: - break; - case HWSETTINGS_RCVRPORT_PWM: + case HWSETTINGS_OP_RCVRPORT_PWM: #if defined(PIOS_INCLUDE_PWM) { uint32_t pios_pwm_id; @@ -1247,7 +1239,7 @@ void PIOS_Board_Init(void) { } #endif /* PIOS_INCLUDE_PWM */ break; - case HWSETTINGS_RCVRPORT_PPM: + case HWSETTINGS_OP_RCVRPORT_PPM: #if defined(PIOS_INCLUDE_PPM) { uint32_t pios_ppm_id; diff --git a/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.cpp b/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.cpp index 1d98874ba..b6c543698 100644 --- a/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.cpp +++ b/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.cpp @@ -43,7 +43,7 @@ ConfigCCHWWidget::ConfigCCHWWidget(QWidget *parent) : ConfigTaskWidget(parent) addUAVObjectToWidgetRelation("TelemetrySettings","Speed",m_telemetry->telemetrySpeed); addUAVObjectToWidgetRelation("HwSettings","CC_FlexiPort",m_telemetry->cbFlexi); addUAVObjectToWidgetRelation("HwSettings","CC_MainPort",m_telemetry->cbTele); - addUAVObjectToWidgetRelation("HwSettings","RcvrPort",m_telemetry->cbRcvr); + addUAVObjectToWidgetRelation("HwSettings","CC_RcvrPort",m_telemetry->cbRcvr); connect(m_telemetry->cchwHelp,SIGNAL(clicked()),this,SLOT(openHelp())); enableControls(false); populateWidgets(); diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index 242cba679..228d50b5e 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -3,12 +3,11 @@ Selection of optional hardware configurations. + - - - + From ce1c56260d32d663331a6daa9c1837ec92fe2636 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sun, 11 Sep 2011 11:14:09 -0500 Subject: [PATCH 129/145] Input Configuration: Refacfor the code a lot to clear up handling the state and transitions to make back and cancel a bit more reliable. --- .../src/plugins/config/configgadgetwidget.cpp | 1 + .../src/plugins/config/configinputwidget.cpp | 904 ++++++++++++------ .../src/plugins/config/configinputwidget.h | 20 +- 3 files changed, 623 insertions(+), 302 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp index a8509d091..4eca15bf9 100644 --- a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp @@ -169,6 +169,7 @@ void ConfigGadgetWidget::onAutopilotConnect() { void ConfigGadgetWidget::tabAboutToChange(int i,bool * proceed) { + Q_UNUSED(i); *proceed=true; ConfigTaskWidget * wid=qobject_cast(ftw->currentWidget()); if(!wid) diff --git a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp index 038c411ed..6982ef91d 100644 --- a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp @@ -46,9 +46,7 @@ #define STICK_MIN_MOVE -8 #define STICK_MAX_MOVE 8 -QList inputList; - -ConfigInputWidget::ConfigInputWidget(QWidget *parent) : ConfigTaskWidget(parent),wizardStep(wizardWelcome),loop(NULL),skipflag(false),transmitterType(heli) +ConfigInputWidget::ConfigInputWidget(QWidget *parent) : ConfigTaskWidget(parent),wizardStep(wizardNone),loop(NULL),skipflag(false),transmitterType(heli) { manualCommandObj = ManualControlCommand::GetInstance(getObjectManager()); manualSettingsObj = ManualControlSettings::GetInstance(getObjectManager()); @@ -58,9 +56,7 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) : ConfigTaskWidget(parent) setupButtons(m_config->saveRCInputToRAM,m_config->saveRCInputToSD); - inputList.clear(); - - int index=0; + unsigned int index=0; foreach(QString name,manualSettingsObj->getFields().at(0)->getElementNames()) { Q_ASSERT(index < ManualControlSettings::CHANNELGROUPS_NUMELEM); @@ -72,11 +68,10 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) : ConfigTaskWidget(parent) addUAVObjectToWidgetRelation("ManualControlSettings","ChannelMin",inp->ui->channelMin,index); addUAVObjectToWidgetRelation("ManualControlSettings","ChannelNeutral",inp->ui->channelNeutral,index); addUAVObjectToWidgetRelation("ManualControlSettings","ChannelMax",inp->ui->channelMax,index); - inputList.append(inp); ++index; } - connect(m_config->configurationWizard,SIGNAL(clicked()),this,SLOT(goToNormalWizard())); + connect(m_config->configurationWizard,SIGNAL(clicked()),this,SLOT(goToWizard())); connect(m_config->runCalibration,SIGNAL(toggled(bool)),this, SLOT(simpleCalibration(bool))); connect(m_config->wzNext,SIGNAL(clicked()),this,SLOT(wzNext())); @@ -228,6 +223,25 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) : ConfigTaskWidget(parent) m_config->graphicsView->fitInView(m_txMainBody, Qt::KeepAspectRatio ); animate=new QTimer(this); connect(animate,SIGNAL(timeout()),this,SLOT(moveTxControls())); + + heliChannelOrder << ManualControlSettings::CHANNELGROUPS_COLLECTIVE << + ManualControlSettings::CHANNELGROUPS_THROTTLE << + ManualControlSettings::CHANNELGROUPS_ROLL << + ManualControlSettings::CHANNELGROUPS_PITCH << + ManualControlSettings::CHANNELGROUPS_YAW << + ManualControlSettings::CHANNELGROUPS_FLIGHTMODE << + ManualControlSettings::CHANNELGROUPS_ACCESSORY0 << + ManualControlSettings::CHANNELGROUPS_ACCESSORY1 << + ManualControlSettings::CHANNELGROUPS_ACCESSORY2; + + acroChannelOrder << ManualControlSettings::CHANNELGROUPS_THROTTLE << + ManualControlSettings::CHANNELGROUPS_ROLL << + ManualControlSettings::CHANNELGROUPS_PITCH << + ManualControlSettings::CHANNELGROUPS_YAW << + ManualControlSettings::CHANNELGROUPS_FLIGHTMODE << + ManualControlSettings::CHANNELGROUPS_ACCESSORY0 << + ManualControlSettings::CHANNELGROUPS_ACCESSORY1 << + ManualControlSettings::CHANNELGROUPS_ACCESSORY2; } void ConfigInputWidget::resetTxControls() { @@ -258,16 +272,6 @@ void ConfigInputWidget::openHelp() QDesktopServices::openUrl( QUrl("http://wiki.openpilot.org/display/Doc/Input+Configuration", QUrl::StrictMode) ); } -void ConfigInputWidget::goToSimpleWizard() -{ - isSimple=true; - goToWizard(); -} -void ConfigInputWidget::goToNormalWizard() -{ - isSimple=false; - goToWizard(); -} void ConfigInputWidget::goToWizard() { QMessageBox msgBox; @@ -276,7 +280,7 @@ void ConfigInputWidget::goToWizard() msgBox.setStandardButtons(QMessageBox::Ok); msgBox.setDefaultButton(QMessageBox::Ok); msgBox.exec(); - setupWizardWidget(wizardWelcome); + wizardSetUpStep(wizardWelcome); m_config->graphicsView->fitInView(m_txBackground, Qt::KeepAspectRatio ); } @@ -285,250 +289,51 @@ void ConfigInputWidget::wzCancel() dimOtherControls(false); manualCommandObj->setMetadata(manualCommandObj->getDefaultMetadata()); m_config->stackedWidget->setCurrentIndex(0); - foreach (QWidget * wd, extraWidgets) - { - if(wd) - delete wd; - } - extraWidgets.clear(); - switch(wizardStep) - { - case wizardWelcome: - break; - case wizardChooseMode: - case wizardChooseType: - break; - case wizardIdentifySticks: - disconnect(receiverActivityObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(identifyControls())); - break; - case wizardIdentifyCenter: - break; - case wizardIdentifyLimits: - disconnect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(identifyLimits())); - manualSettingsObj->setData(manualSettingsData); - break; - case wizardIdentifyInverted: - break; - case wizardFinish: - break; - default: - break; - } - wizardStep=wizardWelcome; + + if(wizardStep != wizardNone) + wizardTearDownStep(wizardStep); + wizardStep=wizardNone; + + // Load settings back from beginning of wizard + manualSettingsObj->setData(previousManualSettingsData); } void ConfigInputWidget::wzNext() { - setupWizardWidget(wizardStep+1); -} + // In identify sticks mode the next button can indicate + // channel advance + if(wizardStep != wizardNone && + wizardStep != wizardIdentifySticks) + wizardTearDownStep(wizardStep); -void ConfigInputWidget::wzBack() -{ - if(wizardStep == wizardIdentifySticks && currentCommand != 0) { - currentCommand --; - usedChannels.removeLast(); - getChannelFromStep(currentCommand); - } else - setupWizardWidget(wizardStep-1); -} - -void ConfigInputWidget::setupWizardWidget(int step) -{ - if(step==wizardWelcome) - { - m_config->graphicsView->setVisible(false); - setTxMovement(nothing); - if(wizardStep==wizardChooseMode) - { - delete extraWidgets.at(0); - delete extraWidgets.at(1); - extraWidgets.clear(); + // State transitions for next button + switch(wizardStep) { + case wizardWelcome: + wizardSetUpStep(wizardChooseMode); + break; + case wizardChooseMode: + wizardSetUpStep(wizardChooseType); + break; + case wizardChooseType: + wizardSetUpStep(wizardIdentifySticks); + break; + case wizardIdentifySticks: + nextChannel(); + if(currentChannelNum==-1) { // Gone through all channels + wizardTearDownStep(wizardIdentifySticks); + wizardSetUpStep(wizardIdentifyCenter); } - manualSettingsData=manualSettingsObj->getData(); - manualSettingsData.Arming=ManualControlSettings::ARMING_ALWAYSDISARMED; - manualSettingsObj->setData(manualSettingsData); - m_config->wzText->setText(tr("Welcome to the inputs configuration wizard.\n" - "Please follow the instructions on the screen and only move your controls when asked to.\n" - "Make sure you already configured your hardware settings on the proper tab and restarted your board.\n" - "At any time you can press 'back' to return to the previous screeen or 'Cancel' to cancel the wizard.\n")); - m_config->stackedWidget->setCurrentIndex(1); - m_config->wzBack->setEnabled(false); - wizardStep=wizardWelcome; - } - else if(step==wizardChooseMode) - { - m_config->graphicsView->setVisible(true); - setTxMovement(nothing); - if(wizardStep==wizardIdentifySticks) - { - disconnect(receiverActivityObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(identifyControls())); - m_config->wzNext->setEnabled(true); - } - m_config->wzText->setText(tr("Please choose your transmiter type.\n" - "Mode 1 means your throttle stick is on the right\n" - "Mode 2 means your throttle stick is on the left\n")); - m_config->wzBack->setEnabled(true); - QRadioButton * mode1=new QRadioButton(tr("Mode 1"),this); - QRadioButton * mode2=new QRadioButton(tr("Mode 2"),this); - mode2->setChecked(true); - extraWidgets.clear(); - extraWidgets.append(mode1); - extraWidgets.append(mode2); - m_config->checkBoxesLayout->layout()->addWidget(mode1); - m_config->checkBoxesLayout->layout()->addWidget(mode2); - wizardStep=wizardChooseMode; - } - else if(step==wizardChooseType) - { - if(wizardStep==wizardChooseMode) - { - QRadioButton * mode=qobject_cast(extraWidgets.at(0)); - if(mode->isChecked()) - transmitterMode=mode1; - else - transmitterMode=mode2; - delete extraWidgets.at(0); - delete extraWidgets.at(1); - extraWidgets.clear(); - } - - m_config->wzText->setText(tr("Please choose your transmiter mode.\n" - "Acro means normal transmitter\n" - "Heli means there is a collective pitch and throttle input\n")); - m_config->wzBack->setEnabled(true); - QRadioButton * typeAcro=new QRadioButton(tr("Acro"),this); - QRadioButton * typeHeli=new QRadioButton(tr("Heli"),this); - typeAcro->setChecked(true); - typeHeli->setChecked(false); - extraWidgets.clear(); - extraWidgets.append(typeAcro); - extraWidgets.append(typeHeli); - m_config->checkBoxesLayout->layout()->addWidget(typeAcro); - m_config->checkBoxesLayout->layout()->addWidget(typeHeli); - wizardStep=wizardChooseType; - } else if(step==wizardIdentifySticks && !isSimple) { - usedChannels.clear(); - if(wizardStep==wizardChooseType) - { - qDebug() << "Chosing type"; - QRadioButton * type=qobject_cast(extraWidgets.at(0)); - if(type->isChecked()) - transmitterType=acro; - else - transmitterType=heli; - qDebug() << "Checked: " << type->isChecked() << " " << "type is" << transmitterType; - delete extraWidgets.at(0); - delete extraWidgets.at(1); - extraWidgets.clear(); - } - wizardStep=wizardIdentifySticks; - currentCommand=0; - getChannelFromStep(currentCommand); - manualSettingsData=manualSettingsObj->getData(); - connect(receiverActivityObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(identifyControls())); - m_config->wzNext->setEnabled(false); - } - else if(step==wizardIdentifyCenter || (isSimple && step==wizardIdentifySticks)) - { - setTxMovement(centerAll); - if(wizardStep==wizardIdentifySticks) - disconnect(receiverActivityObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(identifyControls())); - else - { - disconnect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(identifyLimits())); - manualSettingsObj->setData(manualSettingsData); - manualCommandObj->setMetadata(manualCommandObj->getDefaultMetadata()); - } - wizardStep=wizardIdentifyCenter; - m_config->wzText->setText(QString(tr("Please center all control controls and press next when ready (if your FlightMode switch has only two positions, leave it on either position)"))); - } - else if(step==wizardIdentifyLimits) - { - dimOtherControls(false); - setTxMovement(moveAll); - if(wizardStep==wizardIdentifyCenter) - { - wizardStep=wizardIdentifyLimits; - manualCommandData=manualCommandObj->getData(); - manualSettingsData=manualSettingsObj->getData(); - for(unsigned int i=0;isetData(manualSettingsData); - } - if(wizardStep==wizardIdentifyInverted) - { - foreach(QWidget * wd,extraWidgets) - { - QCheckBox * cb=qobject_cast(wd); - if(cb) - { - disconnect(cb,SIGNAL(toggled(bool)),this,SLOT(invertControls())); - delete cb; - } - } - } - extraWidgets.clear(); - disconnect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); - wizardStep=wizardIdentifyLimits; - m_config->wzText->setText(QString(tr("Please move all controls to their maximum extents on both directions and press next when ready"))); - UAVObject::Metadata mdata= manualCommandObj->getMetadata(); - mdata.flightTelemetryUpdateMode = UAVObject::UPDATEMODE_PERIODIC; - mdata.flightTelemetryUpdatePeriod = 150; - manualCommandObj->setMetadata(mdata); - manualSettingsData=manualSettingsObj->getData(); - for(uint i=0;isetData(manualSettingsData); - } - extraWidgets.clear(); - foreach(QString name,manualSettingsObj->getFields().at(0)->getElementNames()) - { - if(!name.contains("Access") && !name.contains("Flight")) - { - QCheckBox * cb=new QCheckBox(name,this); - extraWidgets.append(cb); - m_config->checkBoxesLayout->layout()->addWidget(cb); - connect(cb,SIGNAL(toggled(bool)),this,SLOT(invertControls())); - } - } - connect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); - wizardStep=wizardIdentifyInverted; - m_config->wzText->setText(QString(tr("Please check the picture below and check all the sticks which show an inverted movement and press next when ready"))); - } - else if(step==wizardFinish) - { - foreach(QWidget * wd,extraWidgets) - { - QCheckBox * cb=qobject_cast(wd); - if(cb) - { - disconnect(cb,SIGNAL(toggled(bool)),this,SLOT(invertControls())); - delete cb; - } - } - wizardStep=wizardFinish; - extraWidgets.clear(); - m_config->wzText->setText(QString(tr("You have completed this wizard, please check below if the picture below mimics your sticks movement.\n" - "This new settings aren't saved to the board yet, after pressing next you will go to the initial screen where you can do that."))); - - } - - else if(step==wizardFinish+1) - { + break; + case wizardIdentifyCenter: + wizardSetUpStep(wizardIdentifyLimits); + break; + case wizardIdentifyLimits: + wizardSetUpStep(wizardIdentifyInverted); + break; + case wizardIdentifyInverted: + wizardSetUpStep(wizardFinish); + break; + case wizardFinish: setTxMovement(nothing); manualCommandObj->setMetadata(manualCommandObj->getDefaultMetadata()); disconnect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); @@ -545,46 +350,560 @@ void ConfigInputWidget::setupWizardWidget(int step) } manualSettingsObj->setData(manualSettingsData); m_config->stackedWidget->setCurrentIndex(0); - wizardStep=wizardWelcome; + wizardStep=wizardNone; + break; + default: + Q_ASSERT(0); } +} +void ConfigInputWidget::wzBack() +{ + if(wizardStep != wizardNone && + wizardStep != wizardIdentifySticks) + wizardTearDownStep(wizardStep); + + // State transitions for next button + switch(wizardStep) { + case wizardChooseMode: + wizardSetUpStep(wizardWelcome); + break; + case wizardChooseType: + wizardSetUpStep(wizardChooseMode); + break; + case wizardIdentifySticks: + prevChannel(); + if(currentChannelNum == -1) { + wizardTearDownStep(wizardIdentifySticks); + wizardSetUpStep(wizardChooseType); + } + break; + case wizardIdentifyCenter: + wizardSetUpStep(wizardIdentifySticks); + break; + case wizardIdentifyLimits: + wizardSetUpStep(wizardIdentifyCenter); + break; + case wizardIdentifyInverted: + wizardSetUpStep(wizardIdentifyLimits); + break; + case wizardFinish: + wizardSetUpStep(wizardIdentifyInverted); + break; + default: + Q_ASSERT(0); + } +} + +void ConfigInputWidget::wizardSetUpStep(enum wizardSteps step) +{ + switch(step) { + case wizardWelcome: + m_config->graphicsView->setVisible(false); + setTxMovement(nothing); + manualSettingsData=manualSettingsObj->getData(); + manualSettingsData.Arming=ManualControlSettings::ARMING_ALWAYSDISARMED; + previousManualSettingsData = manualSettingsData; + manualSettingsObj->setData(manualSettingsData); + m_config->wzText->setText(tr("Welcome to the inputs configuration wizard.\n" + "Please follow the instructions on the screen and only move your controls when asked to.\n" + "Make sure you already configured your hardware settings on the proper tab and restarted your board.\n" + "At any time you can press 'back' to return to the previous screeen or 'Cancel' to cancel the wizard.\n")); + m_config->stackedWidget->setCurrentIndex(1); + m_config->wzBack->setEnabled(false); + break; + case wizardChooseMode: + { + m_config->graphicsView->setVisible(true); + setTxMovement(nothing); + m_config->wzText->setText(tr("Please choose your transmiter type.\n" + "Mode 1 means your throttle stick is on the right\n" + "Mode 2 means your throttle stick is on the left\n")); + m_config->wzBack->setEnabled(true); + QRadioButton * mode1=new QRadioButton(tr("Mode 1"),this); + QRadioButton * mode2=new QRadioButton(tr("Mode 2"),this); + mode2->setChecked(true); + extraWidgets.clear(); + extraWidgets.append(mode1); + extraWidgets.append(mode2); + m_config->checkBoxesLayout->layout()->addWidget(mode1); + m_config->checkBoxesLayout->layout()->addWidget(mode2); + } + break; + case wizardChooseType: + { + m_config->wzText->setText(tr("Please choose your transmiter mode.\n" + "Acro means normal transmitter\n" + "Heli means there is a collective pitch and throttle input\n" + "If you are using a heli transmitter please engage throttle hold now please.\n")); + m_config->wzBack->setEnabled(true); + QRadioButton * typeAcro=new QRadioButton(tr("Acro"),this); + QRadioButton * typeHeli=new QRadioButton(tr("Heli"),this); + typeAcro->setChecked(true); + typeHeli->setChecked(false); + extraWidgets.clear(); + extraWidgets.append(typeAcro); + extraWidgets.append(typeHeli); + m_config->checkBoxesLayout->layout()->addWidget(typeAcro); + m_config->checkBoxesLayout->layout()->addWidget(typeHeli); + wizardStep=wizardChooseType; + } + break; + case wizardIdentifySticks: + usedChannels.clear(); + currentChannelNum=-1; + nextChannel(); + manualSettingsData=manualSettingsObj->getData(); + connect(receiverActivityObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(identifyControls())); + m_config->wzNext->setEnabled(false); + break; + case wizardIdentifyCenter: + setTxMovement(centerAll); + m_config->wzText->setText(QString(tr("Please center all control controls and press next when ready (if your FlightMode switch has only two positions, leave it on either position)"))); + break; + case wizardIdentifyLimits: + { + dimOtherControls(false); + setTxMovement(moveAll); + m_config->wzText->setText(QString(tr("Please move all controls to their maximum extents on both directions and press next when ready"))); + fastMdata(); + manualSettingsData=manualSettingsObj->getData(); + for(uint i=0;igetFields().at(0)->getElementNames()) + { + if(!name.contains("Access") && !name.contains("Flight")) + { + QCheckBox * cb=new QCheckBox(name,this); + extraWidgets.append(cb); + m_config->checkBoxesLayout->layout()->addWidget(cb); + connect(cb,SIGNAL(toggled(bool)),this,SLOT(invertControls())); + } + } + connect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); + m_config->wzText->setText(QString(tr("Please check the picture below and check all the sticks which show an inverted movement and press next when ready"))); + fastMdata(); + break; + case wizardFinish: + foreach(QWidget * wd,extraWidgets) + { + QCheckBox * cb=qobject_cast(wd); + if(cb) + { + disconnect(cb,SIGNAL(toggled(bool)),this,SLOT(invertControls())); + delete cb; + } + } + extraWidgets.clear(); + connect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); + m_config->wzText->setText(QString(tr("You have completed this wizard, please check below if the picture below mimics your sticks movement.\n" + "This new settings aren't saved to the board yet, after pressing next you will go to the initial screen where you can do that."))); + fastMdata(); + + manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_THROTTLE]= + manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_THROTTLE]+ + ((manualSettingsData.ChannelMax[ManualControlSettings::CHANNELMAX_THROTTLE]- + manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_THROTTLE])*0.02); + if((abs(manualSettingsData.ChannelMax[ManualControlSettings::CHANNELMAX_FLIGHTMODE]-manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_FLIGHTMODE])<100) || + (abs(manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_FLIGHTMODE]-manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_FLIGHTMODE])<100)) + { + manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_FLIGHTMODE]=manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_FLIGHTMODE]+ + (manualSettingsData.ChannelMax[ManualControlSettings::CHANNELMAX_FLIGHTMODE]-manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_FLIGHTMODE])/2; + } + manualSettingsObj->setData(manualSettingsData); + break; + default: + Q_ASSERT(0); + } + wizardStep = step; +} + +void ConfigInputWidget::wizardTearDownStep(enum wizardSteps step) +{ + QRadioButton * mode, * type; + Q_ASSERT(step == wizardStep); + switch(step) { + case wizardWelcome: + break; + case wizardChooseMode: + mode=qobject_cast(extraWidgets.at(0)); + if(mode->isChecked()) + transmitterMode=mode1; + else + transmitterMode=mode2; + delete extraWidgets.at(0); + delete extraWidgets.at(1); + extraWidgets.clear(); + break; + case wizardChooseType: + type=qobject_cast(extraWidgets.at(0)); + if(type->isChecked()) + transmitterType=acro; + else + transmitterType=heli; + delete extraWidgets.at(0); + delete extraWidgets.at(1); + extraWidgets.clear(); + break; + case wizardIdentifySticks: + disconnect(receiverActivityObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(identifyControls())); + m_config->wzNext->setEnabled(true); + break; + case wizardIdentifyCenter: + manualCommandData=manualCommandObj->getData(); + manualSettingsData=manualSettingsObj->getData(); + for(unsigned int i=0;isetData(manualSettingsData); + break; + case wizardIdentifyLimits: + disconnect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(identifyLimits())); + manualSettingsObj->setData(manualSettingsData); + restoreMdata(); + break; + case wizardIdentifyInverted: + foreach(QWidget * wd,extraWidgets) + { + QCheckBox * cb=qobject_cast(wd); + if(cb) + { + disconnect(cb,SIGNAL(toggled(bool)),this,SLOT(invertControls())); + delete cb; + } + } + extraWidgets.clear(); + disconnect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); + restoreMdata(); + break; + case wizardFinish: + setTxMovement(nothing); + m_config->stackedWidget->setCurrentIndex(0); + disconnect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); + restoreMdata(); + break; + default: + Q_ASSERT(0); + } } /** - * Unfortunately order of channel should be different in different conditions + * Set manual control command to fast updates */ -int ConfigInputWidget::getChannelFromStep(int currentStep) +void ConfigInputWidget::fastMdata() { - int channelToSet; - if(transmitterType == heli) { - // For heli swap order of collective to beginning - qDebug() << "Transmitter type: " << heli << " channelToSet: " << currentStep; - if(currentStep == 0) - channelToSet = ManualControlSettings::CHANNELGROUPS_COLLECTIVE; - else if(currentStep <= ManualControlSettings::CHANNELGROUPS_COLLECTIVE) - channelToSet = currentStep - 1; - else channelToSet = currentStep; - qDebug() << "Channel to set: " << channelToSet; - } else - channelToSet = currentStep; + manualControlMdata = manualCommandObj->getMetadata(); + UAVObject::Metadata mdata = manualControlMdata; + mdata.flightTelemetryUpdateMode = UAVObject::UPDATEMODE_PERIODIC; + mdata.flightTelemetryUpdatePeriod = 150; + manualCommandObj->setMetadata(mdata); +} - Q_ASSERT(channelToSet >= 0 && channelToSet < ManualControlSettings::CHANNELGROUPS_NUMELEM); +/** + * Restore previous update settings for manual control data + */ +void ConfigInputWidget::restoreMdata() +{ + manualCommandObj->setMetadata(manualControlMdata); +} - if(channelToSet == ManualControlSettings::CHANNELGROUPS_COLLECTIVE) +//void ConfigInputWidget::setupWizardWidget(int step) +//{ +// if(step==wizardWelcome) +// { +// m_config->graphicsView->setVisible(false); +// setTxMovement(nothing); +// if(wizardStep==wizardChooseMode) +// { +// delete extraWidgets.at(0); +// delete extraWidgets.at(1); +// extraWidgets.clear(); +// } +// manualSettingsData=manualSettingsObj->getData(); +// manualSettingsData.Arming=ManualControlSettings::ARMING_ALWAYSDISARMED; +// manualSettingsObj->setData(manualSettingsData); +// m_config->wzText->setText(tr("Welcome to the inputs configuration wizard.\n" +// "Please follow the instructions on the screen and only move your controls when asked to.\n" +// "Make sure you already configured your hardware settings on the proper tab and restarted your board.\n" +// "At any time you can press 'back' to return to the previous screeen or 'Cancel' to cancel the wizard.\n")); +// m_config->stackedWidget->setCurrentIndex(1); +// m_config->wzBack->setEnabled(false); +// wizardStep=wizardWelcome; +// } +// else if(step==wizardChooseMode) +// { +// m_config->graphicsView->setVisible(true); +// setTxMovement(nothing); +// if(wizardStep==wizardIdentifySticks) +// { +// disconnect(receiverActivityObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(identifyControls())); +// m_config->wzNext->setEnabled(true); +// } +// m_config->wzText->setText(tr("Please choose your transmiter type.\n" +// "Mode 1 means your throttle stick is on the right\n" +// "Mode 2 means your throttle stick is on the left\n")); +// m_config->wzBack->setEnabled(true); +// QRadioButton * mode1=new QRadioButton(tr("Mode 1"),this); +// QRadioButton * mode2=new QRadioButton(tr("Mode 2"),this); +// mode2->setChecked(true); +// extraWidgets.clear(); +// extraWidgets.append(mode1); +// extraWidgets.append(mode2); +// m_config->checkBoxesLayout->layout()->addWidget(mode1); +// m_config->checkBoxesLayout->layout()->addWidget(mode2); +// wizardStep=wizardChooseMode; +// } +// else if(step==wizardChooseType) +// { +// if(wizardStep==wizardChooseMode) +// { +// QRadioButton * mode=qobject_cast(extraWidgets.at(0)); +// if(mode->isChecked()) +// transmitterMode=mode1; +// else +// transmitterMode=mode2; +// delete extraWidgets.at(0); +// delete extraWidgets.at(1); +// extraWidgets.clear(); +// } + +// m_config->wzText->setText(tr("Please choose your transmiter mode.\n" +// "Acro means normal transmitter\n" +// "Heli means there is a collective pitch and throttle input\n")); +// m_config->wzBack->setEnabled(true); +// QRadioButton * typeAcro=new QRadioButton(tr("Acro"),this); +// QRadioButton * typeHeli=new QRadioButton(tr("Heli"),this); +// typeAcro->setChecked(true); +// typeHeli->setChecked(false); +// extraWidgets.clear(); +// extraWidgets.append(typeAcro); +// extraWidgets.append(typeHeli); +// m_config->checkBoxesLayout->layout()->addWidget(typeAcro); +// m_config->checkBoxesLayout->layout()->addWidget(typeHeli); +// wizardStep=wizardChooseType; +// } else if(step==wizardIdentifySticks && !isSimple) { +// usedChannels.clear(); +// if(wizardStep==wizardChooseType) +// { +// qDebug() << "Chosing type"; +// QRadioButton * type=qobject_cast(extraWidgets.at(0)); +// if(type->isChecked()) +// transmitterType=acro; +// else +// transmitterType=heli; +// qDebug() << "Checked: " << type->isChecked() << " " << "type is" << transmitterType; +// delete extraWidgets.at(0); +// delete extraWidgets.at(1); +// extraWidgets.clear(); +// } +// wizardStep=wizardIdentifySticks; +// currentCommand=0; +// getChannelFromStep(currentCommand); +// manualSettingsData=manualSettingsObj->getData(); +// connect(receiverActivityObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(identifyControls())); +// m_config->wzNext->setEnabled(false); +// } +// else if(step==wizardIdentifyCenter || (isSimple && step==wizardIdentifySticks)) +// { +// setTxMovement(centerAll); +// if(wizardStep==wizardIdentifySticks) +// disconnect(receiverActivityObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(identifyControls())); +// else +// { +// disconnect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(identifyLimits())); +// manualSettingsObj->setData(manualSettingsData); +// manualCommandObj->setMetadata(manualCommandObj->getDefaultMetadata()); +// } +// wizardStep=wizardIdentifyCenter; +// m_config->wzText->setText(QString(tr("Please center all control controls and press next when ready (if your FlightMode switch has only two positions, leave it on either position)"))); +// } +// else if(step==wizardIdentifyLimits) +// { +// dimOtherControls(false); +// setTxMovement(moveAll); +// if(wizardStep==wizardIdentifyCenter) +// { +// wizardStep=wizardIdentifyLimits; +// manualCommandData=manualCommandObj->getData(); +// manualSettingsData=manualSettingsObj->getData(); +// for(unsigned int i=0;isetData(manualSettingsData); +// } +// if(wizardStep==wizardIdentifyInverted) +// { +// foreach(QWidget * wd,extraWidgets) +// { +// QCheckBox * cb=qobject_cast(wd); +// if(cb) +// { +// disconnect(cb,SIGNAL(toggled(bool)),this,SLOT(invertControls())); +// delete cb; +// } +// } +// } +// extraWidgets.clear(); +// disconnect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); +// wizardStep=wizardIdentifyLimits; +// m_config->wzText->setText(QString(tr("Please move all controls to their maximum extents on both directions and press next when ready"))); +// UAVObject::Metadata mdata= manualCommandObj->getMetadata(); +// mdata.flightTelemetryUpdateMode = UAVObject::UPDATEMODE_PERIODIC; +// mdata.flightTelemetryUpdatePeriod = 150; +// manualCommandObj->setMetadata(mdata); +// manualSettingsData=manualSettingsObj->getData(); +// for(uint i=0;isetData(manualSettingsData); +// } +// extraWidgets.clear(); +// foreach(QString name,manualSettingsObj->getFields().at(0)->getElementNames()) +// { +// if(!name.contains("Access") && !name.contains("Flight")) +// { +// QCheckBox * cb=new QCheckBox(name,this); +// extraWidgets.append(cb); +// m_config->checkBoxesLayout->layout()->addWidget(cb); +// connect(cb,SIGNAL(toggled(bool)),this,SLOT(invertControls())); +// } +// } +// connect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); +// wizardStep=wizardIdentifyInverted; +// m_config->wzText->setText(QString(tr("Please check the picture below and check all the sticks which show an inverted movement and press next when ready"))); +// } +// else if(step==wizardFinish) +// { +// foreach(QWidget * wd,extraWidgets) +// { +// QCheckBox * cb=qobject_cast(wd); +// if(cb) +// { +// disconnect(cb,SIGNAL(toggled(bool)),this,SLOT(invertControls())); +// delete cb; +// } +// } +// wizardStep=wizardFinish; +// extraWidgets.clear(); +// m_config->wzText->setText(QString(tr("You have completed this wizard, please check below if the picture below mimics your sticks movement.\n" +// "This new settings aren't saved to the board yet, after pressing next you will go to the initial screen where you can do that."))); + +// } + +// else if(step==wizardFinish+1) +// { +// setTxMovement(nothing); +// manualCommandObj->setMetadata(manualCommandObj->getDefaultMetadata()); +// disconnect(manualCommandObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(moveSticks())); +// manualSettingsData=manualSettingsObj->getData(); +// manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_THROTTLE]= +// manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_THROTTLE]+ +// ((manualSettingsData.ChannelMax[ManualControlSettings::CHANNELMAX_THROTTLE]- +// manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_THROTTLE])*0.02); +// if((abs(manualSettingsData.ChannelMax[ManualControlSettings::CHANNELMAX_FLIGHTMODE]-manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_FLIGHTMODE])<100) || +// (abs(manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_FLIGHTMODE]-manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_FLIGHTMODE])<100)) +// { +// manualSettingsData.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_FLIGHTMODE]=manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_FLIGHTMODE]+ +// (manualSettingsData.ChannelMax[ManualControlSettings::CHANNELMAX_FLIGHTMODE]-manualSettingsData.ChannelMin[ManualControlSettings::CHANNELMIN_FLIGHTMODE])/2; +// } +// manualSettingsObj->setData(manualSettingsData); +// m_config->stackedWidget->setCurrentIndex(0); +// wizardStep=wizardWelcome; +// } + +//} + +/** + * Set the display to indicate which channel the person should move + */ +void ConfigInputWidget::setChannel(int newChan) +{ + if(newChan == ManualControlSettings::CHANNELGROUPS_COLLECTIVE) m_config->wzText->setText(QString(tr("Please enable throttle hold mode and move the collective pitch stick"))); + else if (newChan == ManualControlSettings::CHANNELGROUPS_FLIGHTMODE) + m_config->wzText->setText(QString(tr("Please flick the flight mode switch. For switches you may have to repeat this rapidly."))); else m_config->wzText->setText(QString(tr("Please move each control once at a time according to the instructions and picture below.\n\n" - "Move the %1 stick")).arg(manualSettingsObj->getField("ChannelGroups")->getElementNames().at(channelToSet))); + "Move the %1 stick")).arg(manualSettingsObj->getField("ChannelGroups")->getElementNames().at(newChan))); - return channelToSet; + if(manualSettingsObj->getField("ChannelGroups")->getElementNames().at(newChan).contains("Accessory")) + m_config->wzNext->setEnabled(true); + + setMoveFromCommand(newChan); + + currentChannelNum = newChan; +} + +/** + * Unfortunately order of channel should be different in different conditions. Selects + * next channel based on heli or acro mode + */ +void ConfigInputWidget::nextChannel() +{ + QList order = (transmitterType == heli) ? heliChannelOrder : acroChannelOrder; + + if(currentChannelNum == -1) { + setChannel(order[0]); + return; + } + for (int i = 0; i < order.length() - 1; i++) { + if(order[i] == currentChannelNum) { + setChannel(order[i+1]); + return; + } + } + currentChannelNum = -1; // hit end of list +} + +/** + * Unfortunately order of channel should be different in different conditions. Selects + * previous channel based on heli or acro mode + */ +void ConfigInputWidget::prevChannel() +{ + QList order = transmitterType == heli ? heliChannelOrder : acroChannelOrder; + + // No previous from unset channel or next state + if(currentChannelNum == -1) + return; + + for (int i = 1; i < order.length(); i++) { + if(order[i] == currentChannelNum) { + setChannel(order[i-1]); + usedChannels.removeLast(); + return; + } + } + currentChannelNum = -1; // hit end of list } void ConfigInputWidget::identifyControls() { static int debounce=0; - int channelToSet = getChannelFromStep(currentCommand); - receiverActivityData=receiverActivityObj->getData(); if(receiverActivityData.ActiveChannel==255) return; @@ -602,26 +921,18 @@ void ConfigInputWidget::identifyControls() debounce=0; usedChannels.append(lastChannel); manualSettingsData=manualSettingsObj->getData(); - manualSettingsData.ChannelGroups[channelToSet]=currentChannel.group; - manualSettingsData.ChannelNumber[channelToSet]=currentChannel.number; + manualSettingsData.ChannelGroups[currentChannelNum]=currentChannel.group; + manualSettingsData.ChannelNumber[currentChannelNum]=currentChannel.number; manualSettingsObj->setData(manualSettingsData); } else return; } - ++currentCommand; - channelToSet = getChannelFromStep(currentCommand); - setMoveFromCommand(channelToSet); - if(currentCommand>ManualControlSettings::CHANNELGROUPS_NUMELEM-1) - { - disconnect(receiverActivityObj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(identifyControls())); - m_config->wzNext->setEnabled(true); - } - if(manualSettingsObj->getField("ChannelGroups")->getElementNames().at(currentCommand).contains("Accessory") || - manualSettingsObj->getField("ChannelGroups")->getElementNames().at(currentCommand).contains("Collective")) - { - m_config->wzNext->setEnabled(true); - } + + m_config->wzText->clear(); + setTxMovement(nothing); + + QTimer::singleShot(300, this, SLOT(wzNext())); } void ConfigInputWidget::identifyLimits() @@ -1080,17 +1391,14 @@ void ConfigInputWidget::simpleCalibration(bool enable) manualSettingsData.ChannelMax[i] = manualCommandData.Channel[i]; } - UAVObject::Metadata mdata= manualCommandObj->getMetadata(); - mdata.flightTelemetryUpdateMode = UAVObject::UPDATEMODE_PERIODIC; - mdata.flightTelemetryUpdatePeriod = 150; - manualCommandObj->setMetadata(mdata); + fastMdata(); connect(manualCommandObj, SIGNAL(objectUnpacked(UAVObject*)), this, SLOT(updateCalibration())); } else { manualCommandData = manualCommandObj->getData(); manualSettingsData = manualSettingsObj->getData(); - manualCommandObj->setMetadata(manualCommandObj->getDefaultMetadata()); + restoreMdata(); for (int i = 0; i < ManualControlCommand::CHANNEL_NUMELEM; i++) manualSettingsData.ChannelNeutral[i] = manualCommandData.Channel[i]; diff --git a/ground/openpilotgcs/src/plugins/config/configinputwidget.h b/ground/openpilotgcs/src/plugins/config/configinputwidget.h index fd6b14001..b8dc664ea 100644 --- a/ground/openpilotgcs/src/plugins/config/configinputwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configinputwidget.h @@ -52,7 +52,7 @@ class ConfigInputWidget: public ConfigTaskWidget public: ConfigInputWidget(QWidget *parent = 0); ~ConfigInputWidget(); - enum wizardSteps{wizardWelcome,wizardChooseMode,wizardChooseType,wizardIdentifySticks,wizardIdentifyCenter,wizardIdentifyLimits,wizardIdentifyInverted,wizardFinish}; + enum wizardSteps{wizardWelcome,wizardChooseMode,wizardChooseType,wizardIdentifySticks,wizardIdentifyCenter,wizardIdentifyLimits,wizardIdentifyInverted,wizardFinish,wizardNone}; enum txMode{mode1,mode2}; enum txMovements{moveLeftVerticalStick,moveRightVerticalStick,moveLeftHorizontalStick,moveRightHorizontalStick,moveAccess0,moveAccess1,moveAccess2,moveFlightMode,centerAll,moveAll,nothing}; enum txMovementType{vertical,horizontal,jump,mix}; @@ -67,7 +67,6 @@ private: void setTxMovement(txMovements movement); Ui_InputWidget *m_config; wizardSteps wizardStep; - void setupWizardWidget(int step); QList extraWidgets; txMode transmitterMode; txType transmitterType; @@ -85,12 +84,16 @@ private: QEventLoop * loop; bool skipflag; - uint currentCommand; + int currentChannelNum; + QList heliChannelOrder; + QList acroChannelOrder; ManualControlCommand * manualCommandObj; ManualControlCommand::DataFields manualCommandData; + UAVObject::Metadata manualControlMdata; ManualControlSettings * manualSettingsObj; ManualControlSettings::DataFields manualSettingsData; + ManualControlSettings::DataFields previousManualSettingsData; ReceiverActivity * receiverActivityObj; ReceiverActivity::DataFields receiverActivityData; @@ -121,7 +124,16 @@ private: void setMoveFromCommand(int command); bool isSimple; void goToWizard(); - int getChannelFromStep(int); + + void fastMdata(); + void restoreMdata(); + + void setChannel(int); + void nextChannel(); + void prevChannel(); + + void wizardSetUpStep(enum wizardSteps); + void wizardTearDownStep(enum wizardSteps); private slots: void wzNext(); void wzBack(); From 3ff7bf7ed00cab8396efe49f12b02f035d2b13a6 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sun, 11 Sep 2011 12:11:15 -0500 Subject: [PATCH 130/145] Input Configuration: Make sure the input channel form always is properly aligned. I hope the fixed size behaves well across platforms. --- .../src/plugins/config/inputchannelform.ui | 82 ++++++++++++++++--- 1 file changed, 72 insertions(+), 10 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/inputchannelform.ui b/ground/openpilotgcs/src/plugins/config/inputchannelform.ui index 4ebaa12eb..b1b69c052 100644 --- a/ground/openpilotgcs/src/plugins/config/inputchannelform.ui +++ b/ground/openpilotgcs/src/plugins/config/inputchannelform.ui @@ -6,8 +6,8 @@ 0 0 - 482 - 48 + 543 + 49 @@ -49,7 +49,7 @@ - 5 + 6 0 @@ -63,16 +63,28 @@ + + QAbstractSpinBox::NoButtons + 9999 + + 1000 + - + + + QAbstractSpinBox::NoButtons + 9999 + + 1000 + @@ -105,7 +117,7 @@ - + true @@ -125,7 +137,7 @@ - + true @@ -135,7 +147,7 @@ - + Qt::Horizontal @@ -174,7 +186,7 @@ - + false @@ -184,13 +196,63 @@ - + + + + 0 + 0 + + + + + 30 + 0 + + + + + 30 + 16777215 + + - TextLabel + + + + + Qt::Horizontal + + + QSizePolicy::Fixed + + + + 10 + 20 + + + + + + + + Qt::Horizontal + + + QSizePolicy::Fixed + + + + 10 + 20 + + + + From 5d43a8dd36980e3a45a4f196fd85c41f70c2b866 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sun, 11 Sep 2011 12:24:48 -0500 Subject: [PATCH 131/145] Input configuration; Remove some old unused variables --- ground/openpilotgcs/src/plugins/config/configinputwidget.h | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/configinputwidget.h b/ground/openpilotgcs/src/plugins/config/configinputwidget.h index b8dc664ea..4828c2a20 100644 --- a/ground/openpilotgcs/src/plugins/config/configinputwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configinputwidget.h @@ -122,8 +122,6 @@ private: QTimer * animate; void resetTxControls(); void setMoveFromCommand(int command); - bool isSimple; - void goToWizard(); void fastMdata(); void restoreMdata(); @@ -138,8 +136,8 @@ private slots: void wzNext(); void wzBack(); void wzCancel(); - void goToNormalWizard(); - void goToSimpleWizard(); + void goToWizard(); + void openHelp(); void identifyControls(); void identifyLimits(); From 6945f17ebae9523b4adcbac646a522b44a289978 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sun, 11 Sep 2011 12:27:17 -0500 Subject: [PATCH 132/145] Make the StabilizationSettings.MaxRate field only apply to the stabilized modes. That way ManualRate can exceed MaxRate. --- flight/Modules/Stabilization/stabilization.c | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/flight/Modules/Stabilization/stabilization.c b/flight/Modules/Stabilization/stabilization.c index 7142c9770..50a8c253a 100644 --- a/flight/Modules/Stabilization/stabilization.c +++ b/flight/Modules/Stabilization/stabilization.c @@ -254,6 +254,13 @@ static void stabilizationTask(void* parameters) } case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE: rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], local_error[i]); + + if(rateDesiredAxis[i] > settings.MaximumRate[i]) + rateDesiredAxis[i] = settings.MaximumRate[i]; + else if(rateDesiredAxis[ct] < -settings.MaximumRate[ct]) + rateDesiredAxis[i] = -settings.MaximumRate[i]; + + axis_lock_accum[i] = 0; break; @@ -272,6 +279,12 @@ static void stabilizationTask(void* parameters) rateDesiredAxis[i] = ApplyPid(&pids[PID_ROLL + i], axis_lock_accum[i]); } + + if(rateDesiredAxis[i] > settings.MaximumRate[i]) + rateDesiredAxis[i] = settings.MaximumRate[i]; + else if(rateDesiredAxis[ct] < -settings.MaximumRate[ct]) + rateDesiredAxis[i] = -settings.MaximumRate[i]; + break; } } @@ -284,11 +297,6 @@ static void stabilizationTask(void* parameters) //Calculate desired command for(int8_t ct=0; ct< MAX_AXES; ct++) { - if(rateDesiredAxis[ct] > settings.MaximumRate[ct]) - rateDesiredAxis[ct] = settings.MaximumRate[ct]; - else if(rateDesiredAxis[ct] < -settings.MaximumRate[ct]) - rateDesiredAxis[ct] = -settings.MaximumRate[ct]; - switch(stabDesired.StabilizationMode[ct]) { case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE: From deade53c5d77cc5df757df2985f72bd8edf58ed5 Mon Sep 17 00:00:00 2001 From: Mike Smith Date: Sun, 11 Sep 2011 10:49:09 -0700 Subject: [PATCH 133/145] OP-578 - check for CODE_SOURCERY before setting -fpromote-loop-indices --- flight/Bootloaders/BootloaderUpdater/Makefile | 2 ++ flight/Bootloaders/CopterControl/Makefile | 2 ++ flight/Bootloaders/OpenPilot/Makefile | 2 ++ flight/Bootloaders/PipXtreme/Makefile | 2 ++ flight/CopterControl/Makefile | 2 ++ 5 files changed, 10 insertions(+) diff --git a/flight/Bootloaders/BootloaderUpdater/Makefile b/flight/Bootloaders/BootloaderUpdater/Makefile index 81aa163ec..aabfc88bf 100644 --- a/flight/Bootloaders/BootloaderUpdater/Makefile +++ b/flight/Bootloaders/BootloaderUpdater/Makefile @@ -258,7 +258,9 @@ CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) -I. CFLAGS += -mapcs-frame CFLAGS += -fomit-frame-pointer +ifeq ($(CODE_SOURCERY), YES) CFLAGS += -fpromote-loop-indices +endif CFLAGS += -Wall CFLAGS += -Werror diff --git a/flight/Bootloaders/CopterControl/Makefile b/flight/Bootloaders/CopterControl/Makefile index fe613f909..99521020d 100644 --- a/flight/Bootloaders/CopterControl/Makefile +++ b/flight/Bootloaders/CopterControl/Makefile @@ -300,7 +300,9 @@ CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) -I. CFLAGS += -mapcs-frame CFLAGS += -fomit-frame-pointer +ifeq ($(CODE_SOURCERY), YES) CFLAGS += -fpromote-loop-indices +endif CFLAGS += -Wall CFLAGS += -Werror diff --git a/flight/Bootloaders/OpenPilot/Makefile b/flight/Bootloaders/OpenPilot/Makefile index e4285f7e0..cad11dedf 100644 --- a/flight/Bootloaders/OpenPilot/Makefile +++ b/flight/Bootloaders/OpenPilot/Makefile @@ -306,7 +306,9 @@ CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) -I. CFLAGS += -mapcs-frame CFLAGS += -fomit-frame-pointer +ifeq ($(CODE_SOURCERY), YES) CFLAGS += -fpromote-loop-indices +endif CFLAGS += -Wall CFLAGS += -Werror diff --git a/flight/Bootloaders/PipXtreme/Makefile b/flight/Bootloaders/PipXtreme/Makefile index 64f13d0d2..19b651a5f 100644 --- a/flight/Bootloaders/PipXtreme/Makefile +++ b/flight/Bootloaders/PipXtreme/Makefile @@ -301,7 +301,9 @@ CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) -I. CFLAGS += -mapcs-frame CFLAGS += -fomit-frame-pointer +ifeq ($(CODE_SOURCERY), YES) CFLAGS += -fpromote-loop-indices +endif CFLAGS += -Wall CFLAGS += -Werror diff --git a/flight/CopterControl/Makefile b/flight/CopterControl/Makefile index 6bfb6e5a6..b3d9142c1 100644 --- a/flight/CopterControl/Makefile +++ b/flight/CopterControl/Makefile @@ -443,7 +443,9 @@ CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) -I. CFLAGS += -mapcs-frame CFLAGS += -fomit-frame-pointer +ifeq ($(CODE_SOURCERY), YES) CFLAGS += -fpromote-loop-indices +endif CFLAGS += -Wall CFLAGS += -Werror From b38081bb1bb331f36fe5395e6f75244ab4d07af7 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sun, 11 Sep 2011 16:33:38 -0500 Subject: [PATCH 134/145] Stabilization: When in `none` mode zero the integral accumulators for rate and attitude loops. When not using outer loop zero that accumulator. --- flight/Modules/Stabilization/stabilization.c | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/flight/Modules/Stabilization/stabilization.c b/flight/Modules/Stabilization/stabilization.c index 50a8c253a..5669df9d5 100644 --- a/flight/Modules/Stabilization/stabilization.c +++ b/flight/Modules/Stabilization/stabilization.c @@ -235,6 +235,9 @@ static void stabilizationTask(void* parameters) { case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE: rateDesiredAxis[i] = attitudeDesiredAxis[i]; + + // Zero attitude and axis lock accumulators + pids[PID_ROLL + i] = 0; axis_lock_accum[i] = 0; break; @@ -249,6 +252,8 @@ static void stabilizationTask(void* parameters) rateDesiredAxis[i] = attitudeDesiredAxis[i] + weak_leveling; + // Zero attitude and axis lock accumulators + pids[PID_ROLL + i] = 0; axis_lock_accum[i] = 0; break; } @@ -314,14 +319,20 @@ static void stabilizationTask(void* parameters) case ROLL: actuatorDesiredAxis[ct] = bound(attitudeDesiredAxis[ct]); shouldUpdate = 1; + pids[PID_RATE_ROLL].iAccumulator = 0; + pids[PID_ROLL].iAccumulator = 0; break; case PITCH: actuatorDesiredAxis[ct] = bound(attitudeDesiredAxis[ct]); shouldUpdate = 1; + pids[PID_RATE_PITCH].iAccumulator = 0; + pids[PID_PITCH].iAccumulator = 0; break; case YAW: actuatorDesiredAxis[ct] = bound(attitudeDesiredAxis[ct]); shouldUpdate = 1; + pids[PID_RATE_YAW].iAccumulator = 0; + pids[PID_YAW].iAccumulator = 0; break; } break; From ae74706b204239add00e5bfea0c372114efcd921 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sun, 11 Sep 2011 17:16:26 -0500 Subject: [PATCH 135/145] Update URL to ARM SDK --- Makefile | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Makefile b/Makefile index 18cdb3b2e..0413d39a6 100644 --- a/Makefile +++ b/Makefile @@ -183,13 +183,13 @@ qt_sdk_clean: ARM_SDK_DIR := $(TOOLS_DIR)/arm-2011.03 .PHONY: arm_sdk_install -arm_sdk_install: ARM_SDK_URL := http://www.codesourcery.com/sgpp/lite/arm/portal/package8734/public/arm-none-eabi/arm-2011.03-42-arm-none-eabi-i686-pc-linux-gnu.tar.bz2 +arm_sdk_install: ARM_SDK_URL := https://sourcery.mentor.com/sgpp/lite/arm/portal/package8736/public/arm-none-eabi/arm-2011.03-42-arm-none-eabi-i686-pc-linux-gnu.tar.bz2 arm_sdk_install: ARM_SDK_FILE := $(notdir $(ARM_SDK_URL)) # order-only prereq on directory existance: arm_sdk_install: | $(DL_DIR) $(TOOLS_DIR) arm_sdk_install: arm_sdk_clean # download the source only if it's newer than what we already have - $(V1) wget -N -P "$(DL_DIR)" "$(ARM_SDK_URL)" + $(V1) wget --no-check-certificate -N -P "$(DL_DIR)" "$(ARM_SDK_URL)" # binary only release so just extract it $(V1) tar -C $(TOOLS_DIR) -xjf "$(DL_DIR)/$(ARM_SDK_FILE)" From 7b3056d10f77cd948ffee690e804a9e01a0245f3 Mon Sep 17 00:00:00 2001 From: Oleg Semyonov Date: Mon, 12 Sep 2011 01:52:30 +0300 Subject: [PATCH 136/145] Input Configuration: fix S.Bus receiver in multi-receiver code --- flight/CopterControl/System/pios_board.c | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/flight/CopterControl/System/pios_board.c b/flight/CopterControl/System/pios_board.c index d9023ffb9..18c379270 100644 --- a/flight/CopterControl/System/pios_board.c +++ b/flight/CopterControl/System/pios_board.c @@ -1029,6 +1029,13 @@ void PIOS_Board_Init(void) { if (PIOS_SBUS_Init(&pios_sbus_id, &pios_sbus_cfg, &pios_usart_com_driver, pios_usart_sbus_id)) { PIOS_Assert(0); } + + uint32_t pios_sbus_rcvr_id; + if (PIOS_RCVR_Init(&pios_sbus_rcvr_id, &pios_sbus_rcvr_driver, pios_sbus_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS] = pios_sbus_rcvr_id; + } #endif /* PIOS_INCLUDE_SBUS */ break; From f5369f933831a69b455d2c907facbf46afc9f674 Mon Sep 17 00:00:00 2001 From: Oleg Semyonov Date: Sun, 11 Sep 2011 22:04:03 +0300 Subject: [PATCH 137/145] Input Configuration: support all 18 S.Bus input channels --- flight/PiOS/STM32F10x/pios_sbus.c | 28 ++++++++++++------- flight/PiOS/inc/pios_sbus_priv.h | 18 +++++++----- .../src/plugins/config/inputchannelform.cpp | 4 ++- 3 files changed, 32 insertions(+), 18 deletions(-) diff --git a/flight/PiOS/STM32F10x/pios_sbus.c b/flight/PiOS/STM32F10x/pios_sbus.c index 543da364b..319142dc6 100644 --- a/flight/PiOS/STM32F10x/pios_sbus.c +++ b/flight/PiOS/STM32F10x/pios_sbus.c @@ -34,8 +34,6 @@ #if defined(PIOS_INCLUDE_SBUS) -/* Global Variables */ - /* Provide a RCVR driver */ static int32_t PIOS_SBUS_Get(uint32_t rcvr_id, uint8_t channel); @@ -65,21 +63,17 @@ static void reset_channels(void) /** * unroll_channels() function computes channel_data[] from received_data[] - * For efficiency it unrolls first 8 channels without loops. If other - * 8 channels are needed they can be unrolled using the same code - * starting from s[11] instead of s[0]. Two extra digital channels are - * accessible using (s[22] & SBUS_FLAG_DGx) logical expressions. + * For efficiency it unrolls first 8 channels without loops and does the + * same for other 8 channels. Also 2 discrete channels will be set. */ static void unroll_channels(void) { uint8_t *s = received_data; uint16_t *d = channel_data; -#if (SBUS_NUMBER_OF_CHANNELS != 8) -#error Current S.Bus code unrolls only first 8 channels -#endif - #define F(v,s) ((v) >> s) & 0x7ff + + /* unroll channels 1-8 */ *d++ = F(s[0] | s[1] << 8, 0); *d++ = F(s[1] | s[2] << 8, 3); *d++ = F(s[2] | s[3] << 8 | s[4] << 16, 6); @@ -88,6 +82,20 @@ static void unroll_channels(void) *d++ = F(s[6] | s[7] << 8 | s[8] << 16, 7); *d++ = F(s[8] | s[9] << 8, 2); *d++ = F(s[9] | s[10] << 8, 5); + + /* unroll channels 9-16 */ + *d++ = F(s[11] | s[12] << 8, 0); + *d++ = F(s[12] | s[13] << 8, 3); + *d++ = F(s[13] | s[14] << 8 | s[15] << 16, 6); + *d++ = F(s[15] | s[16] << 8, 1); + *d++ = F(s[16] | s[17] << 8, 4); + *d++ = F(s[17] | s[18] << 8 | s[19] << 16, 7); + *d++ = F(s[19] | s[20] << 8, 2); + *d++ = F(s[20] | s[21] << 8, 5); + + /* unroll discrete channels 17 and 18 */ + *d++ = (s[22] & SBUS_FLAG_DC1) ? SBUS_VALUE_MAX : SBUS_VALUE_MIN; + *d++ = (s[22] & SBUS_FLAG_DC2) ? SBUS_VALUE_MAX : SBUS_VALUE_MIN; } /** diff --git a/flight/PiOS/inc/pios_sbus_priv.h b/flight/PiOS/inc/pios_sbus_priv.h index 4e46b2089..b7e441c30 100644 --- a/flight/PiOS/inc/pios_sbus_priv.h +++ b/flight/PiOS/inc/pios_sbus_priv.h @@ -44,8 +44,8 @@ * 1 byte - 0x0f (start of frame byte) * 22 bytes - channel data (11 bit/channel, 16 channels, LSB first) * 1 byte - bit flags: - * 0x01 - digital channel 1, - * 0x02 - digital channel 2, + * 0x01 - discrete channel 1, + * 0x02 - discrete channel 2, * 0x04 - lost frame flag, * 0x08 - failsafe flag, * 0xf0 - reserved @@ -54,16 +54,20 @@ #define SBUS_FRAME_LENGTH (1+22+1+1) #define SBUS_SOF_BYTE 0x0f #define SBUS_EOF_BYTE 0x00 -#define SBUS_FLAG_DG1 0x01 -#define SBUS_FLAG_DG2 0x02 +#define SBUS_FLAG_DC1 0x01 +#define SBUS_FLAG_DC2 0x02 #define SBUS_FLAG_FL 0x04 #define SBUS_FLAG_FS 0x08 /* - * S.Bus protocol provides up to 16 analog and 2 digital channels. - * Only 8 channels are currently supported by the OpenPilot. + * S.Bus protocol provides 16 proportional and 2 discrete channels. + * Do not change unless driver code is updated accordingly. */ -#define SBUS_NUMBER_OF_CHANNELS 8 +#define SBUS_NUMBER_OF_CHANNELS (16 + 2) + +/* Discrete channels represented as bits, provide values for them */ +#define SBUS_VALUE_MIN 352 +#define SBUS_VALUE_MAX 1696 /* * S.Bus configuration programmable invertor diff --git a/ground/openpilotgcs/src/plugins/config/inputchannelform.cpp b/ground/openpilotgcs/src/plugins/config/inputchannelform.cpp index c30de6d31..70223d57f 100644 --- a/ground/openpilotgcs/src/plugins/config/inputchannelform.cpp +++ b/ground/openpilotgcs/src/plugins/config/inputchannelform.cpp @@ -85,11 +85,13 @@ void inputChannelForm::groupUpdated() count = 8; // Need to make this 6 for CC break; case ManualControlSettings::CHANNELGROUPS_PPM: - case ManualControlSettings::CHANNELGROUPS_SBUS: case ManualControlSettings::CHANNELGROUPS_SPEKTRUM1: case ManualControlSettings::CHANNELGROUPS_SPEKTRUM2: count = 12; break; + case ManualControlSettings::CHANNELGROUPS_SBUS: + count = 18; + break; case ManualControlSettings::CHANNELGROUPS_GCS: count = 5; case ManualControlSettings::CHANNELGROUPS_NONE: From 8f2fad6918362bf9deed357db9ada54ea0e8e361 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sat, 10 Sep 2011 15:54:10 -0500 Subject: [PATCH 138/145] Fix for merging with the collective channel with the swashplate fix. --- flight/Modules/Actuator/actuator.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/flight/Modules/Actuator/actuator.c b/flight/Modules/Actuator/actuator.c index f610eedbe..f50848ddc 100644 --- a/flight/Modules/Actuator/actuator.c +++ b/flight/Modules/Actuator/actuator.c @@ -240,7 +240,8 @@ static void actuatorTask(void* parameters) break; case MIXERSETTINGS_CURVE2SOURCE_COLLECTIVE: ManualControlCommandCollectiveGet(&curve2); - curve2 = MixerCurve(curve2,mixerSettings.ThrottleCurve2); + curve2 = MixerCurve(curve2,mixerSettings.ThrottleCurve2, + MIXERSETTINGS_THROTTLECURVE2_NUMELEM); break; case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY0: case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY1: From 6bd52832be5890d6e3b7b649c94d44143f2cf4f7 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Mon, 12 Sep 2011 11:47:59 -0500 Subject: [PATCH 139/145] Heli configuration: Now the collective passthrough uses the collective channel only. Please note this requires all heli users to scrub their settings and start over. Please erase your setting and be careful. Remove all blades. --- .../openpilotgcs/src/plugins/config/ccpm.ui | 78 +------------------ .../plugins/config/configairframewidget.cpp | 1 - .../src/plugins/config/configccpmwidget.cpp | 17 +--- .../src/plugins/config/configccpmwidget.h | 1 - 4 files changed, 4 insertions(+), 93 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/ccpm.ui b/ground/openpilotgcs/src/plugins/config/ccpm.ui index 7730a7a29..77e33e8ca 100644 --- a/ground/openpilotgcs/src/plugins/config/ccpm.ui +++ b/ground/openpilotgcs/src/plugins/config/ccpm.ui @@ -880,7 +880,7 @@ - + Link Roll/Pitch @@ -890,7 +890,7 @@ - + Link Cyclic/Collective @@ -900,80 +900,6 @@ - - - - QLayout::SetNoConstraint - - - - - true - - - - 0 - 0 - - - - - 80 - 0 - - - - - 80 - 16777215 - - - - - 11 - - - - Qt::LeftToRight - - - Collective Ch - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - true - - - - - - - true - - - - 0 - 0 - - - - - 90 - 0 - - - - - 100 - 16777215 - - - - - - diff --git a/ground/openpilotgcs/src/plugins/config/configairframewidget.cpp b/ground/openpilotgcs/src/plugins/config/configairframewidget.cpp index 05eb98c4d..a12d34dca 100644 --- a/ground/openpilotgcs/src/plugins/config/configairframewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configairframewidget.cpp @@ -2173,7 +2173,6 @@ void ConfigAirframeWidget::addToDirtyMonitor() addWidget(m_aircraft->widget_3->m_ccpm->ccpmCollectivePassthrough); addWidget(m_aircraft->widget_3->m_ccpm->ccpmLinkRoll); addWidget(m_aircraft->widget_3->m_ccpm->ccpmLinkCyclic); - addWidget(m_aircraft->widget_3->m_ccpm->ccpmCollectiveChannel); addWidget(m_aircraft->widget_3->m_ccpm->ccpmRevoSlider); addWidget(m_aircraft->widget_3->m_ccpm->ccpmREVOspinBox); addWidget(m_aircraft->widget_3->m_ccpm->ccpmCollectiveSlider); diff --git a/ground/openpilotgcs/src/plugins/config/configccpmwidget.cpp b/ground/openpilotgcs/src/plugins/config/configccpmwidget.cpp index 4a103987a..6d7a974c2 100644 --- a/ground/openpilotgcs/src/plugins/config/configccpmwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configccpmwidget.cpp @@ -147,9 +147,6 @@ ConfigccpmWidget::ConfigccpmWidget(QWidget *parent) : ConfigTaskWidget(parent) UAVObjectField * curve2source = mixerSettings->getField("Curve2Source"); Q_ASSERT(curve2source); - m_ccpm->ccpmCollectiveChannel->addItems(curve2source->getOptions()); - m_ccpm->ccpmCollectiveChannel->setCurrentIndex(0); - QStringList channels; channels << "Channel1" << "Channel2" << "Channel3" << "Channel4" << "Channel5" << "Channel6" << "Channel7" << "Channel8" << "None"; @@ -219,7 +216,6 @@ ConfigccpmWidget::ConfigccpmWidget(QWidget *parent) : ConfigTaskWidget(parent) connect(m_ccpm->SwashLvlCancelButton, SIGNAL(clicked()), this, SLOT(SwashLvlCancelButtonPressed())); connect(m_ccpm->SwashLvlFinishButton, SIGNAL(clicked()), this, SLOT(SwashLvlFinishButtonPressed())); - connect(m_ccpm->ccpmCollectivePassthrough, SIGNAL(clicked()), this, SLOT(SetUIComponentVisibilities())); connect(m_ccpm->ccpmLinkCyclic, SIGNAL(clicked()), this, SLOT(SetUIComponentVisibilities())); connect(m_ccpm->ccpmLinkRoll, SIGNAL(clicked()), this, SLOT(SetUIComponentVisibilities())); @@ -1016,9 +1012,6 @@ void ConfigccpmWidget::UpdateCCPMOptionsFromUI() //correction angle GUIConfigData.heli.CorrectionAngle = m_ccpm->ccpmCorrectionAngle->value(); - //CollectiveChannel - GUIConfigData.heli.CollectiveChannel = m_ccpm->ccpmCollectiveChannel->currentIndex(); - //update sliders if (useCCPM) { @@ -1058,10 +1051,7 @@ void ConfigccpmWidget::UpdateCCPMUIFromOptions() //correction angle m_ccpm->ccpmCorrectionAngle->setValue(GUIConfigData.heli.CorrectionAngle); - - //CollectiveChannel - m_ccpm->ccpmCollectiveChannel->setCurrentIndex(GUIConfigData.heli.CollectiveChannel); - + //update sliders m_ccpm->ccpmCollectiveScale->setValue(GUIConfigData.heli.SliderValue0); m_ccpm->ccpmCollectiveScaleBox->setValue(GUIConfigData.heli.SliderValue0); @@ -1092,9 +1082,6 @@ void ConfigccpmWidget::SetUIComponentVisibilities() m_ccpm->ccpmPitchMixingBox->setVisible(!GUIConfigData.heli.ccpmCollectivePassthroughState && GUIConfigData.heli.ccpmLinkCyclicState); m_ccpm->ccpmCollectiveScalingBox->setVisible(GUIConfigData.heli.ccpmCollectivePassthroughState || !GUIConfigData.heli.ccpmLinkCyclicState); - m_ccpm->ccpmCollectiveChLabel->setVisible(GUIConfigData.heli.ccpmCollectivePassthroughState); - m_ccpm->ccpmCollectiveChannel->setVisible(GUIConfigData.heli.ccpmCollectivePassthroughState); - m_ccpm->ccpmLinkCyclic->setVisible(!GUIConfigData.heli.ccpmCollectivePassthroughState); m_ccpm->ccpmCyclicScalingBox->setVisible((GUIConfigData.heli.ccpmCollectivePassthroughState || !GUIConfigData.heli.ccpmLinkCyclicState) && GUIConfigData.heli.ccpmLinkRollState); @@ -1316,7 +1303,7 @@ void ConfigccpmWidget::sendccpmUpdate() //MixerSettings.Curve2Source = Throttle,Roll,Pitch,Yaw,Accessory0,Accessory1,Accessory2,Accessory3,Accessory4,Accessory5 //check if we are using throttle or directly from a channel... if (GUIConfigData.heli.ccpmCollectivePassthroughState) - mixerSettingsData.Curve2Source = GUIConfigData.heli.CollectiveChannel; + mixerSettingsData.Curve2Source = MixerSettings::CURVE2SOURCE_COLLECTIVE; else mixerSettingsData.Curve2Source = MixerSettings::CURVE2SOURCE_THROTTLE; diff --git a/ground/openpilotgcs/src/plugins/config/configccpmwidget.h b/ground/openpilotgcs/src/plugins/config/configccpmwidget.h index 56898f0fb..c6bc1b27b 100644 --- a/ground/openpilotgcs/src/plugins/config/configccpmwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configccpmwidget.h @@ -57,7 +57,6 @@ typedef struct { uint ccpmCollectivePassthroughState:1; uint ccpmLinkCyclicState:1; uint ccpmLinkRollState:1; - uint CollectiveChannel:3;//20bits uint SliderValue0:7; uint SliderValue1:7; uint SliderValue2:7;//41bits From b32f6d9383fd0d614a979bec496a5681ac878154 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Mon, 12 Sep 2011 11:50:11 -0500 Subject: [PATCH 140/145] History.txt update --- HISTORY.txt | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/HISTORY.txt b/HISTORY.txt index b0e1ce8b0..5ec0520b2 100644 --- a/HISTORY.txt +++ b/HISTORY.txt @@ -1,5 +1,15 @@ Short summary of changes. For a complete list see the git log. +2011-09-09 +Some large updates to the input system. Now multiple receivers can be +connected at once. A wizard was added for configuring the input channels. A +specific collective pitch channel was added. + +2011-09-04 +Improvements to the failsafe handling code for inputs. PWM power off is now +detected properly. Powering on transmitter for Spektrum Satellite no longer +causes a glitch on servos. + 2011-08-10 Added Camera Stabilization and a gui to configure this. This is a software selectable module from the GUI. However, a restart is required to make it From 84f38eab78ae68fe1a084396474ef7608785d324 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Mon, 12 Sep 2011 11:52:59 -0500 Subject: [PATCH 141/145] HISTORY update --- HISTORY.txt | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/HISTORY.txt b/HISTORY.txt index 5ec0520b2..98193adce 100644 --- a/HISTORY.txt +++ b/HISTORY.txt @@ -1,5 +1,10 @@ Short summary of changes. For a complete list see the git log. +2011-09-12 +Max rate now ONLY applies to attitude and axis lock mode. Manual rate is the +only term that limits the rate mode now (and in axis lock when you push stick +only manual rate applies). Also integrals are reset when unused. + 2011-09-09 Some large updates to the input system. Now multiple receivers can be connected at once. A wizard was added for configuring the input channels. A From c36297227d046a6f94605e89e79f57002ece9348 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Mon, 12 Sep 2011 11:56:01 -0500 Subject: [PATCH 142/145] Stabilization fix typo --- flight/Modules/Stabilization/stabilization.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/flight/Modules/Stabilization/stabilization.c b/flight/Modules/Stabilization/stabilization.c index 5669df9d5..ffc3c53be 100644 --- a/flight/Modules/Stabilization/stabilization.c +++ b/flight/Modules/Stabilization/stabilization.c @@ -237,7 +237,7 @@ static void stabilizationTask(void* parameters) rateDesiredAxis[i] = attitudeDesiredAxis[i]; // Zero attitude and axis lock accumulators - pids[PID_ROLL + i] = 0; + pids[PID_ROLL + i].iAccumulator = 0; axis_lock_accum[i] = 0; break; @@ -253,7 +253,7 @@ static void stabilizationTask(void* parameters) rateDesiredAxis[i] = attitudeDesiredAxis[i] + weak_leveling; // Zero attitude and axis lock accumulators - pids[PID_ROLL + i] = 0; + pids[PID_ROLL + i].iAccumulator = 0; axis_lock_accum[i] = 0; break; } @@ -262,7 +262,7 @@ static void stabilizationTask(void* parameters) if(rateDesiredAxis[i] > settings.MaximumRate[i]) rateDesiredAxis[i] = settings.MaximumRate[i]; - else if(rateDesiredAxis[ct] < -settings.MaximumRate[ct]) + else if(rateDesiredAxis[i] < -settings.MaximumRate[i]) rateDesiredAxis[i] = -settings.MaximumRate[i]; @@ -287,7 +287,7 @@ static void stabilizationTask(void* parameters) if(rateDesiredAxis[i] > settings.MaximumRate[i]) rateDesiredAxis[i] = settings.MaximumRate[i]; - else if(rateDesiredAxis[ct] < -settings.MaximumRate[ct]) + else if(rateDesiredAxis[i] < -settings.MaximumRate[i]) rateDesiredAxis[i] = -settings.MaximumRate[i]; break; From 06fc5a1110e0e2a3e2031457db79a73c9023523e Mon Sep 17 00:00:00 2001 From: James Cotton Date: Mon, 12 Sep 2011 22:27:11 -0500 Subject: [PATCH 143/145] Input Configuration: Fix flight mode slider detection (looking at wrong channel number). --- .../src/plugins/config/configinputwidget.cpp | 51 +++++++++---------- 1 file changed, 24 insertions(+), 27 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp index 6982ef91d..9e7cfdd04 100644 --- a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp @@ -1317,37 +1317,34 @@ void ConfigInputWidget::moveFMSlider() { ManualControlSettings::DataFields manualSettingsDataPriv = manualSettingsObj->getData(); ManualControlCommand::DataFields manualCommandDataPriv=manualCommandObj->getData(); - uint chIndex = manualSettingsDataPriv.ChannelNumber[ManualControlSettings::CHANNELNUMBER_FLIGHTMODE]; - if (chIndex < 8) { - float valueScaled; - int chMin = manualSettingsDataPriv.ChannelMin[ManualControlSettings::CHANNELMIN_FLIGHTMODE]; - int chMax = manualSettingsDataPriv.ChannelMax[ManualControlSettings::CHANNELMAX_FLIGHTMODE]; - int chNeutral = manualSettingsDataPriv.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_FLIGHTMODE]; + float valueScaled; + int chMin = manualSettingsDataPriv.ChannelMin[ManualControlSettings::CHANNELMIN_FLIGHTMODE]; + int chMax = manualSettingsDataPriv.ChannelMax[ManualControlSettings::CHANNELMAX_FLIGHTMODE]; + int chNeutral = manualSettingsDataPriv.ChannelNeutral[ManualControlSettings::CHANNELNEUTRAL_FLIGHTMODE]; - int value = manualCommandDataPriv.Channel[chIndex]; - if ((chMax > chMin && value >= chNeutral) || (chMin > chMax && value <= chNeutral)) - { - if (chMax != chNeutral) - valueScaled = (float)(value - chNeutral) / (float)(chMax - chNeutral); - else - valueScaled = 0; - } + int value = manualCommandDataPriv.Channel[ManualControlSettings::CHANNELMIN_FLIGHTMODE]; + if ((chMax > chMin && value >= chNeutral) || (chMin > chMax && value <= chNeutral)) + { + if (chMax != chNeutral) + valueScaled = (float)(value - chNeutral) / (float)(chMax - chNeutral); else - { - if (chMin != chNeutral) - valueScaled = (float)(value - chNeutral) / (float)(chNeutral - chMin); - else - valueScaled = 0; - } - - if(valueScaled < -(1.0 / 3.0)) - m_config->fmsSlider->setValue(-100); - else if (valueScaled > (1.0/3.0)) - m_config->fmsSlider->setValue(100); - else - m_config->fmsSlider->setValue(0); + valueScaled = 0; } + else + { + if (chMin != chNeutral) + valueScaled = (float)(value - chNeutral) / (float)(chNeutral - chMin); + else + valueScaled = 0; + } + + if(valueScaled < -(1.0 / 3.0)) + m_config->fmsSlider->setValue(-100); + else if (valueScaled > (1.0/3.0)) + m_config->fmsSlider->setValue(100); + else + m_config->fmsSlider->setValue(0); } void ConfigInputWidget::updateCalibration() From c92870d29ecad66a98e6fd071d3ca24eada54aa3 Mon Sep 17 00:00:00 2001 From: Oleg Semyonov Date: Tue, 13 Sep 2011 16:10:10 +0300 Subject: [PATCH 144/145] Remove unused file (versionblob.py) --- flight/Project/versionblob.py | 99 ----------------------------------- 1 file changed, 99 deletions(-) delete mode 100755 flight/Project/versionblob.py diff --git a/flight/Project/versionblob.py b/flight/Project/versionblob.py deleted file mode 100755 index 82440a746..000000000 --- a/flight/Project/versionblob.py +++ /dev/null @@ -1,99 +0,0 @@ -#!/usr/bin/env python - -# Generate a version blob for -# the OpenPilot firmware. -# -# By E. Lafargue (c) 2011 E. Lafargue & the OpenPilot team -# Licence: GPLv3 -# -### -# Usage: -# versionblob.py --append --firmware= --boardid= -# -# append: if present, then append blob to firmware file directly, otherwise create "blob.bin" -# firmware: the filename of the firmware binary -# boardid: as a string, the board code, for example "0401" for CC board version 1. -# should match the codes in firmware description files. -# -# We have 100 bytes for the whole description. -# -# Only the first 40 are visible on the FirmwareIAP uavobject, the remaining -# 60 are ok to use for packaging and will be saved in the flash -# -# Structure is: -# 4 bytes: header: "OpFw" -# 4 bytes: GIT commit tag (short version of SHA1) -# 4 bytes: Unix timestamp of compile time -# 2 bytes: target platform. Should follow same rule as BOARD_TYPE and BOARD_REVISION in board define files. -# 26 bytes: commit tag if it is there, otherwise branch name. Zero-padded -# ---- 40 bytes limit --- -# 20 bytes: SHA1 sum of the firmware. -# 40 bytes: free for now. - -import binascii -import os -from time import time -import argparse - -# Do the argument parsing: -parser = argparse.ArgumentParser(description='Generate firmware desciption blob') -parser.add_argument('--append', action='store_true') -parser.add_argument('--firmware', help='name of firmware binary file' , required=True) -parser.add_argument('--boardid', help='ID of board model, for example 0401 for CopterControl', required=True) -args = parser.parse_args() -print args - -if args.append == True: - print 'Appending description blob directly to ' + args.firmware - filename = args.firmware - file = open(filename,"ab") -else: - filename = 'blob.bin' - file = open(filename,"wb") - - -# Write the magic value: -file.write("OpFw") -# Get the short commit tag of the current git repository. -# Strip it to 8 characters for a 4 byte (int32) value. -# We have no full guarantee of unicity, but it is good enough -# with the rest of the information in the structure. -hs= os.popen('git rev-parse --short=8 HEAD').read().strip() -print "Version: " + hs -hb=binascii.a2b_hex(hs) -file.write(hb) -# Then the Unix time into a 32 bit integer: -print "Date: " + hex(int(time())).lstrip('0x') -hb = binascii.a2b_hex(hex(int(time())).lstrip('0x')) -file.write(hb) - -# Then write board type and board revision -hb = binascii.a2b_hex(args.boardid) -file.write(hb) - -# Last: a user-friendly description if it exists in GIT, otherwise -# just "unreleased" -hs = os.popen('git describe --exact-match').read() -if len(hs) == 0 : - print "Unreleased: get branch name instead" - hs = os.popen('git branch --contains HEAD').read() - -file.write(hs[0:26]) -file.write("\0"*(26-len(hs))) - -## Now we are at the 40 bytes mark. - -## Add the 20 byte SHA1 hash of the firmware: -import hashlib -sha1 = hashlib.sha1() -with open('build/coptercontrol/CopterControl.bin','rb') as f: - for chunk in iter(lambda: f.read(8192), ''): - sha1.update(chunk) -file.write(sha1.digest()) - -# Pad will null bytes: -file.write('\0'*40) - - -file.close() - From 3e61e21e89b27d5cd960fe68bf525918ff15196b Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sun, 25 Sep 2011 10:50:06 -0500 Subject: [PATCH 145/145] Output config UI: Add labels for what things are --- .../openpilotgcs/src/plugins/config/output.ui | 21 +++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/ground/openpilotgcs/src/plugins/config/output.ui b/ground/openpilotgcs/src/plugins/config/output.ui index 9c1fdacca..b7855221f 100644 --- a/ground/openpilotgcs/src/plugins/config/output.ui +++ b/ground/openpilotgcs/src/plugins/config/output.ui @@ -949,6 +949,27 @@ p, li { white-space: pre-wrap; } + + + + Min + + + + + + + Neutral (slowest for motor) + + + + + + + Max + + +