From 7d7d03cfd0e7dad8bd9ea4b30c3b539d0d8c754b Mon Sep 17 00:00:00 2001 From: James Cotton Date: Wed, 15 Jun 2011 11:18:29 -0500 Subject: [PATCH 01/55] 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 02/55] 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 03/55] 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 04/55] 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 05/55] 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 06/55] 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 07/55] 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 08/55] 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 09/55] 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 10/55] 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 11/55] 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 12/55] 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 13/55] 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 14/55] 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 15/55] 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 16/55] 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 17/55] 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 18/55] 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 19/55] 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 20/55] 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 21/55] 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 a592e7d398db7f2b3ab88cfc728ea216538a8ff5 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Thu, 4 Aug 2011 07:47:50 -0500 Subject: [PATCH 22/55] 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 fd86c25b49de402c3b94b20f1203a0a340b4e359 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Thu, 4 Aug 2011 15:01:54 -0500 Subject: [PATCH 23/55] 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 f11aa8044421cd47e54b7311af9874606dcf30a6 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Tue, 9 Aug 2011 20:43:48 -0500 Subject: [PATCH 24/55] 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 25/55] 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 26/55] 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 27/55] 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 21:57:17 -0500 Subject: [PATCH 28/55] 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 29/55] 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 30/55] 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 31/55] 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 32/55] 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 33/55] 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 34/55] 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 65cb05e212b063191fc28879e8df73b8f884d524 Mon Sep 17 00:00:00 2001 From: dankers Date: Sat, 13 Aug 2011 06:08:07 +1000 Subject: [PATCH 35/55] hhrhhr's fix for modelview gimbal lock, thank you!! --- .../plugins/modelview/modelviewgadgetwidget.cpp | 16 ++++++---------- 1 file changed, 6 insertions(+), 10 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetwidget.cpp index 09ffafd5a..6e1b2858d 100644 --- a/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetwidget.cpp +++ b/ground/openpilotgcs/src/plugins/modelview/modelviewgadgetwidget.cpp @@ -291,16 +291,12 @@ void ModelViewGadgetWidget::updateAttitude() { AttitudeActual::DataFields data = attActual->getData(); // get attitude data GLC_StructOccurence* rootObject= m_World.rootOccurence(); // get the full 3D model - // create quaternions rotations for each axis - QQuaternion qX= QQuaternion::fromAxisAndAngle(QVector3D(1,0,0),data.Pitch); - QQuaternion qY= QQuaternion::fromAxisAndAngle(QVector3D(0,1,0),data.Roll); - QQuaternion qZ= QQuaternion::fromAxisAndAngle(QVector3D(0,0,1),data.Yaw); - QQuaternion quat= qX * qY * qZ; // create the quaternion of all the rotations - // pass values to simpler variables - double x= quat.vector().x(); - double y= quat.vector().y(); - double z= quat.vector().z(); - double w= quat.scalar(); + double x= data.q3; + double y= data.q2; + double z= data.q4; + double w= data.q1; + if (w == 0.0) + w = 1.0; // create and gives the product of 2 4x4 matrices to get the rotation of the 3D model's matrix QMatrix4x4 m1; m1.setRow(0, QVector4D(w,z,-y,x)); From a2b76adc33a3c16656468211a5121ca1efa3eeda Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sat, 13 Aug 2011 18:44:19 -0500 Subject: [PATCH 36/55] Attitude: Do not start attitude estimation until accel data appears. This fixes the NaN when AttitudeSettings not available. --- flight/Modules/Attitude/attitude.c | 39 +++++++++++++++++++++++------- flight/PiOS/Common/pios_adxl345.c | 16 ++++++++++++ flight/PiOS/inc/pios_adxl345.h | 2 ++ 3 files changed, 48 insertions(+), 9 deletions(-) diff --git a/flight/Modules/Attitude/attitude.c b/flight/Modules/Attitude/attitude.c index 682484543..c70751fa6 100644 --- a/flight/Modules/Attitude/attitude.c +++ b/flight/Modules/Attitude/attitude.c @@ -76,7 +76,7 @@ static void AttitudeTask(void *parameters); static float gyro_correct_int[3] = {0,0,0}; static xQueueHandle gyro_queue; -static void updateSensors(AttitudeRawData *); +static int8_t updateSensors(AttitudeRawData *); static void updateAttitude(AttitudeRawData *); static void settingsUpdatedCb(UAVObjEvent * objEv); @@ -162,14 +162,19 @@ static void AttitudeTask(void *parameters) // Keep flash CS pin high while talking accel PIOS_FLASH_DISABLE; PIOS_ADXL345_Init(); - - + + // Set critical error and wait until the accel is producing data + while(PIOS_ADXL345_FifoElements() == 0) { + AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL); + PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE); + } + // Force settings update to make sure rotation loaded settingsUpdatedCb(AttitudeSettingsHandle()); // Main task loop while (1) { - + FlightStatusData flightStatus; FlightStatusGet(&flightStatus); @@ -197,14 +202,24 @@ static void AttitudeTask(void *parameters) AttitudeRawData attitudeRaw; AttitudeRawGet(&attitudeRaw); - updateSensors(&attitudeRaw); - updateAttitude(&attitudeRaw); - AttitudeRawSet(&attitudeRaw); + if(updateSensors(&attitudeRaw) != 0) + AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); + else { + // Only update attitude when sensor data is good + updateAttitude(&attitudeRaw); + AttitudeRawSet(&attitudeRaw); + AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); + } } } -static void updateSensors(AttitudeRawData * attitudeRaw) +/** + * Get an update from the sensors + * @param[in] attitudeRaw Populate the UAVO instead of saving right here + * @return 0 if successfull, -1 if not + */ +static int8_t updateSensors(AttitudeRawData * attitudeRaw) { struct pios_adxl345_data accel_data; float gyro[4]; @@ -212,9 +227,12 @@ static void updateSensors(AttitudeRawData * attitudeRaw) // Only wait the time for two nominal updates before setting an alarm if(xQueueReceive(gyro_queue, (void * const) gyro, UPDATE_RATE * 2) == errQUEUE_EMPTY) { AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); - return; + return -1; } + // No accel data available + if(PIOS_ADXL345_FifoElements() == 0) + return -1; // First sample is temperature attitudeRaw->gyros[ATTITUDERAW_GYROS_X] = -(gyro[1] - GYRO_NEUTRAL) * gyroGain; @@ -270,6 +288,8 @@ static void updateSensors(AttitudeRawData * attitudeRaw) // Because most crafts wont get enough information from gravity to zero yaw gyro, we try // and make it average zero (weakly) gyro_correct_int[2] += - attitudeRaw->gyros[ATTITUDERAW_GYROS_Z] * yawBiasRate; + + return 0; } static void updateAttitude(AttitudeRawData * attitudeRaw) @@ -307,6 +327,7 @@ static void updateAttitude(AttitudeRawData * attitudeRaw) // Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s gyro_correct_int[0] += accel_err[0] * accelKi; gyro_correct_int[1] += accel_err[1] * accelKi; + //gyro_correct_int[2] += accel_err[2] * settings.AccelKI * dT; // Correct rates based on error, integral component dealt with in updateSensors diff --git a/flight/PiOS/Common/pios_adxl345.c b/flight/PiOS/Common/pios_adxl345.c index e11cadf19..91fb3f91f 100644 --- a/flight/PiOS/Common/pios_adxl345.c +++ b/flight/PiOS/Common/pios_adxl345.c @@ -96,6 +96,22 @@ void PIOS_ADXL345_Init() PIOS_ADXL345_SetMeasure(1); } +/** + * @brief Return number of entries in the fifo + */ +uint8_t PIOS_ADXL345_FifoElements() +{ + uint8_t buf[2] = {0,0}; + uint8_t rec[2] = {0,0}; + buf[0] = ADXL_FIFOSTATUS_ADDR | ADXL_READ_BIT ; // Read fifo status + + PIOS_ADXL345_ClaimBus(); + PIOS_SPI_TransferBlock(PIOS_SPI_ACCEL,&buf[0],&rec[0],sizeof(buf),NULL); + PIOS_ADXL345_ReleaseBus(); + + return rec[1] & 0x3f; +} + /** * @brief Read a single set of values from the x y z channels * @returns The number of samples remaining in the fifo diff --git a/flight/PiOS/inc/pios_adxl345.h b/flight/PiOS/inc/pios_adxl345.h index 1e5c83274..1e0b89f6d 100644 --- a/flight/PiOS/inc/pios_adxl345.h +++ b/flight/PiOS/inc/pios_adxl345.h @@ -16,6 +16,7 @@ #define ADXL_MULTI_BIT 0x40 #define ADXL_X0_ADDR 0x32 +#define ADXL_FIFOSTATUS_ADDR 0x39 #define ADXL_RATE_ADDR 0x2C #define ADXL_RATE_100 0x0A @@ -52,5 +53,6 @@ void PIOS_ADXL345_FifoDepth(uint8_t depth); void PIOS_ADXL345_Attach(uint32_t spi_id); void PIOS_ADXL345_Init(); uint8_t PIOS_ADXL345_Read(struct pios_adxl345_data * data); +uint8_t PIOS_ADXL345_FifoElements(); #endif From f766ba47aac230819027d248813235171fc844ab Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sat, 13 Aug 2011 19:21:30 -0500 Subject: [PATCH 37/55] Increase the size of the eventdispatcher task and lower the maximum queue size. --- flight/CopterControl/System/inc/pios_config.h | 5 ++++- flight/UAVObjects/eventdispatcher.c | 4 ++++ 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/flight/CopterControl/System/inc/pios_config.h b/flight/CopterControl/System/inc/pios_config.h index 944fe38c8..8e09d71ca 100644 --- a/flight/CopterControl/System/inc/pios_config.h +++ b/flight/CopterControl/System/inc/pios_config.h @@ -99,10 +99,13 @@ #define PIOS_SYSTEM_STACK_SIZE 460 #define PIOS_STABILIZATION_STACK_SIZE 524 #define PIOS_TELEM_STACK_SIZE 500 -#define PIOS_EVENTDISPATCHER_STACK_SIZE 96 +#define PIOS_EVENTDISPATCHER_STACK_SIZE 130 #define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 1995998 //#define PIOS_QUATERNION_STABILIZATION +// This can't be too high to stop eventdispatcher thread overflowing +#define PIOS_EVENTDISAPTCHER_QUEUE 10 + #endif /* PIOS_CONFIG_H */ /** * @} diff --git a/flight/UAVObjects/eventdispatcher.c b/flight/UAVObjects/eventdispatcher.c index 2d4bd2082..53a6e039d 100644 --- a/flight/UAVObjects/eventdispatcher.c +++ b/flight/UAVObjects/eventdispatcher.c @@ -27,7 +27,11 @@ #include "openpilot.h" // Private constants +#if defined(PIOS_EVENTDISAPTCHER_QUEUE) +#define MAX_QUEUE_SIZE PIOS_EVENTDISAPTCHER_QUEUE +#else #define MAX_QUEUE_SIZE 20 +#endif #if defined(PIOS_EVENTDISPATCHER_STACK_SIZE) #define STACK_SIZE PIOS_EVENTDISPATCHER_STACK_SIZE From 8a47ade16b6f3772114bbfa7355b70d2630ef223 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 14 Aug 2011 12:47:15 +0200 Subject: [PATCH 38/55] flight/Libraries/fifo_buffer: Use platform independent instead of "stm32xxx.h" to get definition of intX_t types --- flight/Libraries/inc/fifo_buffer.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/Libraries/inc/fifo_buffer.h b/flight/Libraries/inc/fifo_buffer.h index 9058d64c6..6e4955f7e 100644 --- a/flight/Libraries/inc/fifo_buffer.h +++ b/flight/Libraries/inc/fifo_buffer.h @@ -26,7 +26,7 @@ #ifndef _FIFO_BUFFER_H_ #define _FIFO_BUFFER_H_ -#include "stm32f10x.h" +#include // ********************* From 5b240a26753dbe6eb05b128c5831677fb81b04bc Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 14 Aug 2011 12:49:07 +0200 Subject: [PATCH 39/55] PiOS.posix: port pios_udp and pios_com to "next" --- flight/OpenPilot/Makefile.posix | 3 +- .../OpenPilot/System/inc/pios_board_posix.h | 1 + .../OpenPilot/System/inc/pios_config_posix.h | 2 + flight/OpenPilot/System/pios_board_posix.c | 126 +-- flight/PiOS.posix/inc/pios_com.h | 120 +-- flight/PiOS.posix/inc/pios_com_priv.h | 17 +- flight/PiOS.posix/inc/pios_irq.h | 38 + flight/PiOS.posix/inc/pios_udp.h | 16 - flight/PiOS.posix/inc/pios_udp_priv.h | 33 +- flight/PiOS.posix/pios.h | 1 + flight/PiOS.posix/posix/pios_com.c | 744 ++++++++++++------ flight/PiOS.posix/posix/pios_irq.c | 69 ++ flight/PiOS.posix/posix/pios_udp.c | 528 ++++--------- 13 files changed, 921 insertions(+), 777 deletions(-) create mode 100644 flight/PiOS.posix/inc/pios_irq.h create mode 100644 flight/PiOS.posix/posix/pios_irq.c diff --git a/flight/OpenPilot/Makefile.posix b/flight/OpenPilot/Makefile.posix index f93e93644..cfcdbf55f 100644 --- a/flight/OpenPilot/Makefile.posix +++ b/flight/OpenPilot/Makefile.posix @@ -165,6 +165,7 @@ endif SRC += $(PIOSPOSIX)/pios_crc.c SRC += $(PIOSPOSIX)/pios_sys.c SRC += $(PIOSPOSIX)/pios_led.c +SRC += $(PIOSPOSIX)/pios_irq.c SRC += $(PIOSPOSIX)/pios_delay.c SRC += $(PIOSPOSIX)/pios_sdcard.c SRC += $(PIOSPOSIX)/pios_udp.c @@ -176,7 +177,7 @@ SRC += $(PIOSPOSIX)/pios_debug.c SRC += $(PIOSPOSIX)/pios_rcvr.c ## Libraries for flight calculations -#SRC += $(FLIGHTLIB)/fifo_buffer.c +SRC += $(FLIGHTLIB)/fifo_buffer.c SRC += $(FLIGHTLIB)/WorldMagModel.c SRC += $(FLIGHTLIB)/CoordinateConversions.c ## RTOS and RTOS Portable diff --git a/flight/OpenPilot/System/inc/pios_board_posix.h b/flight/OpenPilot/System/inc/pios_board_posix.h index 741f36ba7..e43db8bcb 100644 --- a/flight/OpenPilot/System/inc/pios_board_posix.h +++ b/flight/OpenPilot/System/inc/pios_board_posix.h @@ -52,6 +52,7 @@ //------------------------- //#define PIOS_USART_TX_BUFFER_SIZE 256 #define PIOS_COM_BUFFER_SIZE 1024 +#define PIOS_COM_MAX_DEVS 256 #define PIOS_UDP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE #define PIOS_COM_TELEM_RF 0 diff --git a/flight/OpenPilot/System/inc/pios_config_posix.h b/flight/OpenPilot/System/inc/pios_config_posix.h index 99505c55d..4b779cc49 100644 --- a/flight/OpenPilot/System/inc/pios_config_posix.h +++ b/flight/OpenPilot/System/inc/pios_config_posix.h @@ -36,6 +36,8 @@ #define PIOS_INCLUDE_SDCARD #define PIOS_INCLUDE_FREERTOS #define PIOS_INCLUDE_COM +#define PIOS_INCLUDE_IRQ +#define PIOS_INCLUDE_TELEMETRY_RF #define PIOS_INCLUDE_UDP #define PIOS_INCLUDE_SERVO #define PIOS_INCLUDE_RCVR diff --git a/flight/OpenPilot/System/pios_board_posix.c b/flight/OpenPilot/System/pios_board_posix.c index 07cb6868d..2480d0730 100644 --- a/flight/OpenPilot/System/pios_board_posix.c +++ b/flight/OpenPilot/System/pios_board_posix.c @@ -40,42 +40,16 @@ void Stack_Change() { void Stack_Change_Weak() { } -/** - * PIOS_Board_Init() - * initializes all the core systems on this specific hardware - * called from System/openpilot.c - */ -void PIOS_Board_Init(void) { - /* Delay system */ - PIOS_DELAY_Init(); - - /* Initialize UAVObject libraries */ - EventDispatcherInitialize(); - UAVObjInitialize(); - UAVObjectsInitializeAll(); - - /* Initialize the alarms library */ - AlarmsInitialize(); - - /* Initialize the task monitor library */ - TaskMonitorInitialize(); - - /* Initialize the PiOS library */ - PIOS_COM_Init(); - -} - - -const struct pios_udp_cfg pios_udp0_cfg = { +const struct pios_udp_cfg pios_udp_telem_cfg = { .ip = "0.0.0.0", .port = 9000, }; -const struct pios_udp_cfg pios_udp1_cfg = { +const struct pios_udp_cfg pios_udp_gps_cfg = { .ip = "0.0.0.0", .port = 9001, }; -const struct pios_udp_cfg pios_udp2_cfg = { +const struct pios_udp_cfg pios_udp_debug_cfg = { .ip = "0.0.0.0", .port = 9002, }; @@ -84,15 +58,19 @@ const struct pios_udp_cfg pios_udp2_cfg = { /* * AUX USART */ -const struct pios_udp_cfg pios_udp3_cfg = { +const struct pios_udp_cfg pios_udp_aux_cfg = { .ip = "0.0.0.0", .port = 9003, }; #endif +#define PIOS_COM_TELEM_RF_RX_BUF_LEN 192 +#define PIOS_COM_TELEM_RF_TX_BUF_LEN 192 + /* * Board specific number of devices. */ +/* struct pios_udp_dev pios_udp_devs[] = { #define PIOS_UDP_TELEM 0 { @@ -115,7 +93,7 @@ struct pios_udp_dev pios_udp_devs[] = { }; uint8_t pios_udp_num_devices = NELEMENTS(pios_udp_devs); - +*/ /* * COM devices */ @@ -126,28 +104,72 @@ uint8_t pios_udp_num_devices = NELEMENTS(pios_udp_devs); extern const struct pios_com_driver pios_serial_com_driver; extern const struct pios_com_driver pios_udp_com_driver; -struct pios_com_dev pios_com_devs[] = { - { - .id = PIOS_UDP_TELEM, - .driver = &pios_udp_com_driver, - }, - { - .id = PIOS_UDP_GPS, - .driver = &pios_udp_com_driver, - }, - { - .id = PIOS_UDP_LOCAL, - .driver = &pios_udp_com_driver, - }, -#ifdef PIOS_COM_AUX - { - .id = PIOS_UDP_AUX, - .driver = &pios_udp_com_driver, - }, -#endif -}; +uint32_t pios_com_telem_rf_id; +uint32_t pios_com_telem_usb_id; +uint32_t pios_com_gps_id; +uint32_t pios_com_aux_id; +uint32_t pios_com_spectrum_id; -const uint8_t pios_com_num_devices = NELEMENTS(pios_com_devs); +/** + * PIOS_Board_Init() + * initializes all the core systems on this specific hardware + * called from System/openpilot.c + */ +void PIOS_Board_Init(void) { + + /* Delay system */ + PIOS_DELAY_Init(); + + /* Initialize UAVObject libraries */ + EventDispatcherInitialize(); + UAVObjInitialize(); + UAVObjectsInitializeAll(); + + /* Initialize the alarms library */ + AlarmsInitialize(); + + /* Initialize the task monitor library */ + TaskMonitorInitialize(); + +#if defined(PIOS_INCLUDE_COM) +#if defined(PIOS_INCLUDE_TELEMETRY_RF) + { + uint32_t pios_udp_telem_rf_id; + if (PIOS_UDP_Init(&pios_udp_telem_rf_id, &pios_udp_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_udp_com_driver, pios_udp_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_udp_gps_id; + if (PIOS_USART_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); + PIOS_Assert(rx_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)) { + PIOS_Assert(0); + } + } +#endif /* PIOS_INCLUDE_GPS */ +#endif + + +} /** * @} diff --git a/flight/PiOS.posix/inc/pios_com.h b/flight/PiOS.posix/inc/pios_com.h index 361e820d5..cbee33735 100644 --- a/flight/PiOS.posix/inc/pios_com.h +++ b/flight/PiOS.posix/inc/pios_com.h @@ -1,55 +1,65 @@ -/** - ****************************************************************************** - * - * @file pios_com.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) - * @brief COM layer functions header - * @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_COM_H -#define PIOS_COM_H - -/* Public Functions */ -extern int32_t PIOS_COM_Init(void); -extern int32_t PIOS_COM_ChangeBaud(uint8_t port, uint32_t baud); -extern int32_t PIOS_COM_SendCharNonBlocking(uint8_t port, char c); -extern int32_t PIOS_COM_SendChar(uint8_t port, char c); -extern int32_t PIOS_COM_SendBufferNonBlocking(uint8_t port, uint8_t *buffer, uint16_t len); -extern int32_t PIOS_COM_SendBuffer(uint8_t port, uint8_t *buffer, uint16_t len); -extern int32_t PIOS_COM_SendStringNonBlocking(uint8_t port, char *str); -extern int32_t PIOS_COM_SendString(uint8_t port, char *str); -extern int32_t PIOS_COM_SendFormattedStringNonBlocking(uint8_t port, char *format, ...); -extern int32_t PIOS_COM_SendFormattedString(uint8_t port, char *format, ...); -extern uint8_t PIOS_COM_ReceiveBuffer(uint8_t port); -extern int32_t PIOS_COM_ReceiveBufferUsed(uint8_t port); - -extern int32_t PIOS_COM_ReceiveHandler(void); - -struct pios_com_driver { - void (*init)(uint8_t id); - void (*set_baud)(uint8_t id, uint32_t baud); - int32_t (*tx_nb)(uint8_t id, uint8_t *buffer, uint16_t len); - int32_t (*tx)(uint8_t id, uint8_t *buffer, uint16_t len); - int32_t (*rx)(uint8_t id); - int32_t (*rx_avail)(uint8_t id); -}; - -#endif /* PIOS_COM_H */ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_COM COM layer functions + * @brief Hardware communication layer + * @{ + * + * @file pios_com.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * Parts by Thorsten Klose (tk@midibox.org) + * @brief COM layer functions header + * @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_COM_H +#define PIOS_COM_H + +typedef uint16_t (*pios_com_callback)(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * task_woken); + +struct pios_com_driver { + void (*init)(uint32_t id); + void (*set_baud)(uint32_t id, uint32_t baud); + void (*tx_start)(uint32_t id, uint16_t tx_bytes_avail); + void (*rx_start)(uint32_t id, uint16_t rx_bytes_avail); + void (*bind_rx_cb)(uint32_t id, pios_com_callback rx_in_cb, uint32_t context); + void (*bind_tx_cb)(uint32_t id, pios_com_callback tx_out_cb, uint32_t context); +}; + +/* Public Functions */ +extern int32_t PIOS_COM_Init(uint32_t * com_id, const struct pios_com_driver * driver, uint32_t lower_id, uint8_t * rx_buffer, uint16_t rx_buffer_len, uint8_t * tx_buffer, uint16_t tx_buffer_len); +extern int32_t PIOS_COM_ChangeBaud(uint32_t com_id, uint32_t baud); +extern int32_t PIOS_COM_SendCharNonBlocking(uint32_t com_id, char c); +extern int32_t PIOS_COM_SendChar(uint32_t com_id, char c); +extern int32_t PIOS_COM_SendBufferNonBlocking(uint32_t com_id, const uint8_t *buffer, uint16_t len); +extern int32_t PIOS_COM_SendBuffer(uint32_t com_id, const uint8_t *buffer, uint16_t len); +extern int32_t PIOS_COM_SendStringNonBlocking(uint32_t com_id, const char *str); +extern int32_t PIOS_COM_SendString(uint32_t com_id, const char *str); +extern int32_t PIOS_COM_SendFormattedStringNonBlocking(uint32_t com_id, const char *format, ...); +extern int32_t PIOS_COM_SendFormattedString(uint32_t com_id, const char *format, ...); +extern uint16_t PIOS_COM_ReceiveBuffer(uint32_t com_id, uint8_t * buf, uint16_t buf_len, uint32_t timeout_ms); +extern int32_t PIOS_COM_ReceiveBufferUsed(uint32_t com_id); + +#endif /* PIOS_COM_H */ + +/** + * @} + * @} + */ diff --git a/flight/PiOS.posix/inc/pios_com_priv.h b/flight/PiOS.posix/inc/pios_com_priv.h index 5b285edac..54af82bcb 100644 --- a/flight/PiOS.posix/inc/pios_com_priv.h +++ b/flight/PiOS.posix/inc/pios_com_priv.h @@ -1,5 +1,10 @@ /** ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_COM COM layer functions + * @brief Hardware communication layer + * @{ * * @file pios_com_priv.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. @@ -29,13 +34,11 @@ #include -struct pios_com_dev { - uint8_t id; - const struct pios_com_driver * const driver; -}; - -extern struct pios_com_dev pios_com_devs[]; -extern const uint8_t pios_com_num_devices; +extern int32_t PIOS_COM_ReceiveHandler(uint32_t com_id); #endif /* PIOS_COM_PRIV_H */ +/** + * @} + * @} + */ diff --git a/flight/PiOS.posix/inc/pios_irq.h b/flight/PiOS.posix/inc/pios_irq.h new file mode 100644 index 000000000..f9b9dbb97 --- /dev/null +++ b/flight/PiOS.posix/inc/pios_irq.h @@ -0,0 +1,38 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_IRQ IRQ Setup Functions + * @{ + * + * @file pios_irq.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * Parts by Thorsten Klose (tk@midibox.org) + * @brief IRQ functions header. + * @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_IRQ_H +#define PIOS_IRQ_H + +/* Public Functions */ +extern int32_t PIOS_IRQ_Disable(void); +extern int32_t PIOS_IRQ_Enable(void); + +#endif /* PIOS_IRQ_H */ diff --git a/flight/PiOS.posix/inc/pios_udp.h b/flight/PiOS.posix/inc/pios_udp.h index 7376ee6a1..36964a342 100644 --- a/flight/PiOS.posix/inc/pios_udp.h +++ b/flight/PiOS.posix/inc/pios_udp.h @@ -31,21 +31,5 @@ /* Global Types */ /* Public Functions */ -//extern void PIOS_UDP_Init(void); -void PIOS_UDP_Init(void); -extern void PIOS_UDP_ChangeBaud(uint8_t usart, uint32_t baud); - -extern int32_t PIOS_UDP_RxBufferFree(uint8_t usart); -extern int32_t PIOS_UDP_RxBufferUsed(uint8_t usart); -extern int32_t PIOS_UDP_RxBufferGet(uint8_t usart); -extern int32_t PIOS_UDP_RxBufferPeek(uint8_t usart); -extern int32_t PIOS_UDP_RxBufferPut(uint8_t usart, uint8_t b); - -extern int32_t PIOS_UDP_TxBufferFree(uint8_t usart); -extern int32_t PIOS_UDP_TxBufferGet(uint8_t usart); -extern int32_t PIOS_UDP_TxBufferPutMoreNonBlocking(uint8_t usart, uint8_t *buffer, uint16_t len); -extern int32_t PIOS_UDP_TxBufferPutMore(uint8_t usart, uint8_t *buffer, uint16_t len); -extern int32_t PIOS_UDP_TxBufferPutNonBlocking(uint8_t usart, uint8_t b); -extern int32_t PIOS_UDP_TxBufferPut(uint8_t usart, uint8_t b); #endif /* PIOS_UDP_H */ diff --git a/flight/PiOS.posix/inc/pios_udp_priv.h b/flight/PiOS.posix/inc/pios_udp_priv.h index 8f714cc56..999e192e9 100644 --- a/flight/PiOS.posix/inc/pios_udp_priv.h +++ b/flight/PiOS.posix/inc/pios_udp_priv.h @@ -29,6 +29,7 @@ #include #include +#include #include #include #include @@ -37,30 +38,34 @@ #include #include - - struct pios_udp_cfg { const char * ip; uint16_t port; }; -struct pios_udp_buffer { - uint8_t buf[PIOS_UDP_RX_BUFFER_SIZE]; - uint16_t head; - uint16_t tail; - uint16_t size; -}; +typedef struct { + const struct pios_udp_cfg * cfg; + pthread_t rxThread; -struct pios_udp_dev { - const struct pios_udp_cfg * const cfg; - struct pios_udp_buffer rx; int socket; struct sockaddr_in server; struct sockaddr_in client; uint32_t clientLength; -}; -extern struct pios_udp_dev pios_udp_devs[]; -extern uint8_t pios_udp_num_devices; + pthread_cond_t cond; + pthread_mutex_t mutex; + + pios_com_callback tx_out_cb; + uint32_t tx_out_context; + pios_com_callback rx_in_cb; + uint32_t rx_in_context; + + uint8_t rx_buffer[PIOS_UDP_RX_BUFFER_SIZE]; + uint8_t tx_buffer[PIOS_UDP_RX_BUFFER_SIZE]; +} pios_udp_dev; + +extern int32_t PIOS_UDP_Init(uint32_t * udp_id, const struct pios_udp_cfg * cfg); + + #endif /* PIOS_UDP_PRIV_H */ diff --git a/flight/PiOS.posix/pios.h b/flight/PiOS.posix/pios.h index 12c26c64d..9256891b1 100644 --- a/flight/PiOS.posix/pios.h +++ b/flight/PiOS.posix/pios.h @@ -58,6 +58,7 @@ #include #include #include +#include #include #include #include diff --git a/flight/PiOS.posix/posix/pios_com.c b/flight/PiOS.posix/posix/pios_com.c index a70715f24..76597e247 100644 --- a/flight/PiOS.posix/posix/pios_com.c +++ b/flight/PiOS.posix/posix/pios_com.c @@ -1,285 +1,523 @@ -/** - ****************************************************************************** - * - * @file pios_com.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * Parts by Thorsten Klose (tk@midibox.org) - * @brief COM layer functions - * @see The GNU Public License (GPL) Version 3 - * @defgroup PIOS_COM COM layer functions - * @{ - * - *****************************************************************************/ -/* - * 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 */ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_COM COM layer functions + * @brief Hardware communication layer + * @{ + * + * @file pios_com.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * Parts by Thorsten Klose (tk@midibox.org) + * @brief COM layer 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 + */ + +/* Project Includes */ #include "pios.h" #if defined(PIOS_INCLUDE_COM) +#include "fifo_buffer.h" #include -static struct pios_com_dev * find_com_dev_by_id (uint8_t port) -{ - if (port >= pios_com_num_devices) { - /* Undefined COM port for this board (see pios_board.c) */ - return NULL; - } +#if !defined(PIOS_INCLUDE_FREERTOS) +#include "pios_delay.h" /* PIOS_DELAY_WaitmS */ +#endif - /* Get a handle for the device configuration */ - return &(pios_com_devs[port]); +enum pios_com_dev_magic { + PIOS_COM_DEV_MAGIC = 0xaa55aa55, +}; + +struct pios_com_dev { + enum pios_com_dev_magic magic; + uint32_t lower_id; + const struct pios_com_driver * driver; + +#if defined(PIOS_INCLUDE_FREERTOS) + xSemaphoreHandle tx_sem; + xSemaphoreHandle rx_sem; +#endif + + bool has_rx; + bool has_tx; + + t_fifo_buffer rx; + t_fifo_buffer tx; +}; + +static bool PIOS_COM_validate(struct pios_com_dev * com_dev) +{ + return (com_dev && (com_dev->magic == PIOS_COM_DEV_MAGIC)); } -/** -* Initialises COM layer -* \param[in] mode currently only mode 0 supported -* \return < 0 if initialisation failed -*/ -int32_t PIOS_COM_Init(void) -{ - int32_t ret = 0; - - /* If any COM assignment: */ -#if defined(PIOS_INCLUDE_SERIAL) - PIOS_SERIAL_Init(); -#endif - -#if defined(PIOS_INCLUDE_UDP) - PIOS_UDP_Init(); -#endif - - return ret; -} - -/** -* Change the port speed without re-initializing -* \param[in] port COM port -* \param[in] baud Requested baud rate -* \return -1 if port not available -* \return 0 on success -*/ -int32_t PIOS_COM_ChangeBaud(uint8_t port, uint32_t baud) +#if defined(PIOS_INCLUDE_FREERTOS) && 0 +// 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)); +// if (!com_dev) return (NULL); +// +// com_dev->magic = PIOS_COM_DEV_MAGIC; +// return(com_dev); +//} +#else +static struct pios_com_dev pios_com_devs[PIOS_COM_MAX_DEVS]; +static uint8_t pios_com_num_devs; +static uint32_t PIOS_COM_create(void) { - struct pios_com_dev * com_dev; + struct pios_com_dev * com_dev; - com_dev = find_com_dev_by_id (port); + if (pios_com_num_devs >= PIOS_COM_MAX_DEVS) { + return (PIOS_COM_MAX_DEVS+1); + } - if (!com_dev) { - /* Undefined COM port for this board (see pios_board.c) */ - return -1; - } + com_dev = &pios_com_devs[pios_com_num_devs++]; + com_dev->magic = PIOS_COM_DEV_MAGIC; - /* Invoke the driver function if it exists */ - if (com_dev->driver->set_baud) { - com_dev->driver->set_baud(com_dev->id, baud); - } - - return 0; + return (pios_com_num_devs-1); } - -/** -* Sends a package over given port -* \param[in] port COM port -* \param[in] buffer character buffer -* \param[in] len buffer length -* \return -1 if port not available -* \return -2 if non-blocking mode activated: buffer is full -* caller should retry until buffer is free again -* \return 0 on success -*/ -int32_t PIOS_COM_SendBufferNonBlocking(uint8_t port, uint8_t *buffer, uint16_t len) +static struct pios_com_dev * PIOS_COM_find_dev(uint32_t com_dev_id) { - struct pios_com_dev * com_dev; + if (com_dev_id>pios_com_num_devs) { + return NULL; + } + return &pios_com_devs[com_dev_id]; +} +#endif - com_dev = find_com_dev_by_id (port); +static uint16_t PIOS_COM_TxOutCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield); +static uint16_t PIOS_COM_RxInCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield); +static void PIOS_COM_UnblockRx(struct pios_com_dev * com_dev, bool * need_yield); +static void PIOS_COM_UnblockTx(struct pios_com_dev * com_dev, bool * need_yield); - if (!com_dev) { - /* Undefined COM port for this board (see pios_board.c) */ - return -1; - } +/** + * Initialises COM layer + * \param[out] handle + * \param[in] driver + * \param[in] id + * \return < 0 if initialisation failed + */ +int32_t PIOS_COM_Init(uint32_t * com_id, const struct pios_com_driver * driver, uint32_t lower_id, uint8_t * rx_buffer, uint16_t rx_buffer_len, uint8_t * tx_buffer, uint16_t tx_buffer_len) +{ + PIOS_Assert(com_id); + PIOS_Assert(driver); - /* Invoke the driver function if it exists */ - if (com_dev->driver->tx_nb) { - return com_dev->driver->tx_nb(com_dev->id, buffer, len); - } + bool has_rx = (rx_buffer && rx_buffer_len > 0); + bool has_tx = (tx_buffer && tx_buffer_len > 0); + PIOS_Assert(has_rx || has_tx); + PIOS_Assert(driver->bind_tx_cb || !has_tx); + PIOS_Assert(driver->bind_rx_cb || !has_rx); - return 0; + uint32_t com_dev_id; + struct pios_com_dev * com_dev; + + com_dev_id = PIOS_COM_create(); + com_dev = PIOS_COM_find_dev(com_dev_id); + if (!com_dev) goto out_fail; + + com_dev->driver = driver; + com_dev->lower_id = lower_id; + + com_dev->has_rx = has_rx; + com_dev->has_tx = has_tx; + + if (has_rx) { + fifoBuf_init(&com_dev->rx, rx_buffer, rx_buffer_len); +#if defined(PIOS_INCLUDE_FREERTOS) + vSemaphoreCreateBinary(com_dev->rx_sem); +#endif /* PIOS_INCLUDE_FREERTOS */ + (com_dev->driver->bind_rx_cb)(lower_id, PIOS_COM_RxInCallback, com_dev_id); + if (com_dev->driver->rx_start) { + /* Start the receiver */ + (com_dev->driver->rx_start)(com_dev->lower_id, + fifoBuf_getFree(&com_dev->rx)); + } + } + + if (has_tx) { + fifoBuf_init(&com_dev->tx, tx_buffer, tx_buffer_len); +#if defined(PIOS_INCLUDE_FREERTOS) + vSemaphoreCreateBinary(com_dev->tx_sem); +#endif /* PIOS_INCLUDE_FREERTOS */ + (com_dev->driver->bind_tx_cb)(lower_id, PIOS_COM_TxOutCallback, com_dev_id); + } + + *com_id = com_dev_id; + return(0); + +out_fail: + return(-1); } -/** -* Sends a package over given port -* (blocking function) -* \param[in] port COM port -* \param[in] buffer character buffer -* \param[in] len buffer length -* \return -1 if port not available -* \return 0 on success -*/ -int32_t PIOS_COM_SendBuffer(uint8_t port, uint8_t *buffer, uint16_t len) +static void PIOS_COM_UnblockRx(struct pios_com_dev * com_dev, bool * need_yield) { - struct pios_com_dev * com_dev; +#if defined(PIOS_INCLUDE_FREERTOS) + static signed portBASE_TYPE xHigherPriorityTaskWoken; + xSemaphoreGiveFromISR(com_dev->rx_sem, &xHigherPriorityTaskWoken); - com_dev = find_com_dev_by_id (port); - - if (!com_dev) { - /* Undefined COM port for this board (see pios_board.c) */ - return -1; - } - - /* Invoke the driver function if it exists */ - if (com_dev->driver->tx) { - return com_dev->driver->tx(com_dev->id, buffer, len); - } - - return 0; + if (xHigherPriorityTaskWoken != pdFALSE) { + *need_yield = true; + } else { + *need_yield = false; + } +#else + *need_yield = false; +#endif } -/** -* Sends a single character over given port -* \param[in] port COM port -* \param[in] c character -* \return -1 if port not available -* \return -2 buffer is full -* caller should retry until buffer is free again -* \return 0 on success -*/ -int32_t PIOS_COM_SendCharNonBlocking(uint8_t port, char c) -{ - return PIOS_COM_SendBufferNonBlocking(port, (uint8_t *)&c, 1); -} - -/** -* Sends a single character over given port -* (blocking function) -* \param[in] port COM port -* \param[in] c character -* \return -1 if port not available -* \return 0 on success -*/ -int32_t PIOS_COM_SendChar(uint8_t port, char c) -{ - return PIOS_COM_SendBuffer(port, (uint8_t *)&c, 1); -} - -/** -* Sends a string over given port -* \param[in] port COM port -* \param[in] str zero-terminated string -* \return -1 if port not available -* \return -2 buffer is full -* caller should retry until buffer is free again -* \return 0 on success -*/ -int32_t PIOS_COM_SendStringNonBlocking(uint8_t port, char *str) -{ - return PIOS_COM_SendBufferNonBlocking(port, (uint8_t *)str, (uint16_t)strlen(str)); -} - -/** -* Sends a string over given port -* (blocking function) -* \param[in] port COM port -* \param[in] str zero-terminated string -* \return -1 if port not available -* \return 0 on success -*/ -int32_t PIOS_COM_SendString(uint8_t port, char *str) -{ - return PIOS_COM_SendBuffer(port, (uint8_t *)str, strlen(str)); -} - -/** -* Sends a formatted string (-> printf) over given port -* \param[in] port COM port -* \param[in] *format zero-terminated format string - 128 characters supported maximum! -* \param[in] ... optional arguments, -* 128 characters supported maximum! -* \return -2 if non-blocking mode activated: buffer is full -* caller should retry until buffer is free again -* \return 0 on success -*/ -int32_t PIOS_COM_SendFormattedStringNonBlocking(uint8_t port, char *format, ...) -{ - uint8_t buffer[128]; // TODO: tmp!!! Provide a streamed COM method later! - - va_list args; - - va_start(args, format); - vsprintf((char *)buffer, format, args); - return PIOS_COM_SendBufferNonBlocking(port, buffer, (uint16_t)strlen((char *)buffer)); -} - -/** -* Sends a formatted string (-> printf) over given port -* (blocking function) -* \param[in] port COM port -* \param[in] *format zero-terminated format string - 128 characters supported maximum! -* \param[in] ... optional arguments, -* \return -1 if port not available -* \return 0 on success -*/ -int32_t PIOS_COM_SendFormattedString(uint8_t port, char *format, ...) -{ - uint8_t buffer[128]; // TODO: tmp!!! Provide a streamed COM method later! - va_list args; - - va_start(args, format); - vsprintf((char *)buffer, format, args); - return PIOS_COM_SendBuffer(port, buffer, (uint16_t)strlen((char *)buffer)); -} - -/** -* Transfer bytes from port buffers into another buffer -* \param[in] port COM port +static void PIOS_COM_UnblockTx(struct pios_com_dev * com_dev, bool * need_yield) +{ +#if defined(PIOS_INCLUDE_FREERTOS) + static signed portBASE_TYPE xHigherPriorityTaskWoken; + xSemaphoreGiveFromISR(com_dev->tx_sem, &xHigherPriorityTaskWoken); + + if (xHigherPriorityTaskWoken != pdFALSE) { + *need_yield = true; + } else { + *need_yield = false; + } +#else + *need_yield = false; +#endif +} + +static uint16_t PIOS_COM_RxInCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield) +{ + struct pios_com_dev * com_dev = PIOS_COM_find_dev(context); + + bool valid = PIOS_COM_validate(com_dev); + PIOS_Assert(valid); + PIOS_Assert(com_dev->has_rx); + + PIOS_IRQ_Disable(); + uint16_t bytes_into_fifo = fifoBuf_putData(&com_dev->rx, buf, buf_len); + PIOS_IRQ_Enable(); + + if (bytes_into_fifo > 0) { + /* Data has been added to the buffer */ + PIOS_COM_UnblockRx(com_dev, need_yield); + } + + if (headroom) { + *headroom = fifoBuf_getFree(&com_dev->rx); + } + + return (bytes_into_fifo); +} + +static uint16_t PIOS_COM_TxOutCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield) +{ + struct pios_com_dev * com_dev = PIOS_COM_find_dev(context); + + bool valid = PIOS_COM_validate(com_dev); + PIOS_Assert(valid); + PIOS_Assert(buf); + PIOS_Assert(buf_len); + PIOS_Assert(com_dev->has_tx); + + PIOS_IRQ_Disable(); + uint16_t bytes_from_fifo = fifoBuf_getData(&com_dev->tx, buf, buf_len); + PIOS_IRQ_Enable(); + + if (bytes_from_fifo > 0) { + /* More space has been made in the buffer */ + PIOS_COM_UnblockTx(com_dev, need_yield); + } + + if (headroom) { + *headroom = fifoBuf_getUsed(&com_dev->tx); + } + + return (bytes_from_fifo); +} + +/** +* Change the port speed without re-initializing +* \param[in] port COM port +* \param[in] baud Requested baud rate +* \return -1 if port not available +* \return 0 on success +*/ +int32_t PIOS_COM_ChangeBaud(uint32_t com_id, uint32_t baud) +{ + struct pios_com_dev * com_dev = PIOS_COM_find_dev(com_id); + + if (!PIOS_COM_validate(com_dev)) { + /* Undefined COM port for this board (see pios_board.c) */ + return -1; + } + + /* Invoke the driver function if it exists */ + if (com_dev->driver->set_baud) { + com_dev->driver->set_baud(com_dev->lower_id, baud); + } + + return 0; +} + +/** +* Sends a package over given port +* \param[in] port COM port +* \param[in] buffer character buffer +* \param[in] len buffer length +* \return -1 if port not available +* \return -2 if non-blocking mode activated: buffer is full +* caller should retry until buffer is free again +* \return 0 on success +*/ +int32_t PIOS_COM_SendBufferNonBlocking(uint32_t com_id, const uint8_t *buffer, uint16_t len) +{ + struct pios_com_dev * com_dev = PIOS_COM_find_dev(com_id); + + if (!PIOS_COM_validate(com_dev)) { + /* Undefined COM port for this board (see pios_board.c) */ + return -1; + } + + PIOS_Assert(com_dev->has_tx); + + if (len >= fifoBuf_getFree(&com_dev->tx)) { + /* Buffer cannot accept all requested bytes (retry) */ + return -2; + } + + PIOS_IRQ_Disable(); + uint16_t bytes_into_fifo = fifoBuf_putData(&com_dev->tx, buffer, len); + PIOS_IRQ_Enable(); + + if (bytes_into_fifo > 0) { + /* More data has been put in the tx buffer, make sure the tx is started */ + if (com_dev->driver->tx_start) { + com_dev->driver->tx_start(com_dev->lower_id, + fifoBuf_getUsed(&com_dev->tx)); + } + } + + return (0); +} + +/** +* Sends a package over given port +* (blocking function) +* \param[in] port COM port +* \param[in] buffer character buffer +* \param[in] len buffer length +* \return -1 if port not available +* \return 0 on success +*/ +int32_t PIOS_COM_SendBuffer(uint32_t com_id, const uint8_t *buffer, uint16_t len) +{ + struct pios_com_dev * com_dev = PIOS_COM_find_dev(com_id); + + if (!PIOS_COM_validate(com_dev)) { + /* Undefined COM port for this board (see pios_board.c) */ + return -1; + } + + PIOS_Assert(com_dev->has_tx); + + int32_t rc; + do { + rc = PIOS_COM_SendBufferNonBlocking(com_id, buffer, len); + +#if defined(PIOS_INCLUDE_FREERTOS) + if (rc == -2) { + /* Make sure the transmitter is running while we wait */ + if (com_dev->driver->tx_start) { + (com_dev->driver->tx_start)(com_dev->lower_id, + fifoBuf_getUsed(&com_dev->tx)); + } + if (xSemaphoreTake(com_dev->tx_sem, portMAX_DELAY) != pdTRUE) { + return -3; + } + } +#endif + } while (rc == -2); + + return rc; +} + +/** +* Sends a single character over given port +* \param[in] port COM port +* \param[in] c character +* \return -1 if port not available +* \return -2 buffer is full +* caller should retry until buffer is free again +* \return 0 on success +*/ +int32_t PIOS_COM_SendCharNonBlocking(uint32_t com_id, char c) +{ + return PIOS_COM_SendBufferNonBlocking(com_id, (uint8_t *)&c, 1); +} + +/** +* Sends a single character over given port +* (blocking function) +* \param[in] port COM port +* \param[in] c character +* \return -1 if port not available +* \return 0 on success +*/ +int32_t PIOS_COM_SendChar(uint32_t com_id, char c) +{ + return PIOS_COM_SendBuffer(com_id, (uint8_t *)&c, 1); +} + +/** +* Sends a string over given port +* \param[in] port COM port +* \param[in] str zero-terminated string +* \return -1 if port not available +* \return -2 buffer is full +* caller should retry until buffer is free again +* \return 0 on success +*/ +int32_t PIOS_COM_SendStringNonBlocking(uint32_t com_id, const char *str) +{ + return PIOS_COM_SendBufferNonBlocking(com_id, (uint8_t *)str, (uint16_t)strlen(str)); +} + +/** +* Sends a string over given port +* (blocking function) +* \param[in] port COM port +* \param[in] str zero-terminated string +* \return -1 if port not available +* \return 0 on success +*/ +int32_t PIOS_COM_SendString(uint32_t com_id, const char *str) +{ + return PIOS_COM_SendBuffer(com_id, (uint8_t *)str, strlen(str)); +} + +/** +* Sends a formatted string (-> printf) over given port +* \param[in] port COM port +* \param[in] *format zero-terminated format string - 128 characters supported maximum! +* \param[in] ... optional arguments, +* 128 characters supported maximum! +* \return -2 if non-blocking mode activated: buffer is full +* caller should retry until buffer is free again +* \return 0 on success +*/ +int32_t PIOS_COM_SendFormattedStringNonBlocking(uint32_t com_id, const char *format, ...) +{ + uint8_t buffer[128]; // TODO: tmp!!! Provide a streamed COM method later! + + va_list args; + + va_start(args, format); + vsprintf((char *)buffer, format, args); + return PIOS_COM_SendBufferNonBlocking(com_id, buffer, (uint16_t)strlen((char *)buffer)); +} + +/** +* Sends a formatted string (-> printf) over given port +* (blocking function) +* \param[in] port COM port +* \param[in] *format zero-terminated format string - 128 characters supported maximum! +* \param[in] ... optional arguments, +* \return -1 if port not available +* \return 0 on success +*/ +int32_t PIOS_COM_SendFormattedString(uint32_t com_id, const char *format, ...) +{ + uint8_t buffer[128]; // TODO: tmp!!! Provide a streamed COM method later! + va_list args; + + va_start(args, format); + vsprintf((char *)buffer, format, args); + return PIOS_COM_SendBuffer(com_id, buffer, (uint16_t)strlen((char *)buffer)); +} + +/** +* Transfer bytes from port buffers into another buffer +* \param[in] port COM port * \returns Byte from buffer -*/ -uint8_t PIOS_COM_ReceiveBuffer(uint8_t port) -{ - struct pios_com_dev * com_dev; - - com_dev = find_com_dev_by_id (port); - //PIOS_DEBUG_Assert(com_dev); - //PIOS_DEBUG_Assert(com_dev->driver->rx); - - return com_dev->driver->rx(com_dev->id); -} - -/** -* Get the number of bytes waiting in the buffer -* \param[in] port COM port -* \return Number of bytes used in buffer -*/ -int32_t PIOS_COM_ReceiveBufferUsed(uint8_t port) +*/ +uint16_t PIOS_COM_ReceiveBuffer(uint32_t com_id, uint8_t * buf, uint16_t buf_len, uint32_t timeout_ms) { - struct pios_com_dev * com_dev; + PIOS_Assert(buf); + PIOS_Assert(buf_len); - com_dev = find_com_dev_by_id (port); + struct pios_com_dev * com_dev = PIOS_COM_find_dev(com_id); - if (!com_dev) { - /* Undefined COM port for this board (see pios_board.c) */ - return 0; - } + if (!PIOS_COM_validate(com_dev)) { + /* Undefined COM port for this board (see pios_board.c) */ + PIOS_Assert(0); + } + PIOS_Assert(com_dev->has_rx); - if (!com_dev->driver->rx_avail) { - return 0; - } + check_again: + PIOS_IRQ_Disable(); + uint16_t bytes_from_fifo = fifoBuf_getData(&com_dev->rx, buf, buf_len); + PIOS_IRQ_Enable(); - return com_dev->driver->rx_avail(com_dev->id); + if (bytes_from_fifo == 0 && timeout_ms > 0) { + /* No more bytes in receive buffer */ + /* Make sure the receiver is running while we wait */ + if (com_dev->driver->rx_start) { + /* Notify the lower layer that there is now room in the rx buffer */ + (com_dev->driver->rx_start)(com_dev->lower_id, + fifoBuf_getFree(&com_dev->rx)); + } +#if defined(PIOS_INCLUDE_FREERTOS) + if (xSemaphoreTake(com_dev->rx_sem, timeout_ms / portTICK_RATE_MS) == pdTRUE) { + /* Make sure we don't come back here again */ + timeout_ms = 0; + goto check_again; + } +#else + PIOS_DELAY_WaitmS(1); + timeout_ms--; + goto check_again; +#endif + } + + /* Return received byte */ + return (bytes_from_fifo); } - -#endif + +/** +* Get the number of bytes waiting in the buffer +* \param[in] port COM port +* \return Number of bytes used in buffer +*/ +int32_t PIOS_COM_ReceiveBufferUsed(uint32_t com_id) +{ + struct pios_com_dev * com_dev = PIOS_COM_find_dev(com_id); + + if (!PIOS_COM_validate(com_dev)) { + /* Undefined COM port for this board (see pios_board.c) */ + PIOS_Assert(0); + } + + PIOS_Assert(com_dev->has_rx); + return (fifoBuf_getUsed(&com_dev->rx)); +} + +#endif + +/** + * @} + * @} + */ diff --git a/flight/PiOS.posix/posix/pios_irq.c b/flight/PiOS.posix/posix/pios_irq.c new file mode 100644 index 000000000..b30d09fee --- /dev/null +++ b/flight/PiOS.posix/posix/pios_irq.c @@ -0,0 +1,69 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_IRQ IRQ Setup Functions + * @brief STM32 Hardware code to enable and disable interrupts + * @{ + * + * @file pios_irq.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org) + * @brief IRQ Enable/Disable routines + * @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_IRQ) + +/* Private Function Prototypes */ + +/** +* Disables all interrupts (nested) +* \return < 0 On errors +*/ +int32_t PIOS_IRQ_Disable(void) +{ +#if defined(PIOS_INCLUDE_FREERTOS) + taskENTER_CRITICAL(); +#endif + return 0; +} + +/** +* Enables all interrupts (nested) +* \return < 0 on errors +* \return -1 on nesting errors (PIOS_IRQ_Disable() hasn't been called before) +*/ +int32_t PIOS_IRQ_Enable(void) +{ +#if defined(PIOS_INCLUDE_FREERTOS) + taskEXIT_CRITICAL(); +#endif + return 0; +} + +#endif + +/** + * @} + * @} + */ diff --git a/flight/PiOS.posix/posix/pios_udp.c b/flight/PiOS.posix/posix/pios_udp.c index a7ab90841..7ec6dc309 100644 --- a/flight/PiOS.posix/posix/pios_udp.c +++ b/flight/PiOS.posix/posix/pios_udp.c @@ -34,16 +34,32 @@ #include +/* We need a list of UDP devices */ + +#define PIOS_UDP_MAX_DEV 256 +static int8_t pios_udp_num_devices = 0; + +static pios_udp_dev pios_udp_devices[PIOS_UDP_MAX_DEV]; + + + /* Provide a COM driver */ +static void PIOS_UDP_ChangeBaud(uint32_t udp_id, uint32_t baud); +static void PIOS_UDP_RegisterRxCallback(uint32_t udp_id, pios_com_callback rx_in_cb, uint32_t context); +static void PIOS_UDP_RegisterTxCallback(uint32_t udp_id, pios_com_callback tx_out_cb, uint32_t context); +static void PIOS_UDP_TxStart(uint32_t udp_id, uint16_t tx_bytes_avail); +static void PIOS_UDP_RxStart(uint32_t udp_id, uint16_t rx_bytes_avail); + const struct pios_com_driver pios_udp_com_driver = { - .set_baud = PIOS_UDP_ChangeBaud, - .tx_nb = PIOS_UDP_TxBufferPutMoreNonBlocking, - .tx = PIOS_UDP_TxBufferPutMore, - .rx = PIOS_UDP_RxBufferGet, - .rx_avail = PIOS_UDP_RxBufferUsed, + .set_baud = PIOS_UDP_ChangeBaud, + .tx_start = PIOS_UDP_TxStart, + .rx_start = PIOS_UDP_RxStart, + .bind_tx_cb = PIOS_UDP_RegisterTxCallback, + .bind_rx_cb = PIOS_UDP_RegisterRxCallback, }; -static struct pios_udp_dev * find_udp_dev_by_id (uint8_t udp) + +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) */ @@ -51,410 +67,164 @@ static struct pios_udp_dev * find_udp_dev_by_id (uint8_t udp) } /* Get a handle for the device configuration */ - return &(pios_udp_devs[udp]); + return &(pios_udp_devices[udp]); } /** -* Open some UDP sockets -*/ -void PIOS_UDP_Init(void) -{ - struct pios_udp_dev * udp_dev; - uint8_t i; - - for (i = 0; i < pios_udp_num_devices; i++) { - /* Get a handle for the device configuration */ - udp_dev = find_udp_dev_by_id(i); - //PIOS_DEBUG_Assert(udp_dev); - - /* Clear buffer counters */ - udp_dev->rx.head = udp_dev->rx.tail = udp_dev->rx.size = 0; - - /* assign socket */ - udp_dev->socket = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP); - memset(&udp_dev->server,0,sizeof(udp_dev->server)); - memset(&udp_dev->client,0,sizeof(udp_dev->client)); - udp_dev->server.sin_family = AF_INET; - udp_dev->server.sin_addr.s_addr = inet_addr(udp_dev->cfg->ip); - udp_dev->server.sin_port = htons(udp_dev->cfg->port); - int res= bind(udp_dev->socket, (struct sockaddr *)&udp_dev->server,sizeof(udp_dev->server)); - /* use nonblocking IO */ - int flags = fcntl(udp_dev->socket, F_GETFL, 0); - fcntl(udp_dev->socket, F_SETFL, flags | O_NONBLOCK); - printf("udp dev %i - socket %i opened - result %i\n",i,udp_dev->socket,res); - - /* TODO do some error handling - wait no, we can't - we are void anyway ;) */ - } -} - - -/** -* Changes the baud rate of the UDP peripheral without re-initialising. -* \param[in] udp UDP name (GPS, TELEM, AUX) -* \param[in] baud Requested baud rate -*/ -void PIOS_UDP_ChangeBaud(uint8_t udp, uint32_t baud) -{ -} - - -/** -* puts a byte onto the receive buffer -* \param[in] UDP UDP name -* \param[in] b byte which should be put into Rx buffer -* \return 0 if no error -* \return -1 if UDP not available -* \return -2 if buffer full (retry) -* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions -*/ -int32_t PIOS_UDP_RxBufferPut(uint8_t udp, uint8_t b) -{ - struct pios_udp_dev * udp_dev; - - /* Get a handle for the device configuration */ - udp_dev = find_udp_dev_by_id(udp); - - if (!udp_dev) { - /* Undefined UDP port for this board (see pios_board.c) */ - return -1; - } - - if (udp_dev->rx.size >= sizeof(udp_dev->rx.buf)) { - /* Buffer full (retry) */ - return -2; - } - - /* Copy received byte into receive buffer */ - udp_dev->rx.buf[udp_dev->rx.head++] = b; - if (udp_dev->rx.head >= sizeof(udp_dev->rx.buf)) { - udp_dev->rx.head = 0; - } - udp_dev->rx.size++; - - /* No error */ - return 0; -} - -/** - * attempt to receive + * RxThread */ -void PIOS_UDP_RECV(uint8_t udp) { - - struct pios_udp_dev * udp_dev; - unsigned char localbuffer[PIOS_UDP_RX_BUFFER_SIZE]; - - /* Get a handle for the device configuration */ - udp_dev = find_udp_dev_by_id(udp); - - if (!udp_dev) { - /* Undefined UDP port for this board (see pios_board.c) */ - return; - } - - /* use nonblocking IO */ - int flags = fcntl(udp_dev->socket, F_GETFL, 0); - fcntl(udp_dev->socket, F_SETFL, flags | O_NONBLOCK); - - /* receive data */ - int received; - udp_dev->clientLength=sizeof(udp_dev->client); - if ((received = recvfrom(udp_dev->socket, - localbuffer, - (PIOS_UDP_RX_BUFFER_SIZE - udp_dev->rx.size), - 0, - (struct sockaddr *) &udp_dev->client, - (socklen_t*)&udp_dev->clientLength)) < 0) { - - return; - } - /* copy received data to buffer */ - int t; - for (t=0;tclientLength=sizeof(udp_dev->client); + if ((received = recvfrom(udp_dev->socket, + &udp_dev->rx_buffer, + PIOS_UDP_RX_BUFFER_SIZE, + 0, + (struct sockaddr *) &udp_dev->client, + (socklen_t*)&udp_dev->clientLength)) < 0) { - /* fill buffer */ - PIOS_UDP_RECV(udp); + pthread_exit(NULL); + } - return (sizeof(udp_dev->rx.buf) - udp_dev->rx.size); -} + /* copy received data to buffer if possible */ + /* we do NOT buffer data locally. If the com buffer can't receive, data is discarded! */ + /* (thats what the USART driver does too!) */ + bool rx_need_yield = false; + if (udp_dev->rx_in_cb) { + (void) (udp_dev->rx_in_cb)(udp_dev->rx_in_context, udp_dev->rx_buffer, received, NULL, &rx_need_yield); + } -/** -* Returns number of used bytes in receive buffer -* \param[in] UDP UDP name -* \return > 0: number of used bytes -* \return 0 if UDP not available -* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions -*/ -int32_t PIOS_UDP_RxBufferUsed(uint8_t udp) -{ - struct pios_udp_dev * udp_dev; +#if defined(PIOS_INCLUDE_FREERTOS) + if (rx_need_yield) { + vPortYieldFromISR(); + } +#endif /* PIOS_INCLUDE_FREERTOS */ - /* Get a handle for the device configuration */ - udp_dev = find_udp_dev_by_id(udp); - - if (!udp_dev) { - /* Undefined UDP port for this board (see pios_board.c) */ - return -2; - } - - /* fill buffer */ - PIOS_UDP_RECV(udp); - - return (udp_dev->rx.size); -} - -/** -* Gets a byte from the receive buffer -* \param[in] UDP UDP name -* \return -1 if UDP not available -* \return -2 if no new byte available -* \return >= 0: number of received bytes -* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions -*/ -int32_t PIOS_UDP_RxBufferGet(uint8_t udp) -{ - struct pios_udp_dev * udp_dev; - - /* Get a handle for the device configuration */ - udp_dev = find_udp_dev_by_id(udp); - - if (!udp_dev) { - /* Undefined UDP port for this board (see pios_board.c) */ - return -2; - } - - /* fill buffer */ - PIOS_UDP_RECV(udp); - - if (!udp_dev->rx.size) { - /* Nothing new in the buffer */ - return -1; - } - - /* get byte */ - uint8_t b = udp_dev->rx.buf[udp_dev->rx.tail++]; - if (udp_dev->rx.tail >= sizeof(udp_dev->rx.buf)) { - udp_dev->rx.tail = 0; - } - udp_dev->rx.size--; - - /* Return received byte */ - return b; -} - -/** -* Returns the next byte of the receive buffer without taking it -* \param[in] UDP UDP name -* \return -1 if UDP not available -* \return -2 if no new byte available -* \return >= 0: number of received bytes -* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions -*/ -int32_t PIOS_UDP_RxBufferPeek(uint8_t udp) -{ - struct pios_udp_dev * udp_dev; - - /* Get a handle for the device configuration */ - udp_dev = find_udp_dev_by_id(udp); - - if (!udp_dev) { - /* Undefined UDP port for this board (see pios_board.c) */ - return -2; - } - - /* fill buffer */ - PIOS_UDP_RECV(udp); - - if (!udp_dev->rx.size) { - /* Nothing new in the buffer */ - return -1; - } - - /* get byte */ - uint8_t b = udp_dev->rx.buf[udp_dev->rx.tail]; - - /* Return received byte */ - return b; -} - -/** -* returns number of free bytes in transmit buffer -* \param[in] UDP UDP name -* \return number of free bytes -* \return 0 if UDP not available -* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions -*/ -int32_t PIOS_UDP_TxBufferFree(uint8_t udp) -{ - struct pios_udp_dev * udp_dev; - - /* Get a handle for the device configuration */ - udp_dev = find_udp_dev_by_id(udp); - - if (!udp_dev) { - /* Undefined UDP port for this board (see pios_board.c) */ - return 0; - } - - return PIOS_UDP_RX_BUFFER_SIZE; -} - -/** -* returns number of used bytes in transmit buffer -* \param[in] UDP UDP name -* \return number of used bytes -* \return 0 if UDP not available -* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions -*/ -int32_t PIOS_UDP_TxBufferUsed(uint8_t udp) -{ - struct pios_udp_dev * udp_dev; - - /* Get a handle for the device configuration */ - udp_dev = find_udp_dev_by_id(udp); - - if (!udp_dev) { - /* Undefined UDP port for this board (see pios_board.c) */ - return 0; - } - - return 0; + } } /** -* puts more than one byte onto the transmit buffer (used for atomic sends) -* \param[in] UDP UDP name -* \param[in] *buffer pointer to buffer to be sent -* \param[in] len number of bytes to be sent -* \return 0 if no error -* \return -1 if UDP not available -* \return -2 if buffer full or cannot get all requested bytes (retry) -* \return -3 if UDP not supported by UDPTxBufferPut Routine -* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions +* Open UDP socket */ -int32_t PIOS_UDP_TxBufferPutMoreNonBlocking(uint8_t udp, uint8_t *buffer, uint16_t len) +int32_t PIOS_UDP_Init(uint32_t * udp_id, const struct pios_udp_cfg * cfg) { - struct pios_udp_dev * udp_dev; - /* Get a handle for the device configuration */ - udp_dev = find_udp_dev_by_id(udp); + pios_udp_dev * udp_dev = &pios_udp_devices[pios_udp_num_devices]; - if (!udp_dev) { - /* Undefined UDP port for this board (see pios_board.c) */ - return -1; - } - - if (len >= PIOS_UDP_RX_BUFFER_SIZE) { - /* Buffer cannot accept all requested bytes (retry) */ - return -2; - } - /* send data to client - non blocking*/ - - /* use nonblocking IO */ - int flags = fcntl(udp_dev->socket, F_GETFL, 0); - fcntl(udp_dev->socket, F_SETFL, flags | O_NONBLOCK); - sendto(udp_dev->socket, buffer, len, 0, - (struct sockaddr *) &udp_dev->client, - (socklen_t)sizeof(udp_dev->client)); + pios_udp_num_devices++; - /* No error */ - return 0; + /* initialize */ + udp_dev->rx_in_cb = NULL; + udp_dev->tx_out_cb = NULL; + udp_dev->cfg=cfg; + + /* assign socket */ + udp_dev->socket = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP); + memset(&udp_dev->server,0,sizeof(udp_dev->server)); + memset(&udp_dev->client,0,sizeof(udp_dev->client)); + udp_dev->server.sin_family = AF_INET; + udp_dev->server.sin_addr.s_addr = inet_addr(udp_dev->cfg->ip); + udp_dev->server.sin_port = htons(udp_dev->cfg->port); + int res= bind(udp_dev->socket, (struct sockaddr *)&udp_dev->server,sizeof(udp_dev->server)); + + /* Create transmit thread for this connection */ + pthread_create(&udp_dev->rxThread, NULL, PIOS_UDP_RxThread, (void*)udp_dev); + + printf("udp dev %i - socket %i opened - result %i\n",pios_udp_num_devices-1,udp_dev->socket,res); + + return res; } -/** -* puts more than one byte onto the transmit buffer (used for atomic sends)
-* (blocking function) -* \param[in] UDP UDP name -* \param[in] *buffer pointer to buffer to be sent -* \param[in] len number of bytes to be sent -* \return 0 if no error -* \return -1 if UDP not available -* \return -3 if UDP not supported by UDPTxBufferPut Routine -* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions -*/ -int32_t PIOS_UDP_TxBufferPutMore(uint8_t udp, uint8_t *buffer, uint16_t len) + +void PIOS_UDP_ChangeBaud(uint32_t udp_id, uint32_t baud) { - struct pios_udp_dev * udp_dev; - - /* Get a handle for the device configuration */ - udp_dev = find_udp_dev_by_id(udp); - - if (!udp_dev) { - /* Undefined UDP port for this board (see pios_board.c) */ - return -1; - } - - if (len >= PIOS_UDP_RX_BUFFER_SIZE) { - /* Buffer cannot accept all requested bytes (retry) */ - return -2; - } - - /* send data to client - blocking*/ - /* use blocking IO */ - int flags = fcntl(udp_dev->socket, F_GETFL, 0); - fcntl(udp_dev->socket, F_SETFL, flags & ~O_NONBLOCK); - sendto(udp_dev->socket, buffer, len, 0, - (struct sockaddr *) &udp_dev->client, - sizeof(udp_dev->client)); - - /* No error */ - return 0; + /** + * doesn't apply! + */ } -/** -* puts a byte onto the transmit buffer -* \param[in] UDP UDP name -* \param[in] b byte which should be put into Tx buffer -* \return 0 if no error -* \return -1 if UDP not available -* \return -2 if buffer full (retry) -* \return -3 if UDP not supported by UDPTxBufferPut Routine -* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions -*/ -int32_t PIOS_UDP_TxBufferPut_NonBlocking(uint8_t udp, uint8_t b) + +static void PIOS_UDP_RxStart(uint32_t udp_id, uint16_t rx_bytes_avail) { - return PIOS_UDP_TxBufferPutMoreNonBlocking(udp, &b, 1); + /** + * lazy! + */ } -/** -* puts a byte onto the transmit buffer
-* (blocking function) -* \param[in] UDP UDP name -* \param[in] b byte which should be put into Tx buffer -* \return 0 if no error -* \return -1 if UDP not available -* \return -3 if UDP not supported by UDPTxBufferPut Routine -* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions -*/ -int32_t PIOS_UDP_TxBufferPut(uint8_t udp, uint8_t b) + +static void PIOS_UDP_TxStart(uint32_t udp_id, uint16_t tx_bytes_avail) { - return PIOS_UDP_TxBufferPutMore(udp, &b, 1); + pios_udp_dev * udp_dev = find_udp_dev_by_id(udp_id); + + PIOS_Assert(udp_dev); + + uint16_t length,len,rem; + + /** + * we send everything directly whenever notified of data to send (lazy!) + */ + if (udp_dev->tx_out_cb) { + while (tx_bytes_avail>0) { + bool tx_need_yield = false; + length = (udp_dev->tx_out_cb)(udp_dev->tx_out_context, udp_dev->tx_buffer, PIOS_UDP_RX_BUFFER_SIZE, NULL, &tx_need_yield); + rem = length; + while (rem>0) { + len = sendto(udp_dev->socket, udp_dev->tx_buffer, length, 0, + (struct sockaddr *) &udp_dev->client, + sizeof(udp_dev->client)); + if (!len) return; + rem -= len; + } + tx_bytes_avail -= length; + } + } + } +static void PIOS_UDP_RegisterRxCallback(uint32_t udp_id, pios_com_callback rx_in_cb, uint32_t context) +{ + pios_udp_dev * udp_dev = find_udp_dev_by_id(udp_id); + + PIOS_Assert(udp_dev); + + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + udp_dev->rx_in_context = context; + udp_dev->rx_in_cb = rx_in_cb; +} + +static void PIOS_UDP_RegisterTxCallback(uint32_t udp_id, pios_com_callback tx_out_cb, uint32_t context) +{ + pios_udp_dev * udp_dev = find_udp_dev_by_id(udp_id); + + PIOS_Assert(udp_dev); + + /* + * Order is important in these assignments since ISR uses _cb + * field to determine if it's ok to dereference _cb and _context + */ + udp_dev->tx_out_context = context; + udp_dev->tx_out_cb = tx_out_cb; +} + + + + #endif From b75653a22679141116f0353c3ce1f313920b4b49 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 14 Aug 2011 14:19:15 +0200 Subject: [PATCH 40/55] PiOS.posix: small corrections and bugfixes to make pios_com_udp work correctly. Telemetry works now. --- .../OpenPilot/System/inc/pios_board_posix.h | 16 ++++--- flight/PiOS.posix/posix/pios_com.c | 9 ++-- flight/PiOS.posix/posix/pios_udp.c | 45 ++++++++++++------- 3 files changed, 43 insertions(+), 27 deletions(-) diff --git a/flight/OpenPilot/System/inc/pios_board_posix.h b/flight/OpenPilot/System/inc/pios_board_posix.h index e43db8bcb..fe6f6ba51 100644 --- a/flight/OpenPilot/System/inc/pios_board_posix.h +++ b/flight/OpenPilot/System/inc/pios_board_posix.h @@ -55,13 +55,19 @@ #define PIOS_COM_MAX_DEVS 256 #define PIOS_UDP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE -#define PIOS_COM_TELEM_RF 0 -#define PIOS_COM_GPS 1 -#define PIOS_COM_TELEM_USB 2 +extern uint32_t pios_com_telem_rf_id; +extern uint32_t pios_com_telem_usb_id; +extern uint32_t pios_com_gps_id; +extern uint32_t pios_com_aux_id; +extern uint32_t pios_com_spectrum_id; + +#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) +#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id) +#define PIOS_COM_GPS (pios_com_gps_id) #ifdef PIOS_ENABLE_AUX_UART -#define PIOS_COM_AUX 3 -#define PIOS_COM_DEBUG PIOS_COM_AUX +#define PIOS_COM_AUX (pios_com_aux_id) +#define PIOS_COM_DEBUG (PIOS_COM_AUX #endif /** diff --git a/flight/PiOS.posix/posix/pios_com.c b/flight/PiOS.posix/posix/pios_com.c index 76597e247..77c2e2595 100644 --- a/flight/PiOS.posix/posix/pios_com.c +++ b/flight/PiOS.posix/posix/pios_com.c @@ -92,14 +92,13 @@ static uint32_t PIOS_COM_create(void) com_dev = &pios_com_devs[pios_com_num_devs++]; com_dev->magic = PIOS_COM_DEV_MAGIC; - return (pios_com_num_devs-1); + return (pios_com_num_devs); } static struct pios_com_dev * PIOS_COM_find_dev(uint32_t com_dev_id) { - if (com_dev_id>pios_com_num_devs) { - return NULL; - } - return &pios_com_devs[com_dev_id]; + if (!com_dev_id) return NULL; + if (com_dev_id>pios_com_num_devs+1) return NULL; + return &pios_com_devs[com_dev_id-1]; } #endif diff --git a/flight/PiOS.posix/posix/pios_udp.c b/flight/PiOS.posix/posix/pios_udp.c index 7ec6dc309..39077b455 100644 --- a/flight/PiOS.posix/posix/pios_udp.c +++ b/flight/PiOS.posix/posix/pios_udp.c @@ -32,6 +32,7 @@ #if defined(PIOS_INCLUDE_UDP) +#include #include /* We need a list of UDP devices */ @@ -75,6 +76,12 @@ static pios_udp_dev * find_udp_dev_by_id (uint8_t udp) */ void * PIOS_UDP_RxThread(void * udp_dev_n) { + + /* needed because of FreeRTOS.posix scheduling */ + sigset_t set; + sigfillset(&set); + sigprocmask(SIG_BLOCK, &set, NULL); + pios_udp_dev * udp_dev = (pios_udp_dev*) udp_dev_n; /** @@ -93,25 +100,26 @@ void * PIOS_UDP_RxThread(void * udp_dev_n) PIOS_UDP_RX_BUFFER_SIZE, 0, (struct sockaddr *) &udp_dev->client, - (socklen_t*)&udp_dev->clientLength)) < 0) { + (socklen_t*)&udp_dev->clientLength)) >= 0) + { - pthread_exit(NULL); - } - - /* copy received data to buffer if possible */ - /* we do NOT buffer data locally. If the com buffer can't receive, data is discarded! */ - /* (thats what the USART driver does too!) */ - bool rx_need_yield = false; - if (udp_dev->rx_in_cb) { - (void) (udp_dev->rx_in_cb)(udp_dev->rx_in_context, udp_dev->rx_buffer, received, NULL, &rx_need_yield); - } + /* copy received data to buffer if possible */ + /* we do NOT buffer data locally. If the com buffer can't receive, data is discarded! */ + /* (thats what the USART driver does too!) */ + bool rx_need_yield = false; + if (udp_dev->rx_in_cb) { + (void) (udp_dev->rx_in_cb)(udp_dev->rx_in_context, udp_dev->rx_buffer, received, NULL, &rx_need_yield); + } #if defined(PIOS_INCLUDE_FREERTOS) - if (rx_need_yield) { - vPortYieldFromISR(); - } + if (rx_need_yield) { + vPortYieldFromISR(); + } #endif /* PIOS_INCLUDE_FREERTOS */ + } + + } } @@ -172,7 +180,7 @@ static void PIOS_UDP_TxStart(uint32_t udp_id, uint16_t tx_bytes_avail) PIOS_Assert(udp_dev); - uint16_t length,len,rem; + int32_t length,len,rem; /** * we send everything directly whenever notified of data to send (lazy!) @@ -186,8 +194,11 @@ static void PIOS_UDP_TxStart(uint32_t udp_id, uint16_t tx_bytes_avail) len = sendto(udp_dev->socket, udp_dev->tx_buffer, length, 0, (struct sockaddr *) &udp_dev->client, sizeof(udp_dev->client)); - if (!len) return; - rem -= len; + if (len<=0) { + rem=0; + } else { + rem -= len; + } } tx_bytes_avail -= length; } From ba58ec7e08a668fbea403612ec2d9095cd10ae57 Mon Sep 17 00:00:00 2001 From: Oleg Semyonov Date: Sun, 14 Aug 2011 14:07:24 +0300 Subject: [PATCH 41/55] gcs: (uploader plugin) replace red warning icon with less serious yellow triangle --- .../src/plugins/uploader/images/warning.svg | 317 ++++++++++++++++-- 1 file changed, 286 insertions(+), 31 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/uploader/images/warning.svg b/ground/openpilotgcs/src/plugins/uploader/images/warning.svg index 8de454769..697b80a56 100644 --- a/ground/openpilotgcs/src/plugins/uploader/images/warning.svg +++ b/ground/openpilotgcs/src/plugins/uploader/images/warning.svg @@ -1,35 +1,290 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + image/svg+xml + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - From 2f7f920ef327b9416db29688229d178e74a9d373 Mon Sep 17 00:00:00 2001 From: Oleg Semyonov Date: Sun, 14 Aug 2011 14:28:42 +0300 Subject: [PATCH 42/55] gcs: (uploader plugin) few firmware info string and icon changes Also it looks like the date check clause doesn't work, needs to be checked. --- .../src/plugins/uploader/devicewidget.cpp | 26 +++++++------- .../src/plugins/uploader/devicewidget.ui | 10 +++--- .../src/plugins/uploader/images/error.svg | 35 +++++++++++++++++++ .../plugins/uploader/runningdevicewidget.cpp | 4 +-- .../src/plugins/uploader/uploader.qrc | 1 + 5 files changed, 56 insertions(+), 20 deletions(-) create mode 100644 ground/openpilotgcs/src/plugins/uploader/images/error.svg diff --git a/ground/openpilotgcs/src/plugins/uploader/devicewidget.cpp b/ground/openpilotgcs/src/plugins/uploader/devicewidget.cpp index e2d2c0148..e2300bdcd 100644 --- a/ground/openpilotgcs/src/plugins/uploader/devicewidget.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/devicewidget.cpp @@ -189,7 +189,7 @@ bool deviceWidget::populateBoardStructuredDescription(QByteArray desc) myDevice->lblDescription->setText(QString("Firmware tag: ")+onBoardDescrition.description); QPixmap pix = QPixmap(QString(":uploader/images/application-certificate.svg")); myDevice->lblCertified->setPixmap(pix); - myDevice->lblCertified->setToolTip(tr("Official Firmware Build")); + myDevice->lblCertified->setToolTip(tr("Tagged officially released firmware build")); } else @@ -197,7 +197,7 @@ bool deviceWidget::populateBoardStructuredDescription(QByteArray desc) myDevice->lblDescription->setText(onBoardDescrition.description); QPixmap pix = QPixmap(QString(":uploader/images/warning.svg")); myDevice->lblCertified->setPixmap(pix); - myDevice->lblCertified->setToolTip(tr("Custom Firmware Build")); + myDevice->lblCertified->setToolTip(tr("Untagged or custom firmware build")); } myDevice->lblBrdName->setText(idToBoardName(onBoardDescrition.boardType<<8)); @@ -220,7 +220,7 @@ bool deviceWidget::populateLoadedStructuredDescription(QByteArray desc) myDevice->description->setText(LoadedDescrition.description); QPixmap pix = QPixmap(QString(":uploader/images/application-certificate.svg")); myDevice->lblCertifiedL->setPixmap(pix); - myDevice->lblCertifiedL->setToolTip(tr("Official Firmware Build")); + myDevice->lblCertifiedL->setToolTip(tr("Tagged officially released firmware build")); } else { @@ -228,7 +228,7 @@ bool deviceWidget::populateLoadedStructuredDescription(QByteArray desc) myDevice->description->setText(LoadedDescrition.description); QPixmap pix = QPixmap(QString(":uploader/images/warning.svg")); myDevice->lblCertifiedL->setPixmap(pix); - myDevice->lblCertifiedL->setToolTip(tr("Custom Firmware Build")); + myDevice->lblCertifiedL->setToolTip(tr("Untagged or custom firmware build")); } myDevice->lblBrdNameL->setText(deviceDescriptorStruct::idToBoardName(LoadedDescrition.boardType<<8)); @@ -312,34 +312,34 @@ void deviceWidget::loadFirmware() myDevice->groupCustom->setVisible(false); if(myDevice->lblCRC->text()==myDevice->lblCRCL->text()) { - myDevice->statusLabel->setText(tr("The loaded firmware the same as on the board. You should not upload it")); + myDevice->statusLabel->setText(tr("The board has the same firmware as loaded. No need to update")); px.load(QString(":/uploader/images/warning.svg")); } else if(myDevice->lblDevName->text()!=myDevice->lblBrdNameL->text()) { - myDevice->statusLabel->setText(tr("The loaded firmware is not suited for the HW connected. You shouldn't upload it")); - px.load(QString(":/uploader/images/warning.svg")); + myDevice->statusLabel->setText(tr("WARNING: the loaded firmware is for different hardware. Do not update!")); + px.load(QString(":/uploader/images/error.svg")); } else if(QDateTime::fromString(onBoardDescrition.buildDate)>QDateTime::fromString(LoadedDescrition.buildDate)) { - myDevice->statusLabel->setText(tr("The loaded firmware is older than the firmware on the board. You should not upload it")); + myDevice->statusLabel->setText(tr("The board has newer firmware than loaded. Are you sure you want to update?")); px.load(QString(":/uploader/images/warning.svg")); } else if(!LoadedDescrition.description.startsWith("release",Qt::CaseInsensitive)) { - myDevice->statusLabel->setText(tr("The loaded firmware is not an oficial OpenPilot release. You should upload it only if you know what you are doing")); + myDevice->statusLabel->setText(tr("The loaded firmware is untagged or custom build. Update only if it was received from a trusted source (official website or your own build)")); px.load(QString(":/uploader/images/warning.svg")); } else { - myDevice->statusLabel->setText(tr("Everything seems OK. You should upload the loaded firmware by pressing 'Flash'")); + myDevice->statusLabel->setText(tr("This is the tagged officially released OpenPilot firmware")); px.load(QString(":/uploader/images/gtk-info.svg")); } } else { - myDevice->statusLabel->setText(tr("The loaded firmware was not packaged with the OpenPilot format. You should not upload it unless you know what you are doing.")); - px.load(QString(":/uploader/images/warning.svg")); + myDevice->statusLabel->setText(tr("WARNING: the loaded firmware was not packaged with the OpenPilot format. Do not update unless you know what you are doing")); + px.load(QString(":/uploader/images/error.svg")); myDevice->youdont->setChecked(false); myDevice->youdont->setVisible(true); myDevice->verticalGroupBox_loaded->setVisible(false); @@ -514,7 +514,7 @@ QString deviceWidget::setOpenFileName() QString fileName = QFileDialog::getOpenFileName(this, tr("Select firmware file"), "", - tr("Firmware Files (*.bin *.opfw)"), + tr("Firmware Files (*.opfw *.bin)"), &selectedFilter, options); return fileName; diff --git a/ground/openpilotgcs/src/plugins/uploader/devicewidget.ui b/ground/openpilotgcs/src/plugins/uploader/devicewidget.ui index 6f7ed132f..c5c7a807e 100644 --- a/ground/openpilotgcs/src/plugins/uploader/devicewidget.ui +++ b/ground/openpilotgcs/src/plugins/uploader/devicewidget.ui @@ -93,10 +93,10 @@ - Loads the firmware + Open a file with new firmware image to be flashed - Load... + Open... @@ -116,7 +116,7 @@ false - Update the firmware on this board. + Write loaded firmware image to the board Flash @@ -126,7 +126,7 @@ - Download the current board firmware to your computer + Read and save current board firmware to a file Retrieve... @@ -228,7 +228,7 @@ - GIT Tag: + Git tag: Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter diff --git a/ground/openpilotgcs/src/plugins/uploader/images/error.svg b/ground/openpilotgcs/src/plugins/uploader/images/error.svg new file mode 100644 index 000000000..8de454769 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/uploader/images/error.svg @@ -0,0 +1,35 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.cpp b/ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.cpp index 0db22efb0..2cb513344 100644 --- a/ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.cpp +++ b/ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.cpp @@ -117,7 +117,7 @@ void runningDeviceWidget::populate() myDevice->lblFWTag->setText(QString("Firmware tag: ")+devDesc.description); QPixmap pix = QPixmap(QString(":uploader/images/application-certificate.svg")); myDevice->lblCertified->setPixmap(pix); - myDevice->lblCertified->setToolTip(tr("Official Firmware Build")); + myDevice->lblCertified->setToolTip(tr("Tagged officially released firmware build")); } else @@ -125,7 +125,7 @@ void runningDeviceWidget::populate() myDevice->lblFWTag->setText(QString("Firmware tag: ")+devDesc.description); QPixmap pix = QPixmap(QString(":uploader/images/warning.svg")); myDevice->lblCertified->setPixmap(pix); - myDevice->lblCertified->setToolTip(tr("Custom Firmware Build")); + myDevice->lblCertified->setToolTip(tr("Untagged or custom firmware build")); } myDevice->lblGitCommitTag->setText("Git commit tag: "+devDesc.gitTag); myDevice->lblFWDate->setText(QString("Firmware date: ") + devDesc.buildDate); diff --git a/ground/openpilotgcs/src/plugins/uploader/uploader.qrc b/ground/openpilotgcs/src/plugins/uploader/uploader.qrc index f51ea265c..7d2677927 100644 --- a/ground/openpilotgcs/src/plugins/uploader/uploader.qrc +++ b/ground/openpilotgcs/src/plugins/uploader/uploader.qrc @@ -11,5 +11,6 @@ images/deviceID-0101.svg images/application-certificate.svg images/warning.svg + images/error.svg From 2020e336d60448e164813e5557a400f299760a1b Mon Sep 17 00:00:00 2001 From: Oleg Semyonov Date: Tue, 16 Aug 2011 20:58:39 +0300 Subject: [PATCH 43/55] 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 b8d47381d2dd4012502cbff1c3321d14d15b46c7 Mon Sep 17 00:00:00 2001 From: Oleg Semyonov Date: Wed, 17 Aug 2011 17:47:17 +0300 Subject: [PATCH 44/55] 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 45/55] 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 46/55] 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 47/55] 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 48/55] 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 49/55] 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 50/55] 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 51/55] 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 52/55] 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 53/55] 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 54/55] 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 55/55] 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.