From 55b325966a185a4e63dfe3a19024220a2e1107bf Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Tue, 8 Jul 2014 02:47:09 +0200 Subject: [PATCH 01/26] OP-1395 - Pack several objects on a single Debug Log "slot", after ten consecutive failures (log full) gives up trying to save to prevent wasting time. Use PIOS_Delay for timestamp --- flight/pios/common/pios_debuglog.c | 125 ++++++++++++++----- shared/uavobjectdefinition/debuglogentry.xml | 4 +- 2 files changed, 93 insertions(+), 36 deletions(-) diff --git a/flight/pios/common/pios_debuglog.c b/flight/pios/common/pios_debuglog.c index 97cb1d462..05681fc79 100644 --- a/flight/pios/common/pios_debuglog.c +++ b/flight/pios/common/pios_debuglog.c @@ -49,6 +49,9 @@ static xSemaphoreHandle mutex = 0; #endif static bool logging_enabled = false; +#define MAX_CONSECUTIVE_FAILS_COUNT 10 +static bool log_is_full = false; +static uint8_t fails_count = 0; static uint16_t flightnum = 0; static uint16_t lognum = 0; static DebugLogEntryData *buffer = 0; @@ -56,8 +59,16 @@ static DebugLogEntryData *buffer = 0; static DebugLogEntryData staticbuffer; #endif -/* Private Function Prototypes */ +#define LOG_ENTRY_MAX_DATA_SIZE (sizeof(((DebugLogEntryData *)0)->Data)) +#define LOG_ENTRY_HEADER_SIZE (sizeof(DebugLogEntryData) - LOG_ENTRY_MAX_DATA_SIZE) +// build the obj_id as a DEBUGLOGENTRY ID with least significant byte zeroed and filled with flight number +#define LOG_GET_FLIGHT_OBJID(x) ((DEBUGLOGENTRY_OBJID & ~0xFF) | (x & 0xFF)) +static uint32_t used_buffer_space = 0; + +/* Private Function Prototypes */ +static void enqueue_data(uint32_t objid, uint16_t instid, size_t size, uint8_t *data); +static bool write_current_buffer(); /** * @brief Initialize the log facility */ @@ -75,9 +86,12 @@ void PIOS_DEBUGLOG_Initialize() return; } mutexlock(); - lognum = 0; - flightnum = 0; - while (PIOS_FLASHFS_ObjLoad(pios_user_fs_id, flightnum * 256, lognum, (uint8_t *)buffer, sizeof(DebugLogEntryData)) == 0) { + lognum = 0; + flightnum = 0; + fails_count = 0; + used_buffer_space = 0; + log_is_full = false; + while (PIOS_FLASHFS_ObjLoad(pios_user_fs_id, LOG_GET_FLIGHT_OBJID(flightnum), lognum, (uint8_t *)buffer, sizeof(DebugLogEntryData)) == 0) { flightnum++; } mutexunlock(); @@ -103,30 +117,13 @@ void PIOS_DEBUGLOG_Enable(uint8_t enabled) */ void PIOS_DEBUGLOG_UAVObject(uint32_t objid, uint16_t instid, size_t size, uint8_t *data) { - if (!logging_enabled || !buffer) { + if (!logging_enabled || !buffer || log_is_full) { return; } mutexlock(); - buffer->Flight = flightnum; -#if defined(PIOS_INCLUDE_FREERTOS) - buffer->FlightTime = xTaskGetTickCount() * portTICK_RATE_MS; -#else - buffer->FlightTime = 0; -#endif - buffer->Entry = lognum; - buffer->Type = DEBUGLOGENTRY_TYPE_UAVOBJECT; - buffer->ObjectID = objid; - buffer->InstanceID = instid; - if (size > sizeof(buffer->Data)) { - size = sizeof(buffer->Data); - } - buffer->Size = size; - memset(buffer->Data, 0xff, sizeof(buffer->Data)); - memcpy(buffer->Data, data, size); - if (PIOS_FLASHFS_ObjSave(pios_user_fs_id, flightnum * 256, lognum, (uint8_t *)buffer, sizeof(DebugLogEntryData)) == 0) { - lognum++; - } + enqueue_data(objid, instid, size, data); + mutexunlock(); } /** @@ -137,27 +134,30 @@ void PIOS_DEBUGLOG_UAVObject(uint32_t objid, uint16_t instid, size_t size, uint8 */ void PIOS_DEBUGLOG_Printf(char *format, ...) { - if (!logging_enabled || !buffer) { + if (!logging_enabled || !buffer || log_is_full) { return; } + va_list args; va_start(args, format); mutexlock(); + // flush any pending buffer before writing debug text + if (used_buffer_space) { + write_current_buffer(); + } memset(buffer->Data, 0xff, sizeof(buffer->Data)); vsnprintf((char *)buffer->Data, sizeof(buffer->Data), (char *)format, args); buffer->Flight = flightnum; -#if defined(PIOS_INCLUDE_FREERTOS) - buffer->FlightTime = xTaskGetTickCount() * portTICK_RATE_MS; -#else - buffer->FlightTime = 0; -#endif + + buffer->FlightTime = PIOS_DELAY_GetuS(); + buffer->Entry = lognum; buffer->Type = DEBUGLOGENTRY_TYPE_TEXT; buffer->ObjectID = 0; buffer->InstanceID = 0; buffer->Size = strlen((const char *)buffer->Data); - if (PIOS_FLASHFS_ObjSave(pios_user_fs_id, flightnum * 256, lognum, (uint8_t *)buffer, sizeof(DebugLogEntryData)) == 0) { + if (PIOS_FLASHFS_ObjSave(pios_user_fs_id, LOG_GET_FLIGHT_OBJID(flightnum), lognum, (uint8_t *)buffer, sizeof(DebugLogEntryData)) == 0) { lognum++; } mutexunlock(); @@ -179,7 +179,7 @@ void PIOS_DEBUGLOG_Printf(char *format, ...) int32_t PIOS_DEBUGLOG_Read(void *mybuffer, uint16_t flight, uint16_t inst) { PIOS_Assert(mybuffer); - return PIOS_FLASHFS_ObjLoad(pios_user_fs_id, flight * 256, inst, (uint8_t *)mybuffer, sizeof(DebugLogEntryData)); + return PIOS_FLASHFS_ObjLoad(pios_user_fs_id, LOG_GET_FLIGHT_OBJID(flight), inst, (uint8_t *)mybuffer, sizeof(DebugLogEntryData)); } /** @@ -214,11 +214,68 @@ void PIOS_DEBUGLOG_Format(void) { mutexlock(); PIOS_FLASHFS_Format(pios_user_fs_id); - lognum = 0; - flightnum = 0; + lognum = 0; + flightnum = 0; + log_is_full = false; + fails_count = 0; + used_buffer_space = 0; mutexunlock(); } +void enqueue_data(uint32_t objid, uint16_t instid, size_t size, uint8_t *data) +{ + DebugLogEntryData *entry; + + // start a new block + if (!used_buffer_space) { + entry = buffer; + memset(buffer->Data, 0xff, sizeof(buffer->Data)); + used_buffer_space += size; + } else { + // if an instance is being filled and there is enough space, does enqueues new data. + if (used_buffer_space + size + LOG_ENTRY_HEADER_SIZE > LOG_ENTRY_MAX_DATA_SIZE) { + buffer->Type = DEBUGLOGENTRY_TYPE_MULTIPLEUAVOBJECTS; + if (!write_current_buffer()) { + return; + } + entry = buffer; + memset(buffer->Data, 0xff, sizeof(buffer->Data)); + used_buffer_space += size; + } else { + entry = (DebugLogEntryData *)&buffer->Data[used_buffer_space]; + used_buffer_space += size + LOG_ENTRY_HEADER_SIZE; + } + } + + entry->Flight = flightnum; + entry->FlightTime = PIOS_DELAY_GetuS(); + entry->Entry = lognum; + entry->Type = DEBUGLOGENTRY_TYPE_UAVOBJECT; + entry->ObjectID = objid; + entry->InstanceID = instid; + if (size > sizeof(buffer->Data)) { + size = sizeof(buffer->Data); + } + entry->Size = size; + + memcpy(entry->Data, data, size); +} + +bool write_current_buffer() +{ + // not enough space, write the block and start a new one + if (PIOS_FLASHFS_ObjSave(pios_user_fs_id, LOG_GET_FLIGHT_OBJID(flightnum), lognum, (uint8_t *)buffer, sizeof(DebugLogEntryData)) == 0) { + lognum++; + fails_count = 0; + used_buffer_space = 0; + } else { + if (fails_count++ > MAX_CONSECUTIVE_FAILS_COUNT) { + log_is_full = true; + } + return false; + } + return true; +} /** * @} * @} diff --git a/shared/uavobjectdefinition/debuglogentry.xml b/shared/uavobjectdefinition/debuglogentry.xml index 9a9acda83..8628fd73e 100644 --- a/shared/uavobjectdefinition/debuglogentry.xml +++ b/shared/uavobjectdefinition/debuglogentry.xml @@ -2,9 +2,9 @@ Log Entry in Flash - + - + From 80169e998f5b2dbfac6f2d8d867014dff488096f Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Tue, 8 Jul 2014 02:51:50 +0200 Subject: [PATCH 02/26] OP-1395 - Handles and unpack multiple uavos in single Debug Log Slot, default interval set to 500ms, --- .../src/plugins/flightlog/FlightLogDialog.qml | 15 ++++--- .../plugins/flightlog/flightlogmanager.cpp | 42 +++++++++++++++---- .../src/plugins/flightlog/flightlogmanager.h | 5 ++- .../src/plugins/flightlog/functions.js | 6 +++ 4 files changed, 54 insertions(+), 14 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/flightlog/FlightLogDialog.qml b/ground/openpilotgcs/src/plugins/flightlog/FlightLogDialog.qml index 9c3c730c7..115aa7b7f 100644 --- a/ground/openpilotgcs/src/plugins/flightlog/FlightLogDialog.qml +++ b/ground/openpilotgcs/src/plugins/flightlog/FlightLogDialog.qml @@ -7,7 +7,7 @@ import org.openpilot 1.0 import "functions.js" as Functions Rectangle { - width: 600 + width: 700 height: 400 id: root ColumnLayout { @@ -59,11 +59,11 @@ Rectangle { delegate: Text { verticalAlignment: Text.AlignVCenter - text: Functions.millisToTime(styleData.value) + text: Functions.microsToTime(styleData.value) } } TableViewColumn { - role: "Type"; title: "Type"; width: 60; + role: "Type"; title: "Type"; width: 50; delegate: Text { verticalAlignment: Text.AlignVCenter @@ -72,6 +72,7 @@ Rectangle { case 0 : text: qsTr("Empty"); break; case 1 : text: qsTr("Text"); break; case 2 : text: qsTr("UAVO"); break; + case 3 : text: qsTr("UAVO(P)"); break; default: text: qsTr("Unknown"); break; } } @@ -96,10 +97,14 @@ Rectangle { text: "" + qsTr("Flights recorded: ") + "" + (logStatus.Flight + 1) } Text { - id: totalEntries - text: "" + qsTr("Entries logged (free): ") + "" + + id: totalSlots + text: "" + qsTr("Slot used (free): ") + "" + logStatus.UsedSlots + " (" + logStatus.FreeSlots + ")" } + Text { + id: totalEntries + text: "" + qsTr("Entries logged: ") + "" + logManager.uavoEntries.count + } Rectangle { Layout.fillHeight: true } diff --git a/ground/openpilotgcs/src/plugins/flightlog/flightlogmanager.cpp b/ground/openpilotgcs/src/plugins/flightlog/flightlogmanager.cpp index c0dd6f633..c43aa8fc3 100644 --- a/ground/openpilotgcs/src/plugins/flightlog/flightlogmanager.cpp +++ b/ground/openpilotgcs/src/plugins/flightlog/flightlogmanager.cpp @@ -205,21 +205,47 @@ void FlightLogManager::retrieveLogs(int flightToRetrieve) for (int flight = startFlight; flight <= endFlight; flight++) { m_flightLogControl->setFlight(flight); bool gotLast = false; - int entry = 0; + int slot = 0; while (!gotLast) { // Send request for loading flight entry on flight side and wait for ack/nack - m_flightLogControl->setEntry(entry); + m_flightLogControl->setEntry(slot); if (updateHelper.doObjectAndWait(m_flightLogControl, UAVTALK_TIMEOUT) == UAVObjectUpdaterHelper::SUCCESS && requestHelper.doObjectAndWait(m_flightLogEntry, UAVTALK_TIMEOUT) == UAVObjectUpdaterHelper::SUCCESS) { if (m_flightLogEntry->getType() != DebugLogEntry::TYPE_EMPTY) { // Ok, we retrieved the entry, and it was the correct one. clone it and add it to the list ExtendedDebugLogEntry *logEntry = new ExtendedDebugLogEntry(); + logEntry->setData(m_flightLogEntry->getData(), m_objectManager); m_logEntries << logEntry; + if (logEntry->getData().Type == DebugLogEntry::TYPE_MULTIPLEUAVOBJECTS) { + const quint32 total_len = sizeof(DebugLogEntry::DataFields); + const quint32 data_len = sizeof(((DebugLogEntry::DataFields *)0)->Data); + const quint32 header_len = total_len - data_len; + + DebugLogEntry::DataFields fields; + quint32 start = logEntry->getData().Size; + + // cycle until there is space for another object + while (start + header_len + 1 < data_len) { + memset(&fields, 0xFF, total_len); + memcpy(&fields, &logEntry->getData().Data[start], header_len); + // check wether a packed object is found + // note that empty data blocks are set as 0xFF in flight side to minimize flash wearing + // thus as soon as this read outside of used area, the test will fail as lenght would be 0xFFFF + quint32 toread = header_len + fields.Size; + if (!(toread + start > data_len)) { + memcpy(&fields, &logEntry->getData().Data[start], toread); + ExtendedDebugLogEntry *subEntry = new ExtendedDebugLogEntry(); + subEntry->setData(fields, m_objectManager); + m_logEntries << subEntry; + } + start += toread; + } + } // Increment to get next entry from flight side - entry++; + slot++; } else { // We are done, not more entries on this flight gotLast = true; @@ -280,7 +306,7 @@ void FlightLogManager::exportToOPL(QString fileName) ExtendedDebugLogEntry *entry = m_logEntries[currentEntry]; // Only log uavobjects - if (entry->getType() == ExtendedDebugLogEntry::TYPE_UAVOBJECT) { + if (entry->getType() == ExtendedDebugLogEntry::TYPE_UAVOBJECT || entry->getType() == ExtendedDebugLogEntry::TYPE_MULTIPLEUAVOBJECTS) { // Set timestamp that should be logged for this entry logFile.setNextTimeStamp(entry->getFlightTime() - adjustedBaseTime); @@ -615,7 +641,7 @@ QString ExtendedDebugLogEntry::getLogString() { if (getType() == DebugLogEntry::TYPE_TEXT) { return QString((const char *)getData().Data); - } else if (getType() == DebugLogEntry::TYPE_UAVOBJECT) { + } else if (getType() == DebugLogEntry::TYPE_UAVOBJECT || getType() == DebugLogEntry::TYPE_MULTIPLEUAVOBJECTS) { return m_object->toString().replace("\n", " ").replace("\t", " "); } else { return ""; @@ -631,7 +657,7 @@ void ExtendedDebugLogEntry::toXML(QXmlStreamWriter *xmlWriter, quint32 baseTime) if (getType() == DebugLogEntry::TYPE_TEXT) { xmlWriter->writeAttribute("type", "text"); xmlWriter->writeTextElement("message", QString((const char *)getData().Data)); - } else if (getType() == DebugLogEntry::TYPE_UAVOBJECT) { + } else if (getType() == DebugLogEntry::TYPE_UAVOBJECT || getType() == DebugLogEntry::TYPE_MULTIPLEUAVOBJECTS) { xmlWriter->writeAttribute("type", "uavobject"); m_object->toXML(xmlWriter); } @@ -644,7 +670,7 @@ void ExtendedDebugLogEntry::toCSV(QTextStream *csvStream, quint32 baseTime) if (getType() == DebugLogEntry::TYPE_TEXT) { data = QString((const char *)getData().Data); - } else if (getType() == DebugLogEntry::TYPE_UAVOBJECT) { + } else if (getType() == DebugLogEntry::TYPE_UAVOBJECT || getType() == DebugLogEntry::TYPE_MULTIPLEUAVOBJECTS) { data = m_object->toString().replace("\n", "").replace("\t", ""); } *csvStream << QString::number(getFlight() + 1) << '\t' << QString::number(getFlightTime() - baseTime) << '\t' << QString::number(getEntry()) << '\t' << data << '\n'; @@ -654,7 +680,7 @@ void ExtendedDebugLogEntry::setData(const DebugLogEntry::DataFields &data, UAVOb { DebugLogEntry::setData(data); - if (getType() == DebugLogEntry::TYPE_UAVOBJECT) { + if (getType() == DebugLogEntry::TYPE_UAVOBJECT || getType() == DebugLogEntry::TYPE_MULTIPLEUAVOBJECTS) { UAVDataObject *object = (UAVDataObject *)objectManager->getObject(getObjectID(), getInstanceID()); Q_ASSERT(object); m_object = object->clone(getInstanceID()); diff --git a/ground/openpilotgcs/src/plugins/flightlog/flightlogmanager.h b/ground/openpilotgcs/src/plugins/flightlog/flightlogmanager.h index 7fcf4c4d9..58d62954e 100644 --- a/ground/openpilotgcs/src/plugins/flightlog/flightlogmanager.h +++ b/ground/openpilotgcs/src/plugins/flightlog/flightlogmanager.h @@ -94,6 +94,10 @@ public slots: setDirty(true); if (m_setting != 1 && m_setting != 3) { setPeriod(0); + } else { + if (!period()) { + setPeriod(500); + } } emit settingChanged(setting); } @@ -240,7 +244,6 @@ public: { return m_loggingEnabled; } - signals: void logEntriesChanged(); void flightEntriesChanged(); diff --git a/ground/openpilotgcs/src/plugins/flightlog/functions.js b/ground/openpilotgcs/src/plugins/flightlog/functions.js index 47884432b..83b5c9e82 100644 --- a/ground/openpilotgcs/src/plugins/flightlog/functions.js +++ b/ground/openpilotgcs/src/plugins/flightlog/functions.js @@ -11,6 +11,12 @@ function millisToTime(ms) { return pad(hours, 2) + ":" + pad(minutes, 2) + ":" + pad(seconds, 2) + ":" + pad(msleft, 3); } + +function microsToTime(us) { + var ms = Math.floor(us / 1000); + return millisToTime(ms); +} + function pad(number, length) { var str = '' + number; while (str.length < length) { From 711c3609b65d3676b2b941dedeb1ce08b359ab4d Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Tue, 8 Jul 2014 02:53:25 +0200 Subject: [PATCH 03/26] OP-1395 - Add support for Fast read, explicitly set SPI clock speed to maximum allowed by all parts used --- flight/pios/common/pios_flash_jedec.c | 58 +++++++++++++--------- flight/pios/inc/pios_flash_jedec_catalog.h | 24 ++++++--- flight/pios/inc/pios_flash_jedec_priv.h | 12 +++-- 3 files changed, 58 insertions(+), 36 deletions(-) diff --git a/flight/pios/common/pios_flash_jedec.c b/flight/pios/common/pios_flash_jedec.c index 9b1ceeee2..74a768184 100644 --- a/flight/pios/common/pios_flash_jedec.c +++ b/flight/pios/common/pios_flash_jedec.c @@ -75,13 +75,16 @@ struct jedec_flash_dev { enum pios_jedec_dev_magic magic; }; +#define FLASH_FAST_PRESCALER PIOS_SPI_PRESCALER_2 +#define FLASH_PRESCALER PIOS_SPI_PRESCALER_4 + // ! Private functions static int32_t PIOS_Flash_Jedec_Validate(struct jedec_flash_dev *flash_dev); static struct jedec_flash_dev *PIOS_Flash_Jedec_alloc(void); static int32_t PIOS_Flash_Jedec_ReadID(struct jedec_flash_dev *flash_dev); static int32_t PIOS_Flash_Jedec_ReadStatus(struct jedec_flash_dev *flash_dev); -static int32_t PIOS_Flash_Jedec_ClaimBus(struct jedec_flash_dev *flash_dev); +static int32_t PIOS_Flash_Jedec_ClaimBus(struct jedec_flash_dev *flash_dev, bool fast); static int32_t PIOS_Flash_Jedec_ReleaseBus(struct jedec_flash_dev *flash_dev); static int32_t PIOS_Flash_Jedec_WriteEnable(struct jedec_flash_dev *flash_dev); static int32_t PIOS_Flash_Jedec_Busy(struct jedec_flash_dev *flash_dev); @@ -166,12 +169,12 @@ int32_t PIOS_Flash_Jedec_Init(uintptr_t *flash_id, uint32_t spi_id, uint32_t sla * @brief Claim the SPI bus for flash use and assert CS pin * @return 0 for sucess, -1 for failure to get semaphore */ -static int32_t PIOS_Flash_Jedec_ClaimBus(struct jedec_flash_dev *flash_dev) +static int32_t PIOS_Flash_Jedec_ClaimBus(struct jedec_flash_dev *flash_dev, bool fast) { if (PIOS_SPI_ClaimBus(flash_dev->spi_id) < 0) { return -1; } - + PIOS_SPI_SetClockSpeed(flash_dev->spi_id, fast ? FLASH_FAST_PRESCALER : FLASH_PRESCALER); PIOS_SPI_RC_PinSet(flash_dev->spi_id, flash_dev->slave_num, 0); flash_dev->claimed = true; @@ -209,7 +212,7 @@ static int32_t PIOS_Flash_Jedec_Busy(struct jedec_flash_dev *flash_dev) */ static int32_t PIOS_Flash_Jedec_WriteEnable(struct jedec_flash_dev *flash_dev) { - if (PIOS_Flash_Jedec_ClaimBus(flash_dev) != 0) { + if (PIOS_Flash_Jedec_ClaimBus(flash_dev, true) != 0) { return -1; } @@ -226,7 +229,7 @@ static int32_t PIOS_Flash_Jedec_WriteEnable(struct jedec_flash_dev *flash_dev) */ static int32_t PIOS_Flash_Jedec_ReadStatus(struct jedec_flash_dev *flash_dev) { - if (PIOS_Flash_Jedec_ClaimBus(flash_dev) < 0) { + if (PIOS_Flash_Jedec_ClaimBus(flash_dev, true) < 0) { return -1; } @@ -247,7 +250,7 @@ static int32_t PIOS_Flash_Jedec_ReadStatus(struct jedec_flash_dev *flash_dev) */ static int32_t PIOS_Flash_Jedec_ReadID(struct jedec_flash_dev *flash_dev) { - if (PIOS_Flash_Jedec_ClaimBus(flash_dev) < 0) { + if (PIOS_Flash_Jedec_ClaimBus(flash_dev, true) < 0) { return -2; } @@ -354,7 +357,7 @@ static int32_t PIOS_Flash_Jedec_EraseSector(uintptr_t flash_id, uint32_t addr) return ret; } - if (PIOS_Flash_Jedec_ClaimBus(flash_dev) != 0) { + if (PIOS_Flash_Jedec_ClaimBus(flash_dev, true) != 0) { return -1; } @@ -368,7 +371,7 @@ static int32_t PIOS_Flash_Jedec_EraseSector(uintptr_t flash_id, uint32_t addr) // Keep polling when bus is busy too while (PIOS_Flash_Jedec_Busy(flash_dev) != 0) { #if defined(FLASH_FREERTOS) - vTaskDelay(1); + vTaskDelay(2); #endif } @@ -394,7 +397,7 @@ static int32_t PIOS_Flash_Jedec_EraseChip(uintptr_t flash_id) return ret; } - if (PIOS_Flash_Jedec_ClaimBus(flash_dev) != 0) { + if (PIOS_Flash_Jedec_ClaimBus(flash_dev, true) != 0) { return -1; } @@ -455,16 +458,14 @@ static int32_t PIOS_Flash_Jedec_WriteData(uintptr_t flash_id, uint32_t addr, uin if (((addr & 0xff) + len) > 0x100) { return -3; } - if ((ret = PIOS_Flash_Jedec_WriteEnable(flash_dev)) != 0) { return ret; } /* Execute write page command and clock in address. Keep CS asserted */ - if (PIOS_Flash_Jedec_ClaimBus(flash_dev) != 0) { + if (PIOS_Flash_Jedec_ClaimBus(flash_dev, true) != 0) { return -1; } - if (PIOS_SPI_TransferBlock(flash_dev->spi_id, out, NULL, sizeof(out), NULL) < 0) { PIOS_Flash_Jedec_ReleaseBus(flash_dev); return -1; @@ -486,7 +487,7 @@ static int32_t PIOS_Flash_Jedec_WriteData(uintptr_t flash_id, uint32_t addr, uin #else // Query status this way to prevent accel chip locking us out - if (PIOS_Flash_Jedec_ClaimBus(flash_dev) < 0) { + if (PIOS_Flash_Jedec_ClaimBus(flash_dev, true) < 0) { return -1; } @@ -536,13 +537,12 @@ static int32_t PIOS_Flash_Jedec_WriteChunks(uintptr_t flash_id, uint32_t addr, s if (((addr & 0xff) + len) > 0x100) { return -3; } - if ((ret = PIOS_Flash_Jedec_WriteEnable(flash_dev)) != 0) { return ret; } /* Execute write page command and clock in address. Keep CS asserted */ - if (PIOS_Flash_Jedec_ClaimBus(flash_dev) != 0) { + if (PIOS_Flash_Jedec_ClaimBus(flash_dev, true) != 0) { return -1; } @@ -582,17 +582,29 @@ static int32_t PIOS_Flash_Jedec_ReadData(uintptr_t flash_id, uint32_t addr, uint if (PIOS_Flash_Jedec_Validate(flash_dev) != 0) { return -1; } - - if (PIOS_Flash_Jedec_ClaimBus(flash_dev) == -1) { + bool fast_read = flash_dev->cfg->fast_read != 0; + if (PIOS_Flash_Jedec_ClaimBus(flash_dev, fast_read) == -1) { return -1; } - /* Execute read command and clock in address. Keep CS asserted */ - uint8_t out[] = { JEDEC_READ_DATA, (addr >> 16) & 0xff, (addr >> 8) & 0xff, addr & 0xff }; - - if (PIOS_SPI_TransferBlock(flash_dev->spi_id, out, NULL, sizeof(out), NULL) < 0) { - PIOS_Flash_Jedec_ReleaseBus(flash_dev); - return -2; + if (!fast_read) { + uint8_t out[] = { JEDEC_READ_DATA, (addr >> 16) & 0xff, (addr >> 8) & 0xff, addr & 0xff }; + if (PIOS_SPI_TransferBlock(flash_dev->spi_id, out, NULL, sizeof(out), NULL) < 0) { + PIOS_Flash_Jedec_ReleaseBus(flash_dev); + return -2; + } + } else { + uint8_t cmdlen = flash_dev->cfg->fast_read_dummy_bytes + 4; + uint8_t out[cmdlen]; + memset(out, 0x0, cmdlen); + out[0] = flash_dev->cfg->fast_read; + out[1] = (addr >> 16) & 0xff; + out[2] = (addr >> 8) & 0xff; + out[3] = addr & 0xff; + if (PIOS_SPI_TransferBlock(flash_dev->spi_id, out, NULL, cmdlen, NULL) < 0) { + PIOS_Flash_Jedec_ReleaseBus(flash_dev); + return -2; + } } /* Copy the transfer data to the buffer */ diff --git a/flight/pios/inc/pios_flash_jedec_catalog.h b/flight/pios/inc/pios_flash_jedec_catalog.h index cb15fa8b9..32e5a70c8 100644 --- a/flight/pios/inc/pios_flash_jedec_catalog.h +++ b/flight/pios/inc/pios_flash_jedec_catalog.h @@ -40,29 +40,37 @@ const struct pios_flash_jedec_cfg pios_flash_jedec_catalog[] = .expect_manufacturer = JEDEC_MANUFACTURER_ST, .expect_memorytype = 0x20, .expect_capacity = 0x15, - .sector_erase = 0xD8, - .chip_erase = 0xC7, + .sector_erase = 0xD8, + .chip_erase = 0xC7, + .fast_read = 0x0B, + .fast_read_dummy_bytes = 1, }, { // m25px16 .expect_manufacturer = JEDEC_MANUFACTURER_ST, .expect_memorytype = 0x71, .expect_capacity = 0x15, - .sector_erase = 0xD8, - .chip_erase = 0xC7, + .sector_erase = 0xD8, + .chip_erase = 0xC7, + .fast_read = 0x0B, + .fast_read_dummy_bytes = 1, }, { // w25x .expect_manufacturer = JEDEC_MANUFACTURER_WINBOND, .expect_memorytype = 0x30, .expect_capacity = 0x13, - .sector_erase = 0x20, - .chip_erase = 0x60 + .sector_erase = 0x20, + .chip_erase = 0x60, + .fast_read = 0x0B, + .fast_read_dummy_bytes = 1, }, { // 25q16 .expect_manufacturer = JEDEC_MANUFACTURER_WINBOND, .expect_memorytype = 0x40, .expect_capacity = 0x15, - .sector_erase = 0x20, - .chip_erase = 0x60 + .sector_erase = 0x20, + .chip_erase = 0x60, + .fast_read = 0x0B, + .fast_read_dummy_bytes = 1, } }; const uint32_t pios_flash_jedec_catalog_size = NELEMENTS(pios_flash_jedec_catalog); diff --git a/flight/pios/inc/pios_flash_jedec_priv.h b/flight/pios/inc/pios_flash_jedec_priv.h index fb021ee3c..6d1d3353d 100644 --- a/flight/pios/inc/pios_flash_jedec_priv.h +++ b/flight/pios/inc/pios_flash_jedec_priv.h @@ -40,11 +40,13 @@ extern const struct pios_flash_driver pios_jedec_flash_driver; #define JEDEC_MANUFACTURER_WINBOND 0xEF struct pios_flash_jedec_cfg { - uint8_t expect_manufacturer; - uint8_t expect_memorytype; - uint8_t expect_capacity; - uint32_t sector_erase; - uint32_t chip_erase; + uint8_t expect_manufacturer; + uint8_t expect_memorytype; + uint8_t expect_capacity; + uint8_t sector_erase; + uint8_t chip_erase; + uint8_t fast_read; + uint8_t fast_read_dummy_bytes; }; int32_t PIOS_Flash_Jedec_Init(uintptr_t *flash_id, uint32_t spi_id, uint32_t slave_num); From 03d03cfb06260df258d6c4a94da5bc8839cdf9e1 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Tue, 8 Jul 2014 02:54:17 +0200 Subject: [PATCH 04/26] OP-1395 add a vTaskDelay when waiting for dma transfer to complete when using FreeRTOS. --- flight/pios/stm32f4xx/pios_spi.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/flight/pios/stm32f4xx/pios_spi.c b/flight/pios/stm32f4xx/pios_spi.c index 09ad51f98..889b885a9 100644 --- a/flight/pios/stm32f4xx/pios_spi.c +++ b/flight/pios/stm32f4xx/pios_spi.c @@ -585,6 +585,9 @@ static int32_t SPI_DMA_TransferBlock(uint32_t spi_id, const uint8_t *send_buffer /* Wait until all bytes have been transmitted/received */ while (DMA_GetCurrDataCounter(spi_dev->cfg->dma.rx.channel)) { +#if defined(PIOS_INCLUDE_FREERTOS) + vTaskDelay(0); +#endif ; } From 61d26d7fd8a9947502c42d24c95f64a20cdd6e0a Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Wed, 9 Jul 2014 08:39:32 +0200 Subject: [PATCH 05/26] op-1395 - automatically increment flight number after disabling log. --- flight/pios/common/pios_debuglog.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/flight/pios/common/pios_debuglog.c b/flight/pios/common/pios_debuglog.c index 05681fc79..4ac83a757 100644 --- a/flight/pios/common/pios_debuglog.c +++ b/flight/pios/common/pios_debuglog.c @@ -104,6 +104,10 @@ void PIOS_DEBUGLOG_Initialize() */ void PIOS_DEBUGLOG_Enable(uint8_t enabled) { + // increase the flight num as soon as logging is disabled + if (!logging_enabled && enabled) { + flightnum++; + } logging_enabled = enabled; } From dc11ff51b1e3048235339c3dc3df13c4b3ffdfb3 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Wed, 9 Jul 2014 08:40:31 +0200 Subject: [PATCH 06/26] OP-1395 - fix typo in Flight log dialog --- ground/openpilotgcs/src/plugins/flightlog/FlightLogDialog.qml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ground/openpilotgcs/src/plugins/flightlog/FlightLogDialog.qml b/ground/openpilotgcs/src/plugins/flightlog/FlightLogDialog.qml index 115aa7b7f..494dc6ea2 100644 --- a/ground/openpilotgcs/src/plugins/flightlog/FlightLogDialog.qml +++ b/ground/openpilotgcs/src/plugins/flightlog/FlightLogDialog.qml @@ -98,7 +98,7 @@ Rectangle { } Text { id: totalSlots - text: "" + qsTr("Slot used (free): ") + "" + + text: "" + qsTr("Slots used (free): ") + "" + logStatus.UsedSlots + " (" + logStatus.FreeSlots + ")" } Text { From e53be4f8ec54b2864edc76bac5ec2f1a2315dfc1 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Wed, 9 Jul 2014 19:49:14 +0200 Subject: [PATCH 07/26] OP-1395 - Fix log entries downloaded counter in dialog --- .../openpilotgcs/src/plugins/flightlog/FlightLogDialog.qml | 2 +- ground/openpilotgcs/src/plugins/flightlog/flightlogmanager.h | 5 ++++- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/flightlog/FlightLogDialog.qml b/ground/openpilotgcs/src/plugins/flightlog/FlightLogDialog.qml index 494dc6ea2..18b842159 100644 --- a/ground/openpilotgcs/src/plugins/flightlog/FlightLogDialog.qml +++ b/ground/openpilotgcs/src/plugins/flightlog/FlightLogDialog.qml @@ -103,7 +103,7 @@ Rectangle { } Text { id: totalEntries - text: "" + qsTr("Entries logged: ") + "" + logManager.uavoEntries.count + text: "" + qsTr("Entries downloaded: ") + "" + logManager.logEntriesCount } Rectangle { Layout.fillHeight: true diff --git a/ground/openpilotgcs/src/plugins/flightlog/flightlogmanager.h b/ground/openpilotgcs/src/plugins/flightlog/flightlogmanager.h index 58d62954e..80147b1ab 100644 --- a/ground/openpilotgcs/src/plugins/flightlog/flightlogmanager.h +++ b/ground/openpilotgcs/src/plugins/flightlog/flightlogmanager.h @@ -182,7 +182,7 @@ class FlightLogManager : public QObject { Q_PROPERTY(QStringList logSettings READ logSettings NOTIFY logSettingsChanged) Q_PROPERTY(QStringList logStatuses READ logStatuses NOTIFY logStatusesChanged) Q_PROPERTY(int loggingEnabled READ loggingEnabled WRITE setLoggingEnabled NOTIFY loggingEnabledChanged) - + Q_PROPERTY(int logEntriesCount READ logEntriesCount NOTIFY logEntriesChanged) public: explicit FlightLogManager(QObject *parent = 0); @@ -244,6 +244,9 @@ public: { return m_loggingEnabled; } + int logEntriesCount(){ + return m_logEntries.count(); + } signals: void logEntriesChanged(); void flightEntriesChanged(); From 6c38eda19227161c417c29e882968669c20edff3 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Wed, 9 Jul 2014 23:05:38 +0200 Subject: [PATCH 08/26] OP-1395 - Reset log num when changing flight num. fix flight num update logic --- flight/pios/common/pios_debuglog.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/flight/pios/common/pios_debuglog.c b/flight/pios/common/pios_debuglog.c index 4ac83a757..f4549a399 100644 --- a/flight/pios/common/pios_debuglog.c +++ b/flight/pios/common/pios_debuglog.c @@ -105,8 +105,9 @@ void PIOS_DEBUGLOG_Initialize() void PIOS_DEBUGLOG_Enable(uint8_t enabled) { // increase the flight num as soon as logging is disabled - if (!logging_enabled && enabled) { + if (logging_enabled && !enabled) { flightnum++; + lognum = 0; } logging_enabled = enabled; } From c01047920adb9ff869681fa84fcdd4a74f00e79d Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Wed, 9 Jul 2014 23:06:14 +0200 Subject: [PATCH 09/26] OP-1395 - Remove Qt::Dialog flag as it is misbehaving in linux --- ground/openpilotgcs/src/plugins/flightlog/flightlogplugin.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/ground/openpilotgcs/src/plugins/flightlog/flightlogplugin.cpp b/ground/openpilotgcs/src/plugins/flightlog/flightlogplugin.cpp index 07c5df7bf..68b043d86 100644 --- a/ground/openpilotgcs/src/plugins/flightlog/flightlogplugin.cpp +++ b/ground/openpilotgcs/src/plugins/flightlog/flightlogplugin.cpp @@ -86,7 +86,6 @@ void FlightLogPlugin::ShowLogManagementDialog() m_logDialog->rootContext()->setContextProperty("logDialog", m_logDialog); m_logDialog->setResizeMode(QQuickView::SizeRootObjectToView); m_logDialog->setSource(QUrl("qrc:/flightlog/FlightLogDialog.qml")); - m_logDialog->setFlags(Qt::Dialog); m_logDialog->setModality(Qt::ApplicationModal); connect(m_logDialog, SIGNAL(destroyed()), this, SLOT(LogManagementDialogClosed())); } From ca12105527ace72bc7397bb0d4b8493f48154d93 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Thu, 10 Jul 2014 22:05:12 +0200 Subject: [PATCH 10/26] OP-1395 - missing uncrustification --- ground/openpilotgcs/src/plugins/flightlog/flightlogmanager.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ground/openpilotgcs/src/plugins/flightlog/flightlogmanager.h b/ground/openpilotgcs/src/plugins/flightlog/flightlogmanager.h index 80147b1ab..0c93d014b 100644 --- a/ground/openpilotgcs/src/plugins/flightlog/flightlogmanager.h +++ b/ground/openpilotgcs/src/plugins/flightlog/flightlogmanager.h @@ -244,7 +244,8 @@ public: { return m_loggingEnabled; } - int logEntriesCount(){ + int logEntriesCount() + { return m_logEntries.count(); } signals: From fd307479e810b4ab006a825eaae716a734f3dbd1 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Thu, 10 Jul 2014 22:12:25 +0200 Subject: [PATCH 11/26] OP-1395 - Change labels for better readability remove trailing spaces from translated strings --- .../src/plugins/flightlog/FlightLogDialog.qml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/flightlog/FlightLogDialog.qml b/ground/openpilotgcs/src/plugins/flightlog/FlightLogDialog.qml index 18b842159..a24a28551 100644 --- a/ground/openpilotgcs/src/plugins/flightlog/FlightLogDialog.qml +++ b/ground/openpilotgcs/src/plugins/flightlog/FlightLogDialog.qml @@ -94,16 +94,16 @@ Rectangle { spacing: 10 Text { id: totalFlights - text: "" + qsTr("Flights recorded: ") + "" + (logStatus.Flight + 1) + text: "" + qsTr("Flights recorded:") + " " + (logStatus.Flight + 1) } Text { id: totalSlots - text: "" + qsTr("Slots used (free): ") + "" + - logStatus.UsedSlots + " (" + logStatus.FreeSlots + ")" + text: "" + qsTr("Slots used/free:") + " " + + logStatus.UsedSlots + "/" + logStatus.FreeSlots } Text { id: totalEntries - text: "" + qsTr("Entries downloaded: ") + "" + logManager.logEntriesCount + text: "" + qsTr("Entries downloaded:") + " " + logManager.logEntriesCount } Rectangle { Layout.fillHeight: true From 6d096cad4ad840178e916e614e8d90e4420eec6d Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Sun, 27 Jul 2014 12:43:03 +0200 Subject: [PATCH 12/26] OP-1337 French_translations_updates : Calibration help text again --- .../translations/openpilotgcs_fr.ts | 59 +++++++++++-------- 1 file changed, 34 insertions(+), 25 deletions(-) diff --git a/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_fr.ts b/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_fr.ts index 6d0ef3f96..37b91ba9a 100644 --- a/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_fr.ts +++ b/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_fr.ts @@ -7091,6 +7091,7 @@ Une valeur de 0.00 désactive le filtre. p, li { white-space: pre-wrap; } </style></head><body style=" font-family:'MS Shell Dlg 2'; font-size:10pt; font-weight:400; font-style:normal;"> <p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-weight:600;"><br /></p></body></html> + Blank text ? @@ -7138,34 +7139,38 @@ p, li { white-space: pre-wrap; } <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> <html><head><meta name="qrichtext" content="1" /><style type="text/css"> p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'Ubuntu'; font-size:9pt; font-weight:400; font-style:normal;"> -<table border="0" style="-qt-table-type: root; margin-top:4px; margin-bottom:4px; margin-left:4px; margin-right:4px;"> -<tr> -<td style="border: none;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:16pt; font-weight:600; font-style:italic;">Aide</span></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt;">Les étapes #1, #2 et #3 sont nécessaires. L'étape #4 vous aidera à atteindre les meilleurs résultats possibles.</span></p> +</style></head><body style=" font-family:'MS Shell Dlg 2'; font-size:8.25pt; font-weight:400; font-style:normal;"> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:14pt; font-weight:600;">Aide</span></p> <p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:11pt;"><br /></p> -<p align="center" style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'MS Shell Dlg 2'; font-size:8pt;"><br /></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:14pt; font-weight:600; font-style:italic;">#1 : Étalonnage multipoints</span></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt;">Cet étalonnage va calculer les échelles pour les capteurs Accéléromètres ou Magnétomètre. </span></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt;">Cliquer sur &quot;Étalonner Accéléromètre&quot; ou &quot;Étalonner Magnétomètre&quot; pour démarrer l'étalonnage puis suivez les instructions qui seront affichées ici. </span></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt;">Pour un calibrage optimal effectuez l'étalonnage des Accéléromètres sans monter la carte sur l'appareil, de cette façon vous pourrez précisément la mettre de niveau sur votre bureau/table pendant le processus. </span></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt;">Le calibrage du magnétomètre devra être effectué avec la carte montée dans votre appareil pour tenir compte des champs magnétiques/ masses métalliques à bord.</span></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt; font-weight:600; font-style:italic;">Note 1:</span><span style=" font-size:11pt;"> Votre </span><span style=" font-size:11pt; font-weight:600;">PositionHome doit être définie en premier</span><span style=" font-size:11pt;">, ainsi que le vecteur de champ magnétique (Be) et l'accélération due à la gravité (g_e).</span></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt; font-weight:600; font-style:italic;">Note 2:</span><span style=" font-size:11pt;"> Vous n'avez pas besoin de pointer exactement dans les directions Sud/Nord/Ouest/Est. Ceci est simplement utilisé pour indiquer clairement à l'utilisateur comment orienter son appareil.</span></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt;">Vous pouvez simplement admettre que le Nord est face à vous, l'Est à votre droite, etc. et effectuer l'étalonnage de cette façon.</span></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt;">Les étapes 1, 2 et 3 sont nécessaires</span></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt;">L'étape 4 est optionnelle mais vous aidera à atteindre les meilleurs résultats possibles.</span></p> <p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:11pt;"><br /></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:14pt; font-weight:600; font-style:italic;">#2 : Étalonnage du niveau de la carte</span></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt;">Cette étape fera en sorte que la mise de niveau de la carte soit parfaite. Veuillez placer l'appareil le plus horizontalement possible (utilisez un niveau à bulle si nécessaire), puis appuyez sur Démarrer ci-dessus et ne bougez pas du tout votre appareil jusqu'à la fin de l'étalonnage.</span></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:12pt; font-weight:600; font-style:italic;">Étape 1 : Étalonnage des Accéléromètres et Magnétomètre</span></p> <p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:11pt;"><br /></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:14pt; font-weight:600; font-style:italic;">#3 : Étalonnage de l'ajustement des Gyroscopes</span></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt;">Cette étape permet d'étalonner la valeur mesurée sur les gyros lorsque la carte ne bouge pas. Pour effectuer l'étalonnage laissez la carte/l'appareil sans bouger et cliquez sur Démarrer. </span></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt;">Cet étape va étalonner les échelles des capteurs Accéléromètres ou Magnétomètre. </span></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt;">Appuyez sur </span><span style=" font-size:11pt; font-style:italic;">Démarrer</span><span style=" font-size:11pt;"> pour commencer et suivez les instructions pour chaque étape. </span></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt;">Pour de meilleurs résultats avec l'étalonnage des accéléromètres, il est conseillé de le réaliser avec le contrôleur de vol démonté, ce qui permet de positionner avec précision la carte pour chaque orientation de la séquence.</span></p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:11pt;"><br /></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt;">L'étalonnage du magnétomètre devra être effectué avec la carte montée sur votre appareil pour tenir compte des déviations produites par les champs magnétiques / masses métalliques à bord.</span></p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:11pt;"><br /></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt;">Note 1 : Avant de débuter l'étalonnage du magnétomètre ou de des accéléromètres votre Position Home devra être réglée. Cela est nécessaire pour déterminer le vecteur de champ magnétique (Be) et l'accélération due à la gravité (g_e) de votre position.</span></p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:11pt;"><br /></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt;">Note 2 : Vous n'avez pas besoin d'orienter votre appareil exactement vers le sud, nord, est, ouest à chaque étape. Les orientations demandées servent uniquement à vous indiquer dans quelle direction l'appareil doit être positionné par rapport à un point. Vous pouvez simplement admettre que le Nord est face à vous, l'Est est à droite, l'Ouest à gauche et le Sud pointe vers vous.</span></p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:11pt;"><br /></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:12pt; font-weight:600; font-style:italic;">Étape 2 : Étalonnage du niveau de la carte</span></p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:11pt;"><br /></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt;">Cette étape fera en sorte que la mise de niveau de la carte soit parfait. Veuillez placer l'appareil le plus horizontalement possible (utilisez un niveau à bulle si nécessaire), puis appuyez sur </span><span style=" font-size:11pt; font-style:italic;">Démarrer</span><span style=" font-size:11pt;">. Ne bougez pas du tout votre appareil jusqu’à la fin de l'étalonnage.</span></p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:11pt;"><br /></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:12pt; font-weight:600; font-style:italic;">Étape 3 : Étalonnage de l'ajustement des Gyroscopes</span></p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:11pt;"><br /></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt;">Cette étape permet d'étalonner la valeur mesurée sur les gyros lorsque la carte ne bouge pas. Pour effectuer l'étalonnage laissez la carte/l'appareil sans bouger et appuyez sur </span><span style=" font-size:11pt; font-style:italic;">Démarrer</span><span style=" font-size:11pt;">. </span></p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:11pt;"><br /></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:12pt; font-weight:600; font-style:italic;">Étape 4 : Étalonnage de la compensation de température</span></p> <p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:11pt;"><br /></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:14pt; font-weight:600; font-style:italic;">#4 : Étalonnage de la compensation de température</span></p> <p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt;">Cet étalonnage va calculer les variations des capteurs à plusieurs températures lors du réchauffement de la carte.</span></p> <p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt;">Ceci permet un certain niveau de correction sur la variation des ajustements en fonction des changements de température. Il améliore à la fois le maintien d'altitude et les performances sur l'axe de lacet.</span></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt;">Pour effectuer cette calibration laissez refroidir la carte à température ambiante dans un endroit le plus froid possible. Après 15-20 minutes branchez le connecteur USB à la carte, cliquez sur le bouton Démarrer et ne bougez plus la carte. Attendez jusqu'à la fin du processus.</span></p> -<p align="center" style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:11pt;"><br /></p></td></tr></table></body></html> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt;">Pour effectuer cet étalonnage, déconnectez toutes les alimentations de la carte et laissez-là refroidir à température ambiante pendant 15-20 minutes. Branchez ensuite la prise USB sur la carte et appuyez sur </span><span style=" font-size:11pt; font-style:italic;">Démarrer</span><span style=" font-size:11pt;"> et ne bougez plus la carte. Attendez jusqu'à la fin du processus.</span></p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:11pt;"><br /></p></body></html> @@ -13556,9 +13561,8 @@ p, li { white-space: pre-wrap; } OK - Manage flight side logs - Gérer les logs de vol + Gérer les logs de vol @@ -13678,10 +13682,15 @@ Veuillez vérifier le fichier. FlightLogPlugin - + Manage flight side logs... Gérer les logs de vol... + + + Manage flight side logs + Gérer les logs de vol + MonitorGadgetFactory From e7ce7ae4f71becbc7d862862b903cf5468886b25 Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Sun, 27 Jul 2014 12:55:22 +0200 Subject: [PATCH 13/26] OP-1337 French_translations_updates : Typo --- .../share/openpilotgcs/translations/openpilotgcs_fr.ts | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_fr.ts b/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_fr.ts index 37b91ba9a..38cd9e7f3 100644 --- a/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_fr.ts +++ b/ground/openpilotgcs/share/openpilotgcs/translations/openpilotgcs_fr.ts @@ -7153,7 +7153,7 @@ p, li { white-space: pre-wrap; } <p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:11pt;"><br /></p> <p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt;">L'étalonnage du magnétomètre devra être effectué avec la carte montée sur votre appareil pour tenir compte des déviations produites par les champs magnétiques / masses métalliques à bord.</span></p> <p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:11pt;"><br /></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt;">Note 1 : Avant de débuter l'étalonnage du magnétomètre ou de des accéléromètres votre Position Home devra être réglée. Cela est nécessaire pour déterminer le vecteur de champ magnétique (Be) et l'accélération due à la gravité (g_e) de votre position.</span></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt;">Note 1 : Avant de débuter l'étalonnage du magnétomètre ou des accéléromètres votre Position Home devra être réglée. Cela est nécessaire pour déterminer le vecteur de champ magnétique (Be) et l'accélération due à la gravité (g_e) de votre position.</span></p> <p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:11pt;"><br /></p> <p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt;">Note 2 : Vous n'avez pas besoin d'orienter votre appareil exactement vers le sud, nord, est, ouest à chaque étape. Les orientations demandées servent uniquement à vous indiquer dans quelle direction l'appareil doit être positionné par rapport à un point. Vous pouvez simplement admettre que le Nord est face à vous, l'Est est à droite, l'Ouest à gauche et le Sud pointe vers vous.</span></p> <p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:11pt;"><br /></p> @@ -7169,7 +7169,7 @@ p, li { white-space: pre-wrap; } <p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:11pt;"><br /></p> <p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt;">Cet étalonnage va calculer les variations des capteurs à plusieurs températures lors du réchauffement de la carte.</span></p> <p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt;">Ceci permet un certain niveau de correction sur la variation des ajustements en fonction des changements de température. Il améliore à la fois le maintien d'altitude et les performances sur l'axe de lacet.</span></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt;">Pour effectuer cet étalonnage, déconnectez toutes les alimentations de la carte et laissez-là refroidir à température ambiante pendant 15-20 minutes. Branchez ensuite la prise USB sur la carte et appuyez sur </span><span style=" font-size:11pt; font-style:italic;">Démarrer</span><span style=" font-size:11pt;"> et ne bougez plus la carte. Attendez jusqu'à la fin du processus.</span></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt;">Pour effectuer cet étalonnage, déconnectez toutes les alimentations de la carte et laissez-là refroidir à température ambiante pendant 15-20 minutes. Branchez ensuite la prise USB sur la carte, appuyez sur </span><span style=" font-size:11pt; font-style:italic;">Démarrer</span><span style=" font-size:11pt;"> et ne bougez plus la carte. Attendez jusqu'à la fin du processus.</span></p> <p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:11pt;"><br /></p></body></html> From 401aeee9eb1872606d69821c8b06eb8ba5172ddd Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sun, 27 Jul 2014 20:25:57 +0200 Subject: [PATCH 14/26] OP-1419 - fix mag factor r2c2 not saved --- .../src/plugins/config/calibration/sixpointcalibrationmodel.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp b/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp index e5da80250..00f9e7c98 100644 --- a/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp +++ b/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp @@ -481,7 +481,7 @@ void SixPointCalibrationModel::save() if (calibratingMag) { RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData(); - for (int i = 0; i < RevoCalibration::MAG_TRANSFORM_R2C2; i++) { + for (int i = 0; i < RevoCalibration::MAG_TRANSFORM_R2C2 + 1; i++) { revoCalibrationData.mag_transform[i] = result.revoCalibrationData.mag_transform[i]; } for (int i = 0; i < 3; i++) { From 5f8900f18f73c32f80b17438639a3f43f5a550e5 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sun, 27 Jul 2014 20:40:17 +0200 Subject: [PATCH 15/26] OP-1419 - Uses MAG_TRANSFORM_NUMELEM instead --- .../src/plugins/config/calibration/sixpointcalibrationmodel.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp b/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp index 00f9e7c98..6a9200d8a 100644 --- a/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp +++ b/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp @@ -481,7 +481,7 @@ void SixPointCalibrationModel::save() if (calibratingMag) { RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData(); - for (int i = 0; i < RevoCalibration::MAG_TRANSFORM_R2C2 + 1; i++) { + for (int i = 0; i < RevoCalibration::MAG_TRANSFORM_NUMELEM; i++) { revoCalibrationData.mag_transform[i] = result.revoCalibrationData.mag_transform[i]; } for (int i = 0; i < 3; i++) { From 53c01fe2a921243a9280580da896315cb74b1d8c Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Tue, 29 Jul 2014 01:35:48 +0200 Subject: [PATCH 16/26] Fix WhatsNew for release --- WHATSNEW.txt | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/WHATSNEW.txt b/WHATSNEW.txt index b55b53a21..5dddad0c1 100644 --- a/WHATSNEW.txt +++ b/WHATSNEW.txt @@ -1,3 +1,11 @@ +--- RELEASE-14.06-RC5 --- Peanuts Schnapps --- +This Release Candidate fixes the following issues: + * [OP-1419] - GCS does not set Z magnetometer scale correctly on mag calibration + * [OP-1337] - French translations updates (14.04/05) + +The full list of features, improvements and bugfixes in this release candidate is accessible here: +http://progress.openpilot.org/issues/?filter=11563 + --- RELEASE-14.06-RC4 --- Peanuts Schnapps --- This Release Candidate fixes the following issues: * [OP-1413] - Disable Land flight mode From 16ca5b67ff868e19e3270dca91e80a78ff7a5ea8 Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Tue, 29 Jul 2014 13:31:05 -0400 Subject: [PATCH 17/26] OP-1421 Cruise Control fix xml defaultvalue --- shared/uavobjectdefinition/stabilizationsettings.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/shared/uavobjectdefinition/stabilizationsettings.xml b/shared/uavobjectdefinition/stabilizationsettings.xml index 230ceed95..a79798b4c 100644 --- a/shared/uavobjectdefinition/stabilizationsettings.xml +++ b/shared/uavobjectdefinition/stabilizationsettings.xml @@ -39,7 +39,7 @@ - + From c39ad878b024da07ef3afc5e6aa7809e9d9efd42 Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Wed, 30 Jul 2014 16:56:48 +0200 Subject: [PATCH 18/26] OP-1415 Repeated_names_in_credits : Updated with Wiki contributors --- CREDITS.txt | 60 +++++++++++++++++++++++++++++------------------------ 1 file changed, 33 insertions(+), 27 deletions(-) diff --git a/CREDITS.txt b/CREDITS.txt index 94dfedd10..85368387f 100644 --- a/CREDITS.txt +++ b/CREDITS.txt @@ -1,73 +1,90 @@ Connor Abbott -David Ankers Sergiy Anikeyev -Pedro Assuncao +David Ankers Fredrik Arvidsson +Pedro Assuncao Werner Backes Jose Barros +Andre Bernet Mikael Blomqvist -Pete Boehl +Pete Boehl +Berkely Brown +Joel Brueziere +Thierry Bugeat Glenn Campigli David Carlson +Stefan Cenkov +Andrés Chavarría Krauser +Cosimo Corrado James Cotton Steve Doll Piotr Esden-Tempski -Richard Flay Peter Farnworth Ed Faulkner Andrew Finegan +Kevin Finisterre +Richard Flay Darren Furniss Cliff Geerdes Frederic Goddeeris Daniel Godin -Anthony Gomez +Anthony Gomes +Rodney Grainger Bani Greyling Nuno Guedes -Erik Gustavsson Peter Gunnarsson +Erik Gustavsson Dean Hall Joe Hlebasko Andy Honecker Patrick Huebner Ryan Hunt Mark James +Michael Johnston Ricky King Thorsten Klose Sami Korhonen Hallvard Kristiansen Alan Krum -Edouard Lafargue Mike Labranche +Edouard Lafargue +Laurent Lalanne Fredrik Larsson -Richard von Lehe +Xavier Lecluse Pablo Lema -David Llama Matt Lipski -Les Newell -Ken Northup +David Llama Ben Matthews Greg Matthews +Greg Matthews Guy McCaldin -Gary Mortimer Alessio Morale +Gary Mortimer Cathy Moss +Les Newell +Ken Northup +Bertrand Oresve Angus Peart John Pike +Jorge Pombo Marcos Dmytro Poplavskiy Eric Price Richard Querin Randy Ram Philippe Renon Laurent Ribon +Jonathan Rodgers Mathieu Rondonneau +Jörg-D Rothfuchs Julien Rouviere Jackson Russell Zik Saleeba Professor Dale Schinstock +Troy Schultz +Michael Schwingen Professor Kenn Sebesta Oleg Semyonov Stacey Sheldon -Troy Schultz Dr. Erhard Siegl Dusty Anne Smith Mike Smith @@ -77,10 +94,12 @@ Pete Stapley Vova Starikh Rowan Taubitz Andrew Thoms +Vladimir Timofeev +Jasper Van Loenen Philippe Vanhaesendonck -Jasper van Loenen Vassilis Varveropoulos Kevin Vertucio +Richard Von Lehe Alex Vrubel Mike Walters Brian Webb @@ -89,16 +108,3 @@ Mat Wellington Kendal Wells David Willis Dmitriy Zaitsev -Andre Bernet -Anthony Gomes -Cliff Geerdes -Jörg-D Rothfuchs -Jonathan Rodgers -Laurent Lalanne -Patrick Huebner -Rich von Lehe -Stefan Cenkov -Andrés Chavarría Krauser -Bertrand Oresve -Cosimo Corrado -Rodney Grainger From e539a4c2e287d9621f63bb9c7dd0438c2edc6d9f Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sat, 2 Aug 2014 17:01:52 +0200 Subject: [PATCH 19/26] fix WhatsNew for release --- WHATSNEW.txt | 124 +++++++++++++++++++++------------------------------ 1 file changed, 51 insertions(+), 73 deletions(-) diff --git a/WHATSNEW.txt b/WHATSNEW.txt index 5dddad0c1..5396c7911 100644 --- a/WHATSNEW.txt +++ b/WHATSNEW.txt @@ -1,82 +1,39 @@ ---- RELEASE-14.06-RC5 --- Peanuts Schnapps --- -This Release Candidate fixes the following issues: - * [OP-1419] - GCS does not set Z magnetometer scale correctly on mag calibration - * [OP-1337] - French translations updates (14.04/05) - -The full list of features, improvements and bugfixes in this release candidate is accessible here: -http://progress.openpilot.org/issues/?filter=11563 - ---- RELEASE-14.06-RC4 --- Peanuts Schnapps --- -This Release Candidate fixes the following issues: - * [OP-1413] - Disable Land flight mode - * [OP-1412] - INS13Outdoor Yaw Gyro drift - * [OP-1408] - Board rotation is not always saved during Revo calibration - * [OP-1358] - Split board rotation into user set configuration and calibrated offseta - -The full list of features, improvements and bugfixes in this release candidate is accessible here: -http://progress.openpilot.org/issues/?filter=11562 - ---- RELEASE-14.06-RC3 --- Peanuts Schnapps --- -This Release Candidate fixes the following issues: - * [OP-1394] - Flight display widget - telemetry data does not zero on disconnection - * [OP-1393] - SerialPlugin destructor generates valgrind error - * [OP-1391] - System allows arming if current flight mode uses Thrust Control = AH or AV - * [OP-1390] - OpenGL support for older Graphics Cards - * [OP-1389] - GCS Crashes exiting Flight side log window - * [OP-1384] - Revo Board Rotation data is cleared to zero by other calibration steps - * [OP-1383] - GCS crashes when connected via serial port - * [OP-1347] - Flight logs settings - UI / segfault - * [OP-1337] - French translations updates (14.04/05) - * [OP-1226] - screen problems - * [OP-1063] - Multirotor Configuration - * [OP-792] - 'Autotune' still showing up in flight mode settings in GCS - -The full list of features, improvements and bugfixes in this release candidate is accessible here: -http://progress.openpilot.org/issues/?filter=11561 - ---- RELEASE-14.06-RC2 --- Peanuts Schnapps --- -This Release Candidate fixes the following issues: - *[OP-1378] - Check Limits for flight modes - *[OP-1377] - Calibration config panel has Apply button even when not in Expert mode - *[OP-1376] - Calibration results not saved to SD - *[OP-1375] - Update Mag Ki and Kp default settings - *[OP-1374] - Automatically enable the right PathFollower - *[OP-1337] - Fixes for French translation - -The full list of features, improvements and bugfixes in this release candidate is accessible here: -http://progress.openpilot.org/issues/?filter=11462 - ---- RELEASE-14.06-RC1 --- Peanuts Schnapps --- +--- RELEASE-14.06 --- Peanuts Schnapps --- This is the Mid 2014 release. -This version still supports the CopterControl and CC3D. -It includes several additions and changes aimed at gps/navigation functionalities -(that are expected to be fully available on next release). +This version supports the CopterControl, CC3D, Atom and the Revolution Flight controllers as well as the OPLink Modems. -Some additions in this release: -- Addition of GPS assisted flight modes for revo: - Return To Base, Position hold, AutoCruise and Position Vario(LOS, FPV and NSEW); -- Stabilization refactoring that enhance rate performances; -- Sensor calibration has been redesigned for better usability; +This release includes many additions, improvements and fixes, it is the result of many thousands of hours of development and testing. + +Some key additions in this release: +- Many additions and changes aimed at gps/navigation functionality for the Revolution platform including GPS assisted flight modes: Return To Base, Position Hold, AutoCruise and Position Vario(LOS, FPV and NSEW). +- Stabilization refactoring and enhancements for even better flight performance. +- Completely new sensor calibration routines and greatly enhanced GUI. +- Additional 3rd Party Hardware support, notably the MS4525DO based airspeed sensors and WS281x LED drivers. +- Performance improvements in both embedded firmware and GCS. The full list of features, improvements and bugfixes in this release is accessible here: http://progress.openpilot.org/issues/?filter=11460 + ** New Feature & Improvements * [OP-943] - Start using F4's Core Coupled RAM for more than just the IRQ handler stack * [OP-974] - Make Bootloader Version available while flight software is running * [OP-975] - Reconsider the calibration process + * [OP-1063] - Multirotor Configuration * [OP-1068] - Add support for magnetometer calibration matrix in place of scaling parameters - * [OP-1149] - Handle thermal bias calculation/calibration to gyro and accel - * [OP-1150] - Create UI to allow users to perform board thermal calibration - * [OP-1159] - Remove "Rev" checkboxes on input tab for channels on which it doesn't have an effect + * [OP-1149] - handle thermal bias calculation/calibration to gyro and accel + * [OP-1150] - Create UI to allow users to perform board thermal calibration + * [OP-1159] - Remove "Rev" checkboxes on input tab for channels on which it doesn't have an affect * [OP-1161] - Add Alarm for Magnetometer if disturbed or uncalibrated * [OP-1174] - Beautify Uploader gadget popups * [OP-1194] - Scope gadget - plot and legend visibility state should be persisted between runs * [OP-1198] - Allow GCS gadgets to save/restore individual state * [OP-1216] - Refactor Flight Control Modules - * [OP-1230] - Automatically load the correct firmware file when GCS is running in a development environment + * [OP-1230] - automatically load the correct firmware file when GCS is running in a development environment * [OP-1233] - Add make options to skip qmake and build a specific GCS directory * [OP-1245] - Add GUI to control if, what, when and how to do flight side logging. + * [OP-1247] - Remove Noise calibration from Revo calibration config widget * [OP-1250] - Add GPS Protocol configuration in the Hardware configuration panel * [OP-1258] - Update GCC ARM Embedded to 4.8-2014-q1 * [OP-1259] - Cruise Control tweaks @@ -90,23 +47,29 @@ http://progress.openpilot.org/issues/?filter=11460 * [OP-1307] - Create a bare DiscoveryF4 target for debugging and development purposes * [OP-1308] - Set the same logic to CRITICAL Alarm and same logic to ERROR Alarm * [OP-1312] - Implement a PIOS WS281x driver - * [OP-1319] - Fix minor spelling mistakes in fw code * [OP-1335] - ConfigTaskWidget - Add support to bind GComboBox to integer property * [OP-1339] - System Health panel improvement - * [OP-1350] - TakeOff location handling to be used with RTH + * [OP-1378] - Check Limits for flight modes + * [OP-1342] - PFD widget emits lots of warning + * [OP-1350] - TakeOff location handling to be used with RTB + * [OP-1358] - Split board rotation into user set configuration and calibrated offset * [OP-1365] - Add instrumentation functions for flight code - + * [OP-1374] - Automatically enable the right PathFollower + * [OP-1390] - OpenGL support for older Graphics Cards + * [OP-1413] - Disable Land flight mode ** Bug + * [OP-792] - 'Autotune' still showing up in flight mode settings in GCS * [OP-1026] - Provide some standard method of calibrating CPU speed and load measurement for boards * [OP-1033] - Data transfer errors on USB HID on F1 devices * [OP-1043] - Ground OPLinkMini refuses to connect to one Revo unless first connected to another Revo * [OP-1056] - GPS does not set home location when erased after lock has been established * [OP-1080] - Unreliable detection of board through OPLink - * [OP-1100] - GCS plist for mac shows wrong associated filetypes, leftover from qtcreator + * [OP-1100] - gcs plist for mac shows wrong associated filetypes, leftover from qtcreator * [OP-1131] - Firmware mismatch check is not done if Uploader gadget is not active * [OP-1172] - Some fonts are not defined in config files * [OP-1196] - Board rotation in GCS not shown correctly upon connection but correctly saved in memory - * [OP-1212] - Fix Priority queue handling in telemetry + * [OP-1212] - Fix Priority queue handling in telemetry + * [OP-1226] - screen problems * [OP-1227] - High CPU load in ratitude mode on CopterControl * [OP-1232] - Setting high telemetry rates for periodic uavobject triggers eventsystem warning. * [OP-1235] - Some fixes for altitude estimation @@ -120,9 +83,8 @@ http://progress.openpilot.org/issues/?filter=11460 * [OP-1283] - SystemHealthGadgetWidget::updateAlarms misinterprets coordinates in SVG file * [OP-1284] - RTB flies into ground if base is high * [OP-1285] - Erase Settings ToolTip is wrong - * [OP-1286] - GCS Map "Go To Place" doesn't work * [OP-1288] - GPS PositionHold immediately flies several meters away if Home is not close - * [OP-1291] - Fix matlab import after UAVTalk changes + * [OP-1291] - fix matlab import after UAVTalk changes * [OP-1294] - Fix stack sizes for CopterControl * [OP-1295] - Autoupdate not working * [OP-1296] - Altitude Hold causes copter to ascent at full throttle when far from home location @@ -133,25 +95,40 @@ http://progress.openpilot.org/issues/?filter=11460 * [OP-1314] - Fix Airspeed stack size * [OP-1315] - Unable to arm UAV when AirspeedSensorType is set to GroundspeedBaseWindEstimation * [OP-1323] - GCS font fixes - * [OP-1325] - Fix event system warnings to be errors - * [OP-1326] - Set AIrspeedSensor default back to "None" + * [OP-1325] - fix event system warnings to be errors + * [OP-1326] - set AIrspeedSensor default back to "None" * [OP-1327] - SystemAlarms must be non-acked * [OP-1329] - Various fixes to airspeed module * [OP-1330] - Cannot set homelocation.set=false when gps reception is optimal * [OP-1331] - Input and Output Channel Configuration alignments issues * [OP-1332] - PiOS alarms does not reset alarm state on timer overflow - * [OP-1333] - Output Channel Configuration alignments issues + * [OP-1333] - Output Channel Configuration alignments issues * [OP-1340] - Auto-update greyed out - not available * [OP-1343] - GCS Configuration - Input Channel ResponseTime not saved * [OP-1346] - Input Channel Response Time mismatch between GCS config screen and UAVObject + * [OP-1347] - Flight logs settings - UI / segfault * [OP-1348] - Config Gadget flashes next panel when connecting/disconnecting board * [OP-1351] - GCS Calibration UI polishing * [OP-1352] - Headwind-improvements for FixedWingPathFollower * [OP-1353] - HITL Flightgear fails to set Position and velocity correctly * [OP-1354] - Current and voltage not shown in PFD - * [OP-1355] - Magnetometer calibration and board rotation don't play along - * [OP-1363] - Sanitychecks MUST check if magnetometers and GPS are enabled for any pathfollower modes (outdoor mode selected) - * [OP-1371] - Sanitychecks overzealous: hitl/sitl broken + * [OP-1355] - magnetometer calibration and board rotation don't play along + * [OP-1363] - sanitychecks MUST check if magnetometers and GPS are enabled for any pathfollower modes (outdoor mode selected) + * [OP-1371] - sanitychecks overzealous: hitl/sitl broken + * [OP-1375] - Update Mag Ki and Kp default settings + * [OP-1376] - Calibration results not saved to SD + * [OP-1377] - Calibration config panel has Apply button even when not in Expert mode + * [OP-1383] - GCS crashes when connected via serial port + * [OP-1384] - Revo Board Rotation data is cleared to zero by other calibration steps + * [OP-1389] - GCS Crashes exiting Flight side log window + * [OP-1391] - System allows arming if current flight mode uses Thrust Control = AH or AV + * [OP-1393] - SerialPlugin destructor generates valgrind error + * [OP-1394] - Flight display widget - telemetry data does not zero on disconnection + * [OP-1408] - Board rotation is not always saved during Revo calibration + * [OP-1412] - INS13Outdoor Yaw Gyro drift + * [OP-1415] - Repeated names in CREDITS.txt + * [OP-1419] - GCS does not set Z magnetometer scale correctly on mag calibration + * [OP-1421] - Cruise Control xml defaultvalue incorrect ** Tasks * [OP-1274] - Update FreeRTOS to 8.0 @@ -161,6 +138,7 @@ http://progress.openpilot.org/issues/?filter=11460 * [OP-1309] - Stabilization refactoring + --- RELEASE-14.01 --- Cruising Ratt --- This is the first 2014 software release. This version still supports the CopterControl and CC3D. From 50956a8620e9d300b7d8d80afc96de83f3506d04 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sat, 2 Aug 2014 17:13:36 +0200 Subject: [PATCH 20/26] fix WhatsNew for release --- WHATSNEW.txt | 3 --- 1 file changed, 3 deletions(-) diff --git a/WHATSNEW.txt b/WHATSNEW.txt index 5396c7911..5f329b876 100644 --- a/WHATSNEW.txt +++ b/WHATSNEW.txt @@ -15,7 +15,6 @@ The full list of features, improvements and bugfixes in this release is accessib http://progress.openpilot.org/issues/?filter=11460 - ** New Feature & Improvements * [OP-943] - Start using F4's Core Coupled RAM for more than just the IRQ handler stack * [OP-974] - Make Bootloader Version available while flight software is running @@ -137,8 +136,6 @@ http://progress.openpilot.org/issues/?filter=11460 * [OP-1263] - Move SDL out of Qt install * [OP-1309] - Stabilization refactoring - - --- RELEASE-14.01 --- Cruising Ratt --- This is the first 2014 software release. This version still supports the CopterControl and CC3D. From 3c1b6892247fac59952c15c90f76c3022f598135 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Thu, 17 Jul 2014 01:20:42 +0200 Subject: [PATCH 21/26] OP-1403 - Add generic I2C/SPI HMC5x83 mag driver --- flight/pios/common/pios_hmc5x83.c | 518 ++++++++++++++++++++++++++++++ flight/pios/inc/pios_hmc5x83.h | 132 ++++++++ flight/pios/pios.h | 6 + 3 files changed, 656 insertions(+) create mode 100644 flight/pios/common/pios_hmc5x83.c create mode 100644 flight/pios/inc/pios_hmc5x83.h diff --git a/flight/pios/common/pios_hmc5x83.c b/flight/pios/common/pios_hmc5x83.c new file mode 100644 index 000000000..922b5fde7 --- /dev/null +++ b/flight/pios/common/pios_hmc5x83.c @@ -0,0 +1,518 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_HMC5x83 HMC5x83 Functions + * @brief Deals with the hardware interface to the magnetometers + * @{ + * @file pios_hmc5x83.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief HMC5x83 Magnetic Sensor Functions from AHRS + * @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 "pios.h" +#include + +#ifdef PIOS_INCLUDE_HMC5X83 + +/* Global Variables */ + +/* Local Types */ + +/* Local Variables */ +volatile bool pios_hmc5x83_data_ready; + +static int32_t PIOS_HMC5x83_Config(const struct pios_hmc5x83_cfg *cfg); +// static int32_t PIOS_HMC5x83_Read(uint8_t address, uint8_t *buffer, uint8_t len); +// static int32_t PIOS_HMC5x83_Write(uint8_t address, uint8_t buffer); + +static const struct pios_hmc5x83_cfg *dev_cfg; +static uint32_t dev_port_id; +static uint8_t dev_slave_num; +/** + * @brief Initialize the HMC5x83 magnetometer sensor. + * @return none + */ +void PIOS_HMC5x83_Init(const struct pios_hmc5x83_cfg *cfg, uint32_t port_id, uint8_t slave_num) +{ + dev_cfg = cfg; // store config before enabling interrupt + dev_port_id = port_id; + dev_slave_num = slave_num; +#ifdef PIOS_HMC5X83_HAS_GPIOS + PIOS_EXTI_Init(cfg->exti_cfg); +#endif + + int32_t val = PIOS_HMC5x83_Config(cfg); + + PIOS_Assert(val == 0); + + pios_hmc5x83_data_ready = false; +} + +/** + * @brief Initialize the HMC5x83 magnetometer sensor + * \return none + * \param[in] PIOS_HMC5x83_ConfigTypeDef struct to be used to configure sensor. + * + * CTRL_REGA: Control Register A + * Read Write + * Default value: 0x10 + * 7:5 0 These bits must be cleared for correct operation. + * 4:2 DO2-DO0: Data Output Rate Bits + * DO2 | DO1 | DO0 | Minimum Data Output Rate (Hz) + * ------------------------------------------------------ + * 0 | 0 | 0 | 0.75 + * 0 | 0 | 1 | 1.5 + * 0 | 1 | 0 | 3 + * 0 | 1 | 1 | 7.5 + * 1 | 0 | 0 | 15 (default) + * 1 | 0 | 1 | 30 + * 1 | 1 | 0 | 75 + * 1 | 1 | 1 | Not Used + * 1:0 MS1-MS0: Measurement Configuration Bits + * MS1 | MS0 | MODE + * ------------------------------ + * 0 | 0 | Normal + * 0 | 1 | Positive Bias + * 1 | 0 | Negative Bias + * 1 | 1 | Not Used + * + * CTRL_REGB: Control RegisterB + * Read Write + * Default value: 0x20 + * 7:5 GN2-GN0: Gain Configuration Bits. + * GN2 | GN1 | GN0 | Mag Input | Gain | Output Range + * | | | Range[Ga] | [LSB/mGa] | + * ------------------------------------------------------ + * 0 | 0 | 0 | ±0.88Ga | 1370 | 0xF800–0x07FF (-2048:2047) + * 0 | 0 | 1 | ±1.3Ga (def) | 1090 | 0xF800–0x07FF (-2048:2047) + * 0 | 1 | 0 | ±1.9Ga | 820 | 0xF800–0x07FF (-2048:2047) + * 0 | 1 | 1 | ±2.5Ga | 660 | 0xF800–0x07FF (-2048:2047) + * 1 | 0 | 0 | ±4.0Ga | 440 | 0xF800–0x07FF (-2048:2047) + * 1 | 0 | 1 | ±4.7Ga | 390 | 0xF800–0x07FF (-2048:2047) + * 1 | 1 | 0 | ±5.6Ga | 330 | 0xF800–0x07FF (-2048:2047) + * 1 | 1 | 1 | ±8.1Ga | 230 | 0xF800–0x07FF (-2048:2047) + * |Not recommended| + * + * 4:0 CRB4-CRB: 0 This bit must be cleared for correct operation. + * + * _MODE_REG: Mode Register + * Read Write + * Default value: 0x02 + * 7:2 0 These bits must be cleared for correct operation. + * 1:0 MD1-MD0: Mode Select Bits + * MS1 | MS0 | MODE + * ------------------------------ + * 0 | 0 | Continuous-Conversion Mode. + * 0 | 1 | Single-Conversion Mode + * 1 | 0 | Negative Bias + * 1 | 1 | Sleep Mode + */ +static uint8_t CTRLB = 0x00; +static int32_t PIOS_HMC5x83_Config(const struct pios_hmc5x83_cfg *cfg) +{ + uint8_t CTRLA = 0x00; + uint8_t MODE = 0x00; + + CTRLB = 0; + + CTRLA |= (uint8_t)(cfg->M_ODR | cfg->Meas_Conf); + CTRLB |= (uint8_t)(cfg->Gain); + MODE |= (uint8_t)(cfg->Mode); + + // CRTL_REGA + if (dev_cfg->Driver->Write(PIOS_HMC5x83_CONFIG_REG_A, CTRLA) != 0) { + return -1; + } + + // CRTL_REGB + if (dev_cfg->Driver->Write(PIOS_HMC5x83_CONFIG_REG_B, CTRLB) != 0) { + return -1; + } + + // Mode register + if (dev_cfg->Driver->Write(PIOS_HMC5x83_MODE_REG, MODE) != 0) { + return -1; + } + + return 0; +} + +/** + * @brief Read current X, Z, Y values (in that order) + * \param[out] int16_t array of size 3 to store X, Z, and Y magnetometer readings + * \return 0 for success or -1 for failure + */ +int32_t PIOS_HMC5x83_ReadMag(int16_t out[3]) +{ + pios_hmc5x83_data_ready = false; + uint8_t buffer[6]; + int32_t temp; + int32_t sensitivity; + + if (dev_cfg->Driver->Read(PIOS_HMC5x83_DATAOUT_XMSB_REG, buffer, 6) != 0) { + return -1; + } + + switch (CTRLB & 0xE0) { + case 0x00: + sensitivity = PIOS_HMC5x83_Sensitivity_0_88Ga; + break; + case 0x20: + sensitivity = PIOS_HMC5x83_Sensitivity_1_3Ga; + break; + case 0x40: + sensitivity = PIOS_HMC5x83_Sensitivity_1_9Ga; + break; + case 0x60: + sensitivity = PIOS_HMC5x83_Sensitivity_2_5Ga; + break; + case 0x80: + sensitivity = PIOS_HMC5x83_Sensitivity_4_0Ga; + break; + case 0xA0: + sensitivity = PIOS_HMC5x83_Sensitivity_4_7Ga; + break; + case 0xC0: + sensitivity = PIOS_HMC5x83_Sensitivity_5_6Ga; + break; + case 0xE0: + sensitivity = PIOS_HMC5x83_Sensitivity_8_1Ga; + break; + default: + PIOS_Assert(0); + } + + for (int i = 0; i < 3; i++) { + temp = ((int16_t)((uint16_t)buffer[2 * i] << 8) + + buffer[2 * i + 1]) * 1000 / sensitivity; + out[i] = temp; + } + // Data reads out as X,Z,Y + temp = out[2]; + out[2] = out[1]; + out[1] = temp; + + // This should not be necessary but for some reason it is coming out of continuous conversion mode + dev_cfg->Driver->Write(PIOS_HMC5x83_MODE_REG, PIOS_HMC5x83_MODE_CONTINUOUS); + + return 0; +} + + +/** + * @brief Read the identification bytes from the HMC5x83 sensor + * \param[out] uint8_t array of size 4 to store HMC5x83 ID. + * \return 0 if successful, -1 if not + */ +uint8_t PIOS_HMC5x83_ReadID(uint8_t out[4]) +{ + uint8_t retval = dev_cfg->Driver->Read(PIOS_HMC5x83_DATAOUT_IDA_REG, out, 3); + + out[3] = '\0'; + return retval; +} + +/** + * @brief Tells whether new magnetometer readings are available + * \return true if new data is available + * \return false if new data is not available + */ +bool PIOS_HMC5x83_NewDataAvailable(void) +{ + return pios_hmc5x83_data_ready; +} + +/** + * @brief Run self-test operation. Do not call this during operational use!! + * \return 0 if success, -1 if test failed + */ +int32_t PIOS_HMC5x83_Test(void) +{ + int32_t failed = 0; + uint8_t registers[3] = { 0, 0, 0 }; + uint8_t status; + uint8_t ctrl_a_read; + uint8_t ctrl_b_read; + uint8_t mode_read; + int16_t values[3]; + + + /* Verify that ID matches (HMC5x83 ID is null-terminated ASCII string "H43") */ + char id[4]; + + PIOS_HMC5x83_ReadID((uint8_t *)id); + if ((id[0] != 'H') || (id[1] != '4') || (id[2] != '3')) { // Expect H43 + return -1; + } + + /* Backup existing configuration */ + if (dev_cfg->Driver->Read(PIOS_HMC5x83_CONFIG_REG_A, registers, 3) != 0) { + return -1; + } + + /* Stop the device and read out last value */ + PIOS_DELAY_WaitmS(10); + if (dev_cfg->Driver->Write(PIOS_HMC5x83_MODE_REG, PIOS_HMC5x83_MODE_IDLE) != 0) { + return -1; + } + if (dev_cfg->Driver->Read(PIOS_HMC5x83_DATAOUT_STATUS_REG, &status, 1) != 0) { + return -1; + } + if (PIOS_HMC5x83_ReadMag(values) != 0) { + return -1; + } + + /* + * Put HMC5x83 into self test mode + * This is done by placing measurement config into positive (0x01) or negative (0x10) bias + * and then placing the mode register into single-measurement mode. This causes the HMC5x83 + * to create an artificial magnetic field of ~1.1 Gauss. + * + * If gain were PIOS_HMC5x83_GAIN_2_5, for example, X and Y will read around +766 LSB + * (1.16 Ga * 660 LSB/Ga) and Z would read around +713 LSB (1.08 Ga * 660 LSB/Ga) + * + * Changing measurement config back to PIOS_HMC5x83_MEASCONF_NORMAL will leave self-test mode. + */ + PIOS_DELAY_WaitmS(10); + if (dev_cfg->Driver->Write(PIOS_HMC5x83_CONFIG_REG_A, PIOS_HMC5x83_MEASCONF_BIAS_POS | PIOS_HMC5x83_ODR_15) != 0) { + return -1; + } + PIOS_DELAY_WaitmS(10); + if (dev_cfg->Driver->Write(PIOS_HMC5x83_CONFIG_REG_B, PIOS_HMC5x83_GAIN_8_1) != 0) { + return -1; + } + PIOS_DELAY_WaitmS(10); + if (dev_cfg->Driver->Write(PIOS_HMC5x83_MODE_REG, PIOS_HMC5x83_MODE_SINGLE) != 0) { + return -1; + } + + /* Must wait for value to be updated */ + PIOS_DELAY_WaitmS(200); + + if (PIOS_HMC5x83_ReadMag(values) != 0) { + return -1; + } + + /* + if(abs(values[0] - 766) > 20) + failed |= 1; + if(abs(values[1] - 766) > 20) + failed |= 1; + if(abs(values[2] - 713) > 20) + failed |= 1; + */ + + dev_cfg->Driver->Read(PIOS_HMC5x83_CONFIG_REG_A, &ctrl_a_read, 1); + dev_cfg->Driver->Read(PIOS_HMC5x83_CONFIG_REG_B, &ctrl_b_read, 1); + dev_cfg->Driver->Read(PIOS_HMC5x83_MODE_REG, &mode_read, 1); + dev_cfg->Driver->Read(PIOS_HMC5x83_DATAOUT_STATUS_REG, &status, 1); + + + /* Restore backup configuration */ + PIOS_DELAY_WaitmS(10); + if (dev_cfg->Driver->Write(PIOS_HMC5x83_CONFIG_REG_A, registers[0]) != 0) { + return -1; + } + PIOS_DELAY_WaitmS(10); + if (dev_cfg->Driver->Write(PIOS_HMC5x83_CONFIG_REG_B, registers[1]) != 0) { + return -1; + } + PIOS_DELAY_WaitmS(10); + if (dev_cfg->Driver->Write(PIOS_HMC5x83_MODE_REG, registers[2]) != 0) { + return -1; + } + + return failed; +} + +/** + * @brief IRQ Handler + */ +bool PIOS_HMC5x83_IRQHandler(void) +{ + pios_hmc5x83_data_ready = true; + + return false; +} + +#ifdef PIOS_INCLUDE_SPI +int32_t PIOS_HMC5x83_SPI_Read(uint8_t address, uint8_t *buffer, uint8_t len); +int32_t PIOS_HMC5x83_SPI_Write(uint8_t address, uint8_t buffer); + +const struct pios_hmc5x83_io_driver PIOS_HMC5x83_SPI_DRIVER = { + .Read = PIOS_HMC5x83_SPI_Read, + .Write = PIOS_HMC5x83_SPI_Write, +}; + +static int32_t pios_hmc5x83_spi_claim_bus() +{ + if (PIOS_SPI_ClaimBus(dev_port_id) < 0) { + return -1; + } + PIOS_SPI_RC_PinSet(dev_port_id, dev_slave_num, 0); + return 0; +} + +static void pios_hmc5x83_spi_release_bus() +{ + PIOS_SPI_RC_PinSet(dev_port_id, dev_slave_num, 1); + PIOS_SPI_ReleaseBus(dev_port_id); +} +/** + * @brief Reads one or more bytes into a buffer + * \param[in] address HMC5x83 register address (depends on size) + * \param[out] buffer destination buffer + * \param[in] len number of bytes which should be read + * \return 0 if operation was successful + * \return -1 if error during I2C transfer + * \return -2 if unable to claim i2c device + */ +int32_t PIOS_HMC5x83_SPI_Read(uint8_t address, uint8_t *buffer, uint8_t len) +{ + if (pios_hmc5x83_spi_claim_bus() < 0) { + return -1; + } + + memset(buffer, 0xA5, len); + PIOS_SPI_TransferByte(dev_port_id, address | PIOS_HMC5x83_SPI_AUTOINCR_FLAG | PIOS_HMC5x83_SPI_READ_FLAG); + + // buffer[0] = address | PIOS_HMC5x83_SPI_AUTOINCR_FLAG | PIOS_HMC5x83_SPI_READ_FLAG; + /* Copy the transfer data to the buffer */ + if (PIOS_SPI_TransferBlock(dev_port_id, NULL, buffer, len, NULL) < 0) { + pios_hmc5x83_spi_release_bus(); + return -3; + } + pios_hmc5x83_spi_release_bus(); + return 0; +} + +/** + * @brief Writes one or more bytes to the HMC5x83 + * \param[in] address Register address + * \param[in] buffer source buffer + * \return 0 if operation was successful + * \return -1 if error during I2C transfer + * \return -2 if unable to claim spi device + */ +int32_t PIOS_HMC5x83_SPI_Write(uint8_t address, uint8_t buffer) +{ + if (pios_hmc5x83_spi_claim_bus() < 0) { + return -1; + } + uint8_t data[] = { + address | PIOS_HMC5x83_SPI_AUTOINCR_FLAG, + buffer, + }; + + if (PIOS_SPI_TransferBlock(dev_port_id, data, NULL, sizeof(data), NULL) < 0) { + pios_hmc5x83_spi_release_bus(); + return -2; + } + + pios_hmc5x83_spi_release_bus(); + return 0; +} +#endif /* ifdef PIOS_INCLUDE_SPI */ +#ifdef PIOS_INCLUDE_I2C + +int32_t PIOS_HMC5x83_I2C_Read(uint8_t address, uint8_t *buffer, uint8_t len); +int32_t PIOS_HMC5x83_I2C_Write(uint8_t address, uint8_t buffer); + +const struct pios_hmc5x83_io_driver PIOS_HMC5x83_I2C_DRIVER = { + .Read = PIOS_HMC5x83_I2C_Read, + .Write = PIOS_HMC5x83_I2C_Write, +}; + +/** + * @brief Reads one or more bytes into a buffer + * \param[in] address HMC5x83 register address (depends on size) + * \param[out] buffer destination buffer + * \param[in] len number of bytes which should be read + * \return 0 if operation was successful + * \return -1 if error during I2C transfer + * \return -2 if unable to claim i2c device + */ +int32_t PIOS_HMC5x83_I2C_Read(uint8_t address, uint8_t *buffer, uint8_t len) +{ + uint8_t addr_buffer[] = { + address, + }; + + const struct pios_i2c_txn txn_list[] = { + { + .info = __func__, + .addr = PIOS_HMC5x83_I2C_ADDR, + .rw = PIOS_I2C_TXN_WRITE, + .len = sizeof(addr_buffer), + .buf = addr_buffer, + } + , + { + .info = __func__, + .addr = PIOS_HMC5x83_I2C_ADDR, + .rw = PIOS_I2C_TXN_READ, + .len = len, + .buf = buffer, + } + }; + + return PIOS_I2C_Transfer(dev_port_id, txn_list, NELEMENTS(txn_list)); +} + +/** + * @brief Writes one or more bytes to the HMC5x83 + * \param[in] address Register address + * \param[in] buffer source buffer + * \return 0 if operation was successful + * \return -1 if error during I2C transfer + * \return -2 if unable to claim i2c device + */ +int32_t PIOS_HMC5x83_I2C_Write(uint8_t address, uint8_t buffer) +{ + uint8_t data[] = { + address, + buffer, + }; + + const struct pios_i2c_txn txn_list[] = { + { + .info = __func__, + .addr = PIOS_HMC5x83_I2C_ADDR, + .rw = PIOS_I2C_TXN_WRITE, + .len = sizeof(data), + .buf = data, + } + , + }; + + ; + return PIOS_I2C_Transfer(dev_port_id, txn_list, NELEMENTS(txn_list)); +} +#endif /* ifdef PIOS_INCLUDE_I2C */ + + +#endif /* PIOS_INCLUDE_HMC5x83 */ + +/** + * @} + * @} + */ diff --git a/flight/pios/inc/pios_hmc5x83.h b/flight/pios/inc/pios_hmc5x83.h new file mode 100644 index 000000000..fc0d9bbab --- /dev/null +++ b/flight/pios/inc/pios_hmc5x83.h @@ -0,0 +1,132 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_HMC5x83 HMC5x83 Functions + * @brief Deals with the hardware interface to the magnetometers + * @{ + * + * @file pios_hmc5x83.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief HMC5x83 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_HMC5x83_H +#define PIOS_HMC5x83_H + +/* HMC5x83 Addresses */ +#define PIOS_HMC5x83_I2C_ADDR 0x1E +#define PIOS_HMC5x83_I2C_READ_ADDR 0x3D +#define PIOS_HMC5x83_I2C_WRITE_ADDR 0x3C + +#define PIOS_HMC5x83_SPI_READ_FLAG 0x80 +#define PIOS_HMC5x83_SPI_AUTOINCR_FLAG 0x40 +#define PIOS_HMC5x83_CONFIG_REG_A (uint8_t)0x00 +#define PIOS_HMC5x83_CONFIG_REG_B (uint8_t)0x01 +#define PIOS_HMC5x83_MODE_REG (uint8_t)0x02 +#define PIOS_HMC5x83_DATAOUT_XMSB_REG 0x03 +#define PIOS_HMC5x83_DATAOUT_XLSB_REG 0x04 +#define PIOS_HMC5x83_DATAOUT_ZMSB_REG 0x05 +#define PIOS_HMC5x83_DATAOUT_ZLSB_REG 0x06 +#define PIOS_HMC5x83_DATAOUT_YMSB_REG 0x07 +#define PIOS_HMC5x83_DATAOUT_YLSB_REG 0x08 +#define PIOS_HMC5x83_DATAOUT_STATUS_REG 0x09 +#define PIOS_HMC5x83_DATAOUT_IDA_REG 0x0A +#define PIOS_HMC5x83_DATAOUT_IDB_REG 0x0B +#define PIOS_HMC5x83_DATAOUT_IDC_REG 0x0C + +/* Output Data Rate */ +#define PIOS_HMC5x83_ODR_0_75 0x00 +#define PIOS_HMC5x83_ODR_1_5 0x04 +#define PIOS_HMC5x83_ODR_3 0x08 +#define PIOS_HMC5x83_ODR_7_5 0x0C +#define PIOS_HMC5x83_ODR_15 0x10 +#define PIOS_HMC5x83_ODR_30 0x14 +#define PIOS_HMC5x83_ODR_75 0x18 + +/* Measure configuration */ +#define PIOS_HMC5x83_MEASCONF_NORMAL 0x00 +#define PIOS_HMC5x83_MEASCONF_BIAS_POS 0x01 +#define PIOS_HMC5x83_MEASCONF_BIAS_NEG 0x02 + +/* Gain settings */ +#define PIOS_HMC5x83_GAIN_0_88 0x00 +#define PIOS_HMC5x83_GAIN_1_3 0x20 +#define PIOS_HMC5x83_GAIN_1_9 0x40 +#define PIOS_HMC5x83_GAIN_2_5 0x60 +#define PIOS_HMC5x83_GAIN_4_0 0x80 +#define PIOS_HMC5x83_GAIN_4_7 0xA0 +#define PIOS_HMC5x83_GAIN_5_6 0xC0 +#define PIOS_HMC5x83_GAIN_8_1 0xE0 + +/* Modes */ +#define PIOS_HMC5x83_MODE_CONTINUOUS 0x00 +#define PIOS_HMC5x83_MODE_SINGLE 0x01 +#define PIOS_HMC5x83_MODE_IDLE 0x02 +#define PIOS_HMC5x83_MODE_SLEEP 0x03 + +/* Sensitivity Conversion Values */ +#define PIOS_HMC5x83_Sensitivity_0_88Ga 1370 // LSB/Ga +#define PIOS_HMC5x83_Sensitivity_1_3Ga 1090 // LSB/Ga +#define PIOS_HMC5x83_Sensitivity_1_9Ga 820 // LSB/Ga +#define PIOS_HMC5x83_Sensitivity_2_5Ga 660 // LSB/Ga +#define PIOS_HMC5x83_Sensitivity_4_0Ga 440 // LSB/Ga +#define PIOS_HMC5x83_Sensitivity_4_7Ga 390 // LSB/Ga +#define PIOS_HMC5x83_Sensitivity_5_6Ga 330 // LSB/Ga +#define PIOS_HMC5x83_Sensitivity_8_1Ga 230 // LSB/Ga --> NOT RECOMMENDED + +struct pios_hmc5x83_io_driver { + int32_t (*Write)(uint8_t address, uint8_t buffer); + int32_t (*Read)(uint8_t address, uint8_t *buffer, uint8_t len); +}; + +#ifdef PIOS_INCLUDE_SPI +extern const struct pios_hmc5x83_io_driver PIOS_HMC5x83_SPI_DRIVER; +#endif + +#ifdef PIOS_INCLUDE_I2C +extern const struct pios_hmc5x83_io_driver PIOS_HMC5x83_I2C_DRIVER; +#endif + +struct pios_hmc5x83_cfg { +#ifdef PIOS_HMC5x83_HAS_GPIOS + const struct pios_exti_cfg *exti_cfg; /* Pointer to the EXTI configuration */ +#endif + uint8_t M_ODR; /* OUTPUT DATA RATE --> here below the relative define (See datasheet page 11 for more details) */ + uint8_t Meas_Conf; /* Measurement Configuration,: Normal, positive bias, or negative bias --> here below the relative define */ + uint8_t Gain; /* Gain Configuration, select the full scale --> here below the relative define (See datasheet page 11 for more details) */ + uint8_t Mode; + const struct pios_hmc5x83_io_driver *Driver; +}; + +/* Public Functions */ +extern void PIOS_HMC5x83_Init(const struct pios_hmc5x83_cfg *cfg, uint32_t port_id, uint8_t device_num); +extern bool PIOS_HMC5x83_NewDataAvailable(void); +extern int32_t PIOS_HMC5x83_ReadMag(int16_t out[3]); +extern uint8_t PIOS_HMC5x83_ReadID(uint8_t out[4]); +extern int32_t PIOS_HMC5x83_Test(void); +extern bool PIOS_HMC5x83_IRQHandler(); + +#endif /* PIOS_HMC5x83_H */ + +/** + * @} + * @} + */ diff --git a/flight/pios/pios.h b/flight/pios/pios.h index 60535ede9..1bb1ea968 100644 --- a/flight/pios/pios.h +++ b/flight/pios/pios.h @@ -210,6 +210,12 @@ #include #endif +#ifdef PIOS_INCLUDE_HMC5X83 +/* HMC5883/HMC5983 3-Axis Digital Compass */ +/* #define PIOS_HMC5x83_HAS_GPIOS */ +#include +#endif + #ifdef PIOS_INCLUDE_BMP085 /* BMP085 Barometric Pressure Sensor */ #include From 2a8429320d0f6a055e5dfe74948dac82a804b373 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sat, 12 Jul 2014 12:03:10 +0200 Subject: [PATCH 22/26] OP-1403 - Support HMC5983 temperature sensor/compensation --- flight/pios/common/pios_hmc5x83.c | 1 + flight/pios/inc/pios_hmc5x83.h | 9 ++++++--- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/flight/pios/common/pios_hmc5x83.c b/flight/pios/common/pios_hmc5x83.c index 922b5fde7..d1854632f 100644 --- a/flight/pios/common/pios_hmc5x83.c +++ b/flight/pios/common/pios_hmc5x83.c @@ -135,6 +135,7 @@ static int32_t PIOS_HMC5x83_Config(const struct pios_hmc5x83_cfg *cfg) CTRLB = 0; CTRLA |= (uint8_t)(cfg->M_ODR | cfg->Meas_Conf); + CTRLA |= cfg->TempCompensation ? PIOS_HMC5x83_CTRLA_TEMP : 0; CTRLB |= (uint8_t)(cfg->Gain); MODE |= (uint8_t)(cfg->Mode); diff --git a/flight/pios/inc/pios_hmc5x83.h b/flight/pios/inc/pios_hmc5x83.h index fc0d9bbab..94f53b2f9 100644 --- a/flight/pios/inc/pios_hmc5x83.h +++ b/flight/pios/inc/pios_hmc5x83.h @@ -76,6 +76,8 @@ #define PIOS_HMC5x83_GAIN_5_6 0xC0 #define PIOS_HMC5x83_GAIN_8_1 0xE0 +#define PIOS_HMC5x83_CTRLA_TEMP 0x40 + /* Modes */ #define PIOS_HMC5x83_MODE_CONTINUOUS 0x00 #define PIOS_HMC5x83_MODE_SINGLE 0x01 @@ -109,10 +111,11 @@ struct pios_hmc5x83_cfg { #ifdef PIOS_HMC5x83_HAS_GPIOS const struct pios_exti_cfg *exti_cfg; /* Pointer to the EXTI configuration */ #endif - uint8_t M_ODR; /* OUTPUT DATA RATE --> here below the relative define (See datasheet page 11 for more details) */ - uint8_t Meas_Conf; /* Measurement Configuration,: Normal, positive bias, or negative bias --> here below the relative define */ - uint8_t Gain; /* Gain Configuration, select the full scale --> here below the relative define (See datasheet page 11 for more details) */ + uint8_t M_ODR; // OUTPUT DATA RATE --> here below the relative define (See datasheet page 11 for more details) */ + uint8_t Meas_Conf; // Measurement Configuration,: Normal, positive bias, or negative bias --> here below the relative define */ + uint8_t Gain; // Gain Configuration, select the full scale --> here below the relative define (See datasheet page 11 for more details) */ uint8_t Mode; + bool TempCompensation; // enable temperature sensor on HMC5983 for temperature gain compensation const struct pios_hmc5x83_io_driver *Driver; }; From 57a43a5587891565c27452d3d7c2649583cd8d64 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Fri, 18 Jul 2014 20:34:20 +0200 Subject: [PATCH 23/26] OP-1403 - Migrate to new HMC5x83 driver and remove old HMC5883 one --- flight/modules/Attitude/revolution/attitude.c | 2 +- flight/modules/Extensions/MagBaro/magbaro.c | 29 +- flight/modules/Sensors/sensors.c | 12 +- flight/modules/StateEstimation/filtercf.c | 2 +- flight/pios/common/pios_hmc5883.c | 425 ------------------ flight/pios/inc/pios_hmc5883.h | 116 ----- flight/pios/inc/pios_hmc5x83.h | 2 +- flight/pios/pios.h | 6 - .../coptercontrol/firmware/inc/pios_config.h | 2 +- .../firmware/inc/pios_config.h | 4 +- .../discoveryf4bare/firmware/pios_board.c | 27 +- .../oplinkmini/firmware/inc/pios_config.h | 2 +- .../boards/osd/firmware/inc/pios_config.h | 4 +- .../revolution/firmware/inc/pios_config.h | 4 +- .../boards/revolution/firmware/pios_board.c | 27 +- .../revoproto/firmware/inc/pios_config.h | 4 +- .../boards/revoproto/firmware/pios_board.c | 27 +- .../simposix/firmware/inc/pios_config.h | 2 +- make/apps-defs.mk | 2 +- 19 files changed, 78 insertions(+), 621 deletions(-) delete mode 100644 flight/pios/common/pios_hmc5883.c delete mode 100644 flight/pios/inc/pios_hmc5883.h diff --git a/flight/modules/Attitude/revolution/attitude.c b/flight/modules/Attitude/revolution/attitude.c index b49ac751b..2ee8901e0 100644 --- a/flight/modules/Attitude/revolution/attitude.c +++ b/flight/modules/Attitude/revolution/attitude.c @@ -352,7 +352,7 @@ static int32_t updateAttitudeComplementary(bool first_run) // During initialization and if (first_run) { -#if defined(PIOS_INCLUDE_HMC5883) +#if defined(PIOS_INCLUDE_HMC5X83) // To initialize we need a valid mag reading if (xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) != pdTRUE) { return -1; diff --git a/flight/modules/Extensions/MagBaro/magbaro.c b/flight/modules/Extensions/MagBaro/magbaro.c index 2b219ee16..aa956c57e 100644 --- a/flight/modules/Extensions/MagBaro/magbaro.c +++ b/flight/modules/Extensions/MagBaro/magbaro.c @@ -64,7 +64,7 @@ static int32_t alt_ds_pres = 0; static int alt_ds_count = 0; #endif -#if defined(PIOS_INCLUDE_HMC5883) +#if defined(PIOS_INCLUDE_HMC5X83) int32_t mag_test; static float mag_bias[3] = { 0, 0, 0 }; static float mag_scale[3] = { 1, 1, 1 }; @@ -108,7 +108,7 @@ int32_t MagBaroInitialize() #endif if (magbaroEnabled) { -#if defined(PIOS_INCLUDE_HMC5883) +#if defined(PIOS_INCLUDE_HMC5X83) MagSensorInitialize(); #endif @@ -127,15 +127,16 @@ MODULE_INITCALL(MagBaroInitialize, MagBaroStart); /** * Module thread, should not return. */ -#if defined(PIOS_INCLUDE_HMC5883) -static const struct pios_hmc5883_cfg pios_hmc5883_cfg = { -#ifdef PIOS_HMC5883_HAS_GPIOS +#if defined(PIOS_INCLUDE_HMC5X83) +static const struct pios_hmc5x83_cfg pios_hmc5x83_cfg = { +#ifdef PIOS_HMC5X83_HAS_GPIOS .exti_cfg = 0, #endif - .M_ODR = PIOS_HMC5883_ODR_15, - .Meas_Conf = PIOS_HMC5883_MEASCONF_NORMAL, - .Gain = PIOS_HMC5883_GAIN_1_9, - .Mode = PIOS_HMC5883_MODE_CONTINUOUS, + .M_ODR = PIOS_HMC5x83_ODR_15, + .Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL, + .Gain = PIOS_HMC5x83_GAIN_1_9, + .Mode = PIOS_HMC5x83_MODE_CONTINUOUS, + .Driver = &PIOS_HMC5x83_I2C_DRIVER, }; #endif @@ -148,9 +149,9 @@ static void magbaroTask(__attribute__((unused)) void *parameters) PIOS_BMP085_Init(); #endif -#if defined(PIOS_INCLUDE_HMC5883) +#if defined(PIOS_INCLUDE_HMC5X83) MagSensorData mag; - PIOS_HMC5883_Init(&pios_hmc5883_cfg); + PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, PIOS_I2C_MAIN_ADAPTER, 0); uint32_t mag_update_time = PIOS_DELAY_GetRaw(); #endif @@ -197,10 +198,10 @@ static void magbaroTask(__attribute__((unused)) void *parameters) } #endif /* if defined(PIOS_INCLUDE_BMP085) */ -#if defined(PIOS_INCLUDE_HMC5883) - if (PIOS_HMC5883_NewDataAvailable() || PIOS_DELAY_DiffuS(mag_update_time) > 100000) { +#if defined(PIOS_INCLUDE_HMC5X83) + if (PIOS_HMC5x83_NewDataAvailable() || PIOS_DELAY_DiffuS(mag_update_time) > 100000) { int16_t values[3]; - PIOS_HMC5883_ReadMag(values); + PIOS_HMC5x83_ReadMag(values); float mags[3] = { (float)values[1] * mag_scale[0] - mag_bias[0], (float)values[0] * mag_scale[1] - mag_bias[1], -(float)values[2] * mag_scale[2] - mag_bias[2] }; diff --git a/flight/modules/Sensors/sensors.c b/flight/modules/Sensors/sensors.c index e1d85b2f1..c93dc64f8 100644 --- a/flight/modules/Sensors/sensors.c +++ b/flight/modules/Sensors/sensors.c @@ -200,8 +200,8 @@ static void SensorsTask(__attribute__((unused)) void *parameters) PIOS_DEBUG_Assert(0); } -#if defined(PIOS_INCLUDE_HMC5883) - mag_test = PIOS_HMC5883_Test(); +#if defined(PIOS_INCLUDE_HMC5X83) + mag_test = PIOS_HMC5x83_Test(); #else mag_test = 0; #endif @@ -409,11 +409,11 @@ static void SensorsTask(__attribute__((unused)) void *parameters) // Because most crafts wont get enough information from gravity to zero yaw gyro, we try // and make it average zero (weakly) -#if defined(PIOS_INCLUDE_HMC5883) +#if defined(PIOS_INCLUDE_HMC5X83) MagSensorData mag; - if (PIOS_HMC5883_NewDataAvailable() || PIOS_DELAY_DiffuS(mag_update_time) > 150000) { + if (PIOS_HMC5x83_NewDataAvailable() || PIOS_DELAY_DiffuS(mag_update_time) > 150000) { int16_t values[3]; - PIOS_HMC5883_ReadMag(values); + PIOS_HMC5x83_ReadMag(values); float mags[3] = { (float)values[1] - mag_bias[0], (float)values[0] - mag_bias[1], -(float)values[2] - mag_bias[2] }; @@ -428,7 +428,7 @@ static void SensorsTask(__attribute__((unused)) void *parameters) MagSensorSet(&mag); mag_update_time = PIOS_DELAY_GetRaw(); } -#endif /* if defined(PIOS_INCLUDE_HMC5883) */ +#endif /* if defined(PIOS_INCLUDE_HMC5X83) */ #ifdef PIOS_INCLUDE_WDG PIOS_WDG_UpdateFlag(PIOS_WDG_SENSORS); diff --git a/flight/modules/StateEstimation/filtercf.c b/flight/modules/StateEstimation/filtercf.c index 15b61a665..d2e109655 100644 --- a/flight/modules/StateEstimation/filtercf.c +++ b/flight/modules/StateEstimation/filtercf.c @@ -221,7 +221,7 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float // During initialization and if (this->first_run) { -#if defined(PIOS_INCLUDE_HMC5883) +#if defined(PIOS_INCLUDE_HMC5X83) // wait until mags have been updated if (!this->magUpdated) { return FILTERRESULT_ERROR; diff --git a/flight/pios/common/pios_hmc5883.c b/flight/pios/common/pios_hmc5883.c deleted file mode 100644 index d425e16dc..000000000 --- a/flight/pios/common/pios_hmc5883.c +++ /dev/null @@ -1,425 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup PIOS PIOS Core hardware abstraction layer - * @{ - * @addtogroup PIOS_HMC5883 HMC5883 Functions - * @brief Deals with the hardware interface to the magnetometers - * @{ - * @file pios_hmc5883.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. - * @brief HMC5883 Magnetic Sensor Functions from AHRS - * @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 "pios.h" - -#ifdef PIOS_INCLUDE_HMC5883 - -/* Global Variables */ - -/* Local Types */ - -/* Local Variables */ -volatile bool pios_hmc5883_data_ready; - -static int32_t PIOS_HMC5883_Config(const struct pios_hmc5883_cfg *cfg); -static int32_t PIOS_HMC5883_Read(uint8_t address, uint8_t *buffer, uint8_t len); -static int32_t PIOS_HMC5883_Write(uint8_t address, uint8_t buffer); - -static const struct pios_hmc5883_cfg *dev_cfg; - -/** - * @brief Initialize the HMC5883 magnetometer sensor. - * @return none - */ -void PIOS_HMC5883_Init(const struct pios_hmc5883_cfg *cfg) -{ - dev_cfg = cfg; // store config before enabling interrupt - -#ifdef PIOS_HMC5883_HAS_GPIOS - PIOS_EXTI_Init(cfg->exti_cfg); -#endif - - int32_t val = PIOS_HMC5883_Config(cfg); - - PIOS_Assert(val == 0); - - pios_hmc5883_data_ready = false; -} - -/** - * @brief Initialize the HMC5883 magnetometer sensor - * \return none - * \param[in] PIOS_HMC5883_ConfigTypeDef struct to be used to configure sensor. - * - * CTRL_REGA: Control Register A - * Read Write - * Default value: 0x10 - * 7:5 0 These bits must be cleared for correct operation. - * 4:2 DO2-DO0: Data Output Rate Bits - * DO2 | DO1 | DO0 | Minimum Data Output Rate (Hz) - * ------------------------------------------------------ - * 0 | 0 | 0 | 0.75 - * 0 | 0 | 1 | 1.5 - * 0 | 1 | 0 | 3 - * 0 | 1 | 1 | 7.5 - * 1 | 0 | 0 | 15 (default) - * 1 | 0 | 1 | 30 - * 1 | 1 | 0 | 75 - * 1 | 1 | 1 | Not Used - * 1:0 MS1-MS0: Measurement Configuration Bits - * MS1 | MS0 | MODE - * ------------------------------ - * 0 | 0 | Normal - * 0 | 1 | Positive Bias - * 1 | 0 | Negative Bias - * 1 | 1 | Not Used - * - * CTRL_REGB: Control RegisterB - * Read Write - * Default value: 0x20 - * 7:5 GN2-GN0: Gain Configuration Bits. - * GN2 | GN1 | GN0 | Mag Input | Gain | Output Range - * | | | Range[Ga] | [LSB/mGa] | - * ------------------------------------------------------ - * 0 | 0 | 0 | ±0.88Ga | 1370 | 0xF800–0x07FF (-2048:2047) - * 0 | 0 | 1 | ±1.3Ga (def) | 1090 | 0xF800–0x07FF (-2048:2047) - * 0 | 1 | 0 | ±1.9Ga | 820 | 0xF800–0x07FF (-2048:2047) - * 0 | 1 | 1 | ±2.5Ga | 660 | 0xF800–0x07FF (-2048:2047) - * 1 | 0 | 0 | ±4.0Ga | 440 | 0xF800–0x07FF (-2048:2047) - * 1 | 0 | 1 | ±4.7Ga | 390 | 0xF800–0x07FF (-2048:2047) - * 1 | 1 | 0 | ±5.6Ga | 330 | 0xF800–0x07FF (-2048:2047) - * 1 | 1 | 1 | ±8.1Ga | 230 | 0xF800–0x07FF (-2048:2047) - * |Not recommended| - * - * 4:0 CRB4-CRB: 0 This bit must be cleared for correct operation. - * - * _MODE_REG: Mode Register - * Read Write - * Default value: 0x02 - * 7:2 0 These bits must be cleared for correct operation. - * 1:0 MD1-MD0: Mode Select Bits - * MS1 | MS0 | MODE - * ------------------------------ - * 0 | 0 | Continuous-Conversion Mode. - * 0 | 1 | Single-Conversion Mode - * 1 | 0 | Negative Bias - * 1 | 1 | Sleep Mode - */ -static uint8_t CTRLB = 0x00; -static int32_t PIOS_HMC5883_Config(const struct pios_hmc5883_cfg *cfg) -{ - uint8_t CTRLA = 0x00; - uint8_t MODE = 0x00; - - CTRLB = 0; - - CTRLA |= (uint8_t)(cfg->M_ODR | cfg->Meas_Conf); - CTRLB |= (uint8_t)(cfg->Gain); - MODE |= (uint8_t)(cfg->Mode); - - // CRTL_REGA - if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_A, CTRLA) != 0) { - return -1; - } - - // CRTL_REGB - if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_B, CTRLB) != 0) { - return -1; - } - - // Mode register - if (PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, MODE) != 0) { - return -1; - } - - return 0; -} - -/** - * @brief Read current X, Z, Y values (in that order) - * \param[out] int16_t array of size 3 to store X, Z, and Y magnetometer readings - * \return 0 for success or -1 for failure - */ -int32_t PIOS_HMC5883_ReadMag(int16_t out[3]) -{ - pios_hmc5883_data_ready = false; - uint8_t buffer[6]; - int32_t temp; - int32_t sensitivity; - - if (PIOS_HMC5883_Read(PIOS_HMC5883_DATAOUT_XMSB_REG, buffer, 6) != 0) { - return -1; - } - - switch (CTRLB & 0xE0) { - case 0x00: - sensitivity = PIOS_HMC5883_Sensitivity_0_88Ga; - break; - case 0x20: - sensitivity = PIOS_HMC5883_Sensitivity_1_3Ga; - break; - case 0x40: - sensitivity = PIOS_HMC5883_Sensitivity_1_9Ga; - break; - case 0x60: - sensitivity = PIOS_HMC5883_Sensitivity_2_5Ga; - break; - case 0x80: - sensitivity = PIOS_HMC5883_Sensitivity_4_0Ga; - break; - case 0xA0: - sensitivity = PIOS_HMC5883_Sensitivity_4_7Ga; - break; - case 0xC0: - sensitivity = PIOS_HMC5883_Sensitivity_5_6Ga; - break; - case 0xE0: - sensitivity = PIOS_HMC5883_Sensitivity_8_1Ga; - break; - default: - PIOS_Assert(0); - } - - for (int i = 0; i < 3; i++) { - temp = ((int16_t)((uint16_t)buffer[2 * i] << 8) - + buffer[2 * i + 1]) * 1000 / sensitivity; - out[i] = temp; - } - // Data reads out as X,Z,Y - temp = out[2]; - out[2] = out[1]; - out[1] = temp; - - // This should not be necessary but for some reason it is coming out of continuous conversion mode - PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, PIOS_HMC5883_MODE_CONTINUOUS); - - return 0; -} - - -/** - * @brief Read the identification bytes from the HMC5883 sensor - * \param[out] uint8_t array of size 4 to store HMC5883 ID. - * \return 0 if successful, -1 if not - */ -uint8_t PIOS_HMC5883_ReadID(uint8_t out[4]) -{ - uint8_t retval = PIOS_HMC5883_Read(PIOS_HMC5883_DATAOUT_IDA_REG, out, 3); - - out[3] = '\0'; - return retval; -} - -/** - * @brief Tells whether new magnetometer readings are available - * \return true if new data is available - * \return false if new data is not available - */ -bool PIOS_HMC5883_NewDataAvailable(void) -{ - return pios_hmc5883_data_ready; -} - -/** - * @brief Reads one or more bytes into a buffer - * \param[in] address HMC5883 register address (depends on size) - * \param[out] buffer destination buffer - * \param[in] len number of bytes which should be read - * \return 0 if operation was successful - * \return -1 if error during I2C transfer - * \return -2 if unable to claim i2c device - */ -static int32_t PIOS_HMC5883_Read(uint8_t address, uint8_t *buffer, uint8_t len) -{ - uint8_t addr_buffer[] = { - address, - }; - - const struct pios_i2c_txn txn_list[] = { - { - .info = __func__, - .addr = PIOS_HMC5883_I2C_ADDR, - .rw = PIOS_I2C_TXN_WRITE, - .len = sizeof(addr_buffer), - .buf = addr_buffer, - } - , - { - .info = __func__, - .addr = PIOS_HMC5883_I2C_ADDR, - .rw = PIOS_I2C_TXN_READ, - .len = len, - .buf = buffer, - } - }; - - return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list)); -} - -/** - * @brief Writes one or more bytes to the HMC5883 - * \param[in] address Register address - * \param[in] buffer source buffer - * \return 0 if operation was successful - * \return -1 if error during I2C transfer - * \return -2 if unable to claim i2c device - */ -static int32_t PIOS_HMC5883_Write(uint8_t address, uint8_t buffer) -{ - uint8_t data[] = { - address, - buffer, - }; - - const struct pios_i2c_txn txn_list[] = { - { - .info = __func__, - .addr = PIOS_HMC5883_I2C_ADDR, - .rw = PIOS_I2C_TXN_WRITE, - .len = sizeof(data), - .buf = data, - } - , - }; - - ; - return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list)); -} - -/** - * @brief Run self-test operation. Do not call this during operational use!! - * \return 0 if success, -1 if test failed - */ -int32_t PIOS_HMC5883_Test(void) -{ - int32_t failed = 0; - uint8_t registers[3] = { 0, 0, 0 }; - uint8_t status; - uint8_t ctrl_a_read; - uint8_t ctrl_b_read; - uint8_t mode_read; - int16_t values[3]; - - - /* Verify that ID matches (HMC5883 ID is null-terminated ASCII string "H43") */ - char id[4]; - - PIOS_HMC5883_ReadID((uint8_t *)id); - if ((id[0] != 'H') || (id[1] != '4') || (id[2] != '3')) { // Expect H43 - return -1; - } - - /* Backup existing configuration */ - if (PIOS_HMC5883_Read(PIOS_HMC5883_CONFIG_REG_A, registers, 3) != 0) { - return -1; - } - - /* Stop the device and read out last value */ - PIOS_DELAY_WaitmS(10); - if (PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, PIOS_HMC5883_MODE_IDLE) != 0) { - return -1; - } - if (PIOS_HMC5883_Read(PIOS_HMC5883_DATAOUT_STATUS_REG, &status, 1) != 0) { - return -1; - } - if (PIOS_HMC5883_ReadMag(values) != 0) { - return -1; - } - - /* - * Put HMC5883 into self test mode - * This is done by placing measurement config into positive (0x01) or negative (0x10) bias - * and then placing the mode register into single-measurement mode. This causes the HMC5883 - * to create an artificial magnetic field of ~1.1 Gauss. - * - * If gain were PIOS_HMC5883_GAIN_2_5, for example, X and Y will read around +766 LSB - * (1.16 Ga * 660 LSB/Ga) and Z would read around +713 LSB (1.08 Ga * 660 LSB/Ga) - * - * Changing measurement config back to PIOS_HMC5883_MEASCONF_NORMAL will leave self-test mode. - */ - PIOS_DELAY_WaitmS(10); - if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_A, PIOS_HMC5883_MEASCONF_BIAS_POS | PIOS_HMC5883_ODR_15) != 0) { - return -1; - } - PIOS_DELAY_WaitmS(10); - if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_B, PIOS_HMC5883_GAIN_8_1) != 0) { - return -1; - } - PIOS_DELAY_WaitmS(10); - if (PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, PIOS_HMC5883_MODE_SINGLE) != 0) { - return -1; - } - - /* Must wait for value to be updated */ - PIOS_DELAY_WaitmS(200); - - if (PIOS_HMC5883_ReadMag(values) != 0) { - return -1; - } - - /* - if(abs(values[0] - 766) > 20) - failed |= 1; - if(abs(values[1] - 766) > 20) - failed |= 1; - if(abs(values[2] - 713) > 20) - failed |= 1; - */ - - PIOS_HMC5883_Read(PIOS_HMC5883_CONFIG_REG_A, &ctrl_a_read, 1); - PIOS_HMC5883_Read(PIOS_HMC5883_CONFIG_REG_B, &ctrl_b_read, 1); - PIOS_HMC5883_Read(PIOS_HMC5883_MODE_REG, &mode_read, 1); - PIOS_HMC5883_Read(PIOS_HMC5883_DATAOUT_STATUS_REG, &status, 1); - - - /* Restore backup configuration */ - PIOS_DELAY_WaitmS(10); - if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_A, registers[0]) != 0) { - return -1; - } - PIOS_DELAY_WaitmS(10); - if (PIOS_HMC5883_Write(PIOS_HMC5883_CONFIG_REG_B, registers[1]) != 0) { - return -1; - } - PIOS_DELAY_WaitmS(10); - if (PIOS_HMC5883_Write(PIOS_HMC5883_MODE_REG, registers[2]) != 0) { - return -1; - } - - return failed; -} - -/** - * @brief IRQ Handler - */ -bool PIOS_HMC5883_IRQHandler(void) -{ - pios_hmc5883_data_ready = true; - - return false; -} - -#endif /* PIOS_INCLUDE_HMC5883 */ - -/** - * @} - * @} - */ diff --git a/flight/pios/inc/pios_hmc5883.h b/flight/pios/inc/pios_hmc5883.h deleted file mode 100644 index 6d34dae20..000000000 --- a/flight/pios/inc/pios_hmc5883.h +++ /dev/null @@ -1,116 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup PIOS PIOS Core hardware abstraction layer - * @{ - * @addtogroup PIOS_HMC5883 HMC5883 Functions - * @brief Deals with the hardware interface to the magnetometers - * @{ - * - * @file pios_hmc5883.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. - * @brief HMC5883 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_HMC5883_H -#define PIOS_HMC5883_H - -/* HMC5883 Addresses */ -#define PIOS_HMC5883_I2C_ADDR 0x1E -#define PIOS_HMC5883_I2C_READ_ADDR 0x3D -#define PIOS_HMC5883_I2C_WRITE_ADDR 0x3C -#define PIOS_HMC5883_CONFIG_REG_A (uint8_t)0x00 -#define PIOS_HMC5883_CONFIG_REG_B (uint8_t)0x01 -#define PIOS_HMC5883_MODE_REG (uint8_t)0x02 -#define PIOS_HMC5883_DATAOUT_XMSB_REG 0x03 -#define PIOS_HMC5883_DATAOUT_XLSB_REG 0x04 -#define PIOS_HMC5883_DATAOUT_ZMSB_REG 0x05 -#define PIOS_HMC5883_DATAOUT_ZLSB_REG 0x06 -#define PIOS_HMC5883_DATAOUT_YMSB_REG 0x07 -#define PIOS_HMC5883_DATAOUT_YLSB_REG 0x08 -#define PIOS_HMC5883_DATAOUT_STATUS_REG 0x09 -#define PIOS_HMC5883_DATAOUT_IDA_REG 0x0A -#define PIOS_HMC5883_DATAOUT_IDB_REG 0x0B -#define PIOS_HMC5883_DATAOUT_IDC_REG 0x0C - -/* Output Data Rate */ -#define PIOS_HMC5883_ODR_0_75 0x00 -#define PIOS_HMC5883_ODR_1_5 0x04 -#define PIOS_HMC5883_ODR_3 0x08 -#define PIOS_HMC5883_ODR_7_5 0x0C -#define PIOS_HMC5883_ODR_15 0x10 -#define PIOS_HMC5883_ODR_30 0x14 -#define PIOS_HMC5883_ODR_75 0x18 - -/* Measure configuration */ -#define PIOS_HMC5883_MEASCONF_NORMAL 0x00 -#define PIOS_HMC5883_MEASCONF_BIAS_POS 0x01 -#define PIOS_HMC5883_MEASCONF_BIAS_NEG 0x02 - -/* Gain settings */ -#define PIOS_HMC5883_GAIN_0_88 0x00 -#define PIOS_HMC5883_GAIN_1_3 0x20 -#define PIOS_HMC5883_GAIN_1_9 0x40 -#define PIOS_HMC5883_GAIN_2_5 0x60 -#define PIOS_HMC5883_GAIN_4_0 0x80 -#define PIOS_HMC5883_GAIN_4_7 0xA0 -#define PIOS_HMC5883_GAIN_5_6 0xC0 -#define PIOS_HMC5883_GAIN_8_1 0xE0 - -/* Modes */ -#define PIOS_HMC5883_MODE_CONTINUOUS 0x00 -#define PIOS_HMC5883_MODE_SINGLE 0x01 -#define PIOS_HMC5883_MODE_IDLE 0x02 -#define PIOS_HMC5883_MODE_SLEEP 0x03 - -/* Sensitivity Conversion Values */ -#define PIOS_HMC5883_Sensitivity_0_88Ga 1370 // LSB/Ga -#define PIOS_HMC5883_Sensitivity_1_3Ga 1090 // LSB/Ga -#define PIOS_HMC5883_Sensitivity_1_9Ga 820 // LSB/Ga -#define PIOS_HMC5883_Sensitivity_2_5Ga 660 // LSB/Ga -#define PIOS_HMC5883_Sensitivity_4_0Ga 440 // LSB/Ga -#define PIOS_HMC5883_Sensitivity_4_7Ga 390 // LSB/Ga -#define PIOS_HMC5883_Sensitivity_5_6Ga 330 // LSB/Ga -#define PIOS_HMC5883_Sensitivity_8_1Ga 230 // LSB/Ga --> NOT RECOMMENDED - - -struct pios_hmc5883_cfg { -#ifdef PIOS_HMC5883_HAS_GPIOS - const struct pios_exti_cfg *exti_cfg; /* Pointer to the EXTI configuration */ -#endif - uint8_t M_ODR; /* OUTPUT DATA RATE --> here below the relative define (See datasheet page 11 for more details) */ - uint8_t Meas_Conf; /* Measurement Configuration,: Normal, positive bias, or negative bias --> here below the relative define */ - uint8_t Gain; /* Gain Configuration, select the full scale --> here below the relative define (See datasheet page 11 for more details) */ - uint8_t Mode; -}; - -/* Public Functions */ -extern void PIOS_HMC5883_Init(const struct pios_hmc5883_cfg *cfg); -extern bool PIOS_HMC5883_NewDataAvailable(void); -extern int32_t PIOS_HMC5883_ReadMag(int16_t out[3]); -extern uint8_t PIOS_HMC5883_ReadID(uint8_t out[4]); -extern int32_t PIOS_HMC5883_Test(void); -extern bool PIOS_HMC5883_IRQHandler(); - -#endif /* PIOS_HMC5883_H */ - -/** - * @} - * @} - */ diff --git a/flight/pios/inc/pios_hmc5x83.h b/flight/pios/inc/pios_hmc5x83.h index 94f53b2f9..949966ef5 100644 --- a/flight/pios/inc/pios_hmc5x83.h +++ b/flight/pios/inc/pios_hmc5x83.h @@ -108,7 +108,7 @@ extern const struct pios_hmc5x83_io_driver PIOS_HMC5x83_I2C_DRIVER; #endif struct pios_hmc5x83_cfg { -#ifdef PIOS_HMC5x83_HAS_GPIOS +#ifdef PIOS_HMC5X83_HAS_GPIOS const struct pios_exti_cfg *exti_cfg; /* Pointer to the EXTI configuration */ #endif uint8_t M_ODR; // OUTPUT DATA RATE --> here below the relative define (See datasheet page 11 for more details) */ diff --git a/flight/pios/pios.h b/flight/pios/pios.h index 1bb1ea968..124dc1d8a 100644 --- a/flight/pios/pios.h +++ b/flight/pios/pios.h @@ -204,12 +204,6 @@ #include #endif -#ifdef PIOS_INCLUDE_HMC5883 -/* HMC5883 3-Axis Digital Compass */ -/* #define PIOS_HMC5883_HAS_GPIOS */ -#include -#endif - #ifdef PIOS_INCLUDE_HMC5X83 /* HMC5883/HMC5983 3-Axis Digital Compass */ /* #define PIOS_HMC5x83_HAS_GPIOS */ diff --git a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h index e7da296e9..95332df45 100644 --- a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h +++ b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h @@ -84,7 +84,7 @@ #define PIOS_INCLUDE_MPU6000 #define PIOS_MPU6000_ACCEL /* #define PIOS_INCLUDE_HMC5843 */ -/* #define PIOS_INCLUDE_HMC5883 */ +/* #define PIOS_INCLUDE_HMC5X83 */ /* #define PIOS_HMC5883_HAS_GPIOS */ /* #define PIOS_INCLUDE_BMP085 */ /* #define PIOS_INCLUDE_MS5611 */ diff --git a/flight/targets/boards/discoveryf4bare/firmware/inc/pios_config.h b/flight/targets/boards/discoveryf4bare/firmware/inc/pios_config.h index a55d4cb09..44065be33 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/inc/pios_config.h +++ b/flight/targets/boards/discoveryf4bare/firmware/inc/pios_config.h @@ -84,8 +84,8 @@ // #define PIOS_INCLUDE_MPU6000 // #define PIOS_MPU6000_ACCEL /* #define PIOS_INCLUDE_HMC5843 */ -// #define PIOS_INCLUDE_HMC5883 -// #define PIOS_HMC5883_HAS_GPIOS +// #define PIOS_INCLUDE_HMC5X83 +// #define PIOS_HMC5X83_HAS_GPIOS /* #define PIOS_INCLUDE_BMP085 */ // #define PIOS_INCLUDE_MS5611 // #define PIOS_INCLUDE_MPXV diff --git a/flight/targets/boards/discoveryf4bare/firmware/pios_board.c b/flight/targets/boards/discoveryf4bare/firmware/pios_board.c index 687f1c5f7..2e8971636 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/pios_board.c +++ b/flight/targets/boards/discoveryf4bare/firmware/pios_board.c @@ -90,10 +90,10 @@ void PIOS_ADC_DMC_irq_handler(void) #endif /* if defined(PIOS_INCLUDE_ADC) */ -#if defined(PIOS_INCLUDE_HMC5883) -#include "pios_hmc5883.h" -static const struct pios_exti_cfg pios_exti_hmc5883_cfg __exti_config = { - .vector = PIOS_HMC5883_IRQHandler, +#if defined(PIOS_INCLUDE_HMC5X83) +#include "pios_hmc5x83.h" +static const struct pios_exti_cfg pios_exti_hmc5x83_cfg __exti_config = { + .vector = PIOS_HMC5x83_IRQHandler, .line = EXTI_Line7, .pin = { .gpio = GPIOB, @@ -123,14 +123,15 @@ static const struct pios_exti_cfg pios_exti_hmc5883_cfg __exti_config = { }, }; -static const struct pios_hmc5883_cfg pios_hmc5883_cfg = { - .exti_cfg = &pios_exti_hmc5883_cfg, - .M_ODR = PIOS_HMC5883_ODR_75, - .Meas_Conf = PIOS_HMC5883_MEASCONF_NORMAL, - .Gain = PIOS_HMC5883_GAIN_1_9, - .Mode = PIOS_HMC5883_MODE_CONTINUOUS, +static const struct pios_hmc5x83_cfg pios_hmc5x83_cfg = { + .exti_cfg = &pios_exti_hmc5x83_cfg, + .M_ODR = PIOS_HMC5x83_ODR_75, + .Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL, + .Gain = PIOS_HMC5x83_GAIN_1_9, + .Mode = PIOS_HMC5x83_MODE_CONTINUOUS, + .Driver = &PIOS_HMC5x83_I2C_DRIVER, }; -#endif /* PIOS_INCLUDE_HMC5883 */ +#endif /* PIOS_INCLUDE_HMC5X83 */ /** * Configuration for the MS5611 chip @@ -929,8 +930,8 @@ void PIOS_Board_Init(void) PIOS_ADC_Init(&pios_adc_cfg); #endif -#if defined(PIOS_INCLUDE_HMC5883) - PIOS_HMC5883_Init(&pios_hmc5883_cfg); +#if defined(PIOS_INCLUDE_HMC5X83) + PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, pios_i2c_mag_pressure_adapter_id, 0); #endif #if defined(PIOS_INCLUDE_MS5611) diff --git a/flight/targets/boards/oplinkmini/firmware/inc/pios_config.h b/flight/targets/boards/oplinkmini/firmware/inc/pios_config.h index 8dd30c0f3..7717beb52 100644 --- a/flight/targets/boards/oplinkmini/firmware/inc/pios_config.h +++ b/flight/targets/boards/oplinkmini/firmware/inc/pios_config.h @@ -81,7 +81,7 @@ /* #define PIOS_INCLUDE_MPU6000 */ /* #define PIOS_MPU6000_ACCEL */ /* #define PIOS_INCLUDE_HMC5843 */ -/* #define PIOS_INCLUDE_HMC5883 */ +/* #define PIOS_INCLUDE_HMC5X83 */ /* #define PIOS_HMC5883_HAS_GPIOS */ /* #define PIOS_INCLUDE_BMP085 */ /* #define PIOS_INCLUDE_MS5611 */ diff --git a/flight/targets/boards/osd/firmware/inc/pios_config.h b/flight/targets/boards/osd/firmware/inc/pios_config.h index 33542a953..66e178349 100644 --- a/flight/targets/boards/osd/firmware/inc/pios_config.h +++ b/flight/targets/boards/osd/firmware/inc/pios_config.h @@ -81,8 +81,8 @@ /* #define PIOS_INCLUDE_MPU6000 */ /* #define PIOS_MPU6000_ACCEL */ /* #define PIOS_INCLUDE_HMC5843 */ -#define PIOS_INCLUDE_HMC5883 -/* #define PIOS_HMC5883_HAS_GPIOS */ +#define PIOS_INCLUDE_HMC5X83 +/* #define PIOS_HMC5X83_HAS_GPIOS */ #define PIOS_INCLUDE_BMP085 /* #define PIOS_INCLUDE_MS5611 */ /* #define PIOS_INCLUDE_MPXV */ diff --git a/flight/targets/boards/revolution/firmware/inc/pios_config.h b/flight/targets/boards/revolution/firmware/inc/pios_config.h index 49cd0d8e6..5ee827783 100644 --- a/flight/targets/boards/revolution/firmware/inc/pios_config.h +++ b/flight/targets/boards/revolution/firmware/inc/pios_config.h @@ -84,8 +84,8 @@ #define PIOS_INCLUDE_MPU6000 #define PIOS_MPU6000_ACCEL /* #define PIOS_INCLUDE_HMC5843 */ -#define PIOS_INCLUDE_HMC5883 -#define PIOS_HMC5883_HAS_GPIOS +#define PIOS_INCLUDE_HMC5X83 +#define PIOS_HMC5X83_HAS_GPIOS /* #define PIOS_INCLUDE_BMP085 */ #define PIOS_INCLUDE_MS5611 #define PIOS_INCLUDE_MPXV diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c index 4901cc432..b97adb07a 100644 --- a/flight/targets/boards/revolution/firmware/pios_board.c +++ b/flight/targets/boards/revolution/firmware/pios_board.c @@ -91,10 +91,10 @@ void PIOS_ADC_DMC_irq_handler(void) #endif /* if defined(PIOS_INCLUDE_ADC) */ -#if defined(PIOS_INCLUDE_HMC5883) -#include "pios_hmc5883.h" -static const struct pios_exti_cfg pios_exti_hmc5883_cfg __exti_config = { - .vector = PIOS_HMC5883_IRQHandler, +#if defined(PIOS_INCLUDE_HMC5X83) +#include "pios_hmc5x83.h" +static const struct pios_exti_cfg pios_exti_hmc5x83_cfg __exti_config = { + .vector = PIOS_HMC5x83_IRQHandler, .line = EXTI_Line7, .pin = { .gpio = GPIOB, @@ -124,14 +124,15 @@ static const struct pios_exti_cfg pios_exti_hmc5883_cfg __exti_config = { }, }; -static const struct pios_hmc5883_cfg pios_hmc5883_cfg = { - .exti_cfg = &pios_exti_hmc5883_cfg, - .M_ODR = PIOS_HMC5883_ODR_75, - .Meas_Conf = PIOS_HMC5883_MEASCONF_NORMAL, - .Gain = PIOS_HMC5883_GAIN_1_9, - .Mode = PIOS_HMC5883_MODE_CONTINUOUS, +static const struct pios_hmc5x83_cfg pios_hmc5x83_cfg = { + .exti_cfg = &pios_exti_hmc5x83_cfg, + .M_ODR = PIOS_HMC5x83_ODR_75, + .Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL, + .Gain = PIOS_HMC5x83_GAIN_1_9, + .Mode = PIOS_HMC5x83_MODE_CONTINUOUS, + .Driver = &PIOS_HMC5x83_I2C_DRIVER, }; -#endif /* PIOS_INCLUDE_HMC5883 */ +#endif /* PIOS_INCLUDE_HMC5X83 */ /** * Configuration for the MS5611 chip @@ -944,8 +945,8 @@ void PIOS_Board_Init(void) PIOS_ADC_Init(&pios_adc_cfg); #endif -#if defined(PIOS_INCLUDE_HMC5883) - PIOS_HMC5883_Init(&pios_hmc5883_cfg); +#if defined(PIOS_INCLUDE_HMC5X83) + PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, pios_i2c_mag_pressure_adapter_id, 0); #endif #if defined(PIOS_INCLUDE_MS5611) diff --git a/flight/targets/boards/revoproto/firmware/inc/pios_config.h b/flight/targets/boards/revoproto/firmware/inc/pios_config.h index 6077225aa..897f9b360 100644 --- a/flight/targets/boards/revoproto/firmware/inc/pios_config.h +++ b/flight/targets/boards/revoproto/firmware/inc/pios_config.h @@ -81,8 +81,8 @@ #define PIOS_INCLUDE_MPU6000 #define PIOS_MPU6000_ACCEL /* #define PIOS_INCLUDE_HMC5843 */ -#define PIOS_INCLUDE_HMC5883 -#define PIOS_HMC5883_HAS_GPIOS +#define PIOS_INCLUDE_HMC5X83 +#define PIOS_HMC5X83_HAS_GPIOS /* #define PIOS_INCLUDE_BMP085 */ #define PIOS_INCLUDE_MS5611 #define PIOS_INCLUDE_MPXV diff --git a/flight/targets/boards/revoproto/firmware/pios_board.c b/flight/targets/boards/revoproto/firmware/pios_board.c index e628d615e..de2c5982d 100644 --- a/flight/targets/boards/revoproto/firmware/pios_board.c +++ b/flight/targets/boards/revoproto/firmware/pios_board.c @@ -81,10 +81,10 @@ void PIOS_ADC_DMC_irq_handler(void) #endif /* if defined(PIOS_INCLUDE_ADC) */ -#if defined(PIOS_INCLUDE_HMC5883) -#include "pios_hmc5883.h" -static const struct pios_exti_cfg pios_exti_hmc5883_cfg __exti_config = { - .vector = PIOS_HMC5883_IRQHandler, +#if defined(PIOS_INCLUDE_HMC5X83) +#include "pios_hmc5x83.h" +static const struct pios_exti_cfg pios_exti_hmc5x83_cfg __exti_config = { + .vector = PIOS_HMC5x83_IRQHandler, .line = EXTI_Line5, .pin = { .gpio = GPIOB, @@ -114,14 +114,15 @@ static const struct pios_exti_cfg pios_exti_hmc5883_cfg __exti_config = { }, }; -static const struct pios_hmc5883_cfg pios_hmc5883_cfg = { - .exti_cfg = &pios_exti_hmc5883_cfg, - .M_ODR = PIOS_HMC5883_ODR_75, - .Meas_Conf = PIOS_HMC5883_MEASCONF_NORMAL, - .Gain = PIOS_HMC5883_GAIN_1_9, - .Mode = PIOS_HMC5883_MODE_CONTINUOUS, +static const struct pios_hmc5x83_cfg pios_hmc5x83_cfg = { + .exti_cfg = &pios_exti_hmc5x83_cfg, + .M_ODR = PIOS_HMC5x83_ODR_75, + .Meas_Conf = PIOS_HMC5x83_MEASCONF_NORMAL, + .Gain = PIOS_HMC5x83_GAIN_1_9, + .Mode = PIOS_HMC5x83_MODE_CONTINUOUS, + .Driver = &PIOS_HMC5x83_I2C_DRIVER, }; -#endif /* PIOS_INCLUDE_HMC5883 */ +#endif /* PIOS_INCLUDE_HMC5X83 */ /** * Configuration for the MS5611 chip @@ -938,8 +939,8 @@ void PIOS_Board_Init(void) PIOS_ADC_Init(&pios_adc_cfg); #endif -#if defined(PIOS_INCLUDE_HMC5883) - PIOS_HMC5883_Init(&pios_hmc5883_cfg); +#if defined(PIOS_INCLUDE_HMC5X83) + PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, pios_i2c_pressure_adapter_id, 0); #endif #if defined(PIOS_INCLUDE_MS5611) diff --git a/flight/targets/boards/simposix/firmware/inc/pios_config.h b/flight/targets/boards/simposix/firmware/inc/pios_config.h index 9194e68b9..0dc6b7aaa 100644 --- a/flight/targets/boards/simposix/firmware/inc/pios_config.h +++ b/flight/targets/boards/simposix/firmware/inc/pios_config.h @@ -63,7 +63,7 @@ /* Select the sensors to include */ // #define PIOS_INCLUDE_BMA180 -// #define PIOS_INCLUDE_HMC5883 +// #define PIOS_INCLUDE_HMC5X83 // #define PIOS_INCLUDE_MPU6000 // #define PIOS_MPU6000_ACCEL // #define PIOS_INCLUDE_L3GD20 diff --git a/make/apps-defs.mk b/make/apps-defs.mk index a80fc6fc5..72381423e 100644 --- a/make/apps-defs.mk +++ b/make/apps-defs.mk @@ -64,7 +64,7 @@ SRC += $(PIOSCOMMON)/pios_etasv3.c SRC += $(PIOSCOMMON)/pios_gcsrcvr.c SRC += $(PIOSCOMMON)/pios_hcsr04.c SRC += $(PIOSCOMMON)/pios_hmc5843.c -SRC += $(PIOSCOMMON)/pios_hmc5883.c +SRC += $(PIOSCOMMON)/pios_hmc5x83.c SRC += $(PIOSCOMMON)/pios_i2c_esc.c SRC += $(PIOSCOMMON)/pios_l3gd20.c SRC += $(PIOSCOMMON)/pios_mpu6000.c From 6a3cb7b808177386519158e87d83675fd68deefc Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sat, 19 Jul 2014 18:22:44 +0200 Subject: [PATCH 24/26] OP-1403 - Add support for multiple hmc* devices --- flight/pios/common/pios_hmc5x83.c | 212 ++++++++++++++++++------------ flight/pios/inc/pios_hmc5x83.h | 20 +-- 2 files changed, 136 insertions(+), 96 deletions(-) diff --git a/flight/pios/common/pios_hmc5x83.c b/flight/pios/common/pios_hmc5x83.c index d1854632f..73b4902d9 100644 --- a/flight/pios/common/pios_hmc5x83.c +++ b/flight/pios/common/pios_hmc5x83.c @@ -30,46 +30,79 @@ #include "pios.h" #include +#include #ifdef PIOS_INCLUDE_HMC5X83 +#define PIOS_HMC5X83_MAGIC 0x4d783833 /* Global Variables */ /* Local Types */ -/* Local Variables */ -volatile bool pios_hmc5x83_data_ready; +typedef struct { + uint32_t magic; + const struct pios_hmc5x83_cfg *cfg; + uint32_t port_id; + uint8_t slave_num; + uint8_t CTRLB; + volatile bool data_ready; +} pios_hmc5x83_dev_data_t; -static int32_t PIOS_HMC5x83_Config(const struct pios_hmc5x83_cfg *cfg); -// static int32_t PIOS_HMC5x83_Read(uint8_t address, uint8_t *buffer, uint8_t len); -// static int32_t PIOS_HMC5x83_Write(uint8_t address, uint8_t buffer); +static int32_t PIOS_HMC5x83_Config(pios_hmc5x83_dev_data_t *dev); + +/** + * Allocate the device setting structure + * @return pios_hmc5x83_dev_data_t pointer to newly created structure + */ +pios_hmc5x83_dev_data_t *dev_alloc() +{ + pios_hmc5x83_dev_data_t *dev = (pios_hmc5x83_dev_data_t *)pios_malloc(sizeof(pios_hmc5x83_dev_data_t)); + + PIOS_DEBUG_Assert(dev); + memset(dev, 0x00, sizeof(pios_hmc5x83_dev_data_t)); + dev->magic = PIOS_HMC5X83_MAGIC; + return dev; +} + +/** + * Validate a pios_hmc5x83_dev_t handler and return the related pios_hmc5x83_dev_data_t pointer + * @param dev device handler + * @return the device data structure + */ +pios_hmc5x83_dev_data_t *dev_validate(pios_hmc5x83_dev_t dev) +{ + pios_hmc5x83_dev_data_t *dev_data = (pios_hmc5x83_dev_data_t *)dev; + + PIOS_DEBUG_Assert(dev_data->magic == PIOS_HMC5X83_MAGIC); + return dev_data; +} -static const struct pios_hmc5x83_cfg *dev_cfg; -static uint32_t dev_port_id; -static uint8_t dev_slave_num; /** * @brief Initialize the HMC5x83 magnetometer sensor. * @return none */ -void PIOS_HMC5x83_Init(const struct pios_hmc5x83_cfg *cfg, uint32_t port_id, uint8_t slave_num) +pios_hmc5x83_dev_t PIOS_HMC5x83_Init(const struct pios_hmc5x83_cfg *cfg, uint32_t port_id, uint8_t slave_num) { - dev_cfg = cfg; // store config before enabling interrupt - dev_port_id = port_id; - dev_slave_num = slave_num; + pios_hmc5x83_dev_data_t *dev = dev_alloc(); + + dev->cfg = cfg; // store config before enabling interrupt + dev->port_id = port_id; + dev->slave_num = slave_num; #ifdef PIOS_HMC5X83_HAS_GPIOS PIOS_EXTI_Init(cfg->exti_cfg); #endif - int32_t val = PIOS_HMC5x83_Config(cfg); - + int32_t val = PIOS_HMC5x83_Config(dev); PIOS_Assert(val == 0); - pios_hmc5x83_data_ready = false; + dev->data_ready = false; + return (pios_hmc5x83_dev_t)dev; } /** * @brief Initialize the HMC5x83 magnetometer sensor * \return none + * \param[in] pios_hmc5x83_dev_data_t device config to be used. * \param[in] PIOS_HMC5x83_ConfigTypeDef struct to be used to configure sensor. * * CTRL_REGA: Control Register A @@ -126,31 +159,32 @@ void PIOS_HMC5x83_Init(const struct pios_hmc5x83_cfg *cfg, uint32_t port_id, uin * 1 | 0 | Negative Bias * 1 | 1 | Sleep Mode */ -static uint8_t CTRLB = 0x00; -static int32_t PIOS_HMC5x83_Config(const struct pios_hmc5x83_cfg *cfg) +static int32_t PIOS_HMC5x83_Config(pios_hmc5x83_dev_data_t *dev) { uint8_t CTRLA = 0x00; uint8_t MODE = 0x00; - CTRLB = 0; + const struct pios_hmc5x83_cfg *cfg = dev->cfg; + + dev->CTRLB = 0; CTRLA |= (uint8_t)(cfg->M_ODR | cfg->Meas_Conf); CTRLA |= cfg->TempCompensation ? PIOS_HMC5x83_CTRLA_TEMP : 0; - CTRLB |= (uint8_t)(cfg->Gain); + dev->CTRLB |= (uint8_t)(cfg->Gain); MODE |= (uint8_t)(cfg->Mode); // CRTL_REGA - if (dev_cfg->Driver->Write(PIOS_HMC5x83_CONFIG_REG_A, CTRLA) != 0) { + if (cfg->Driver->Write((pios_hmc5x83_dev_t)dev, PIOS_HMC5x83_CONFIG_REG_A, CTRLA) != 0) { return -1; } // CRTL_REGB - if (dev_cfg->Driver->Write(PIOS_HMC5x83_CONFIG_REG_B, CTRLB) != 0) { + if (cfg->Driver->Write((pios_hmc5x83_dev_t)dev, PIOS_HMC5x83_CONFIG_REG_B, dev->CTRLB) != 0) { return -1; } // Mode register - if (dev_cfg->Driver->Write(PIOS_HMC5x83_MODE_REG, MODE) != 0) { + if (cfg->Driver->Write((pios_hmc5x83_dev_t)dev, PIOS_HMC5x83_MODE_REG, MODE) != 0) { return -1; } @@ -159,21 +193,24 @@ static int32_t PIOS_HMC5x83_Config(const struct pios_hmc5x83_cfg *cfg) /** * @brief Read current X, Z, Y values (in that order) + * \param[in] dev device handler * \param[out] int16_t array of size 3 to store X, Z, and Y magnetometer readings * \return 0 for success or -1 for failure */ -int32_t PIOS_HMC5x83_ReadMag(int16_t out[3]) +int32_t PIOS_HMC5x83_ReadMag(pios_hmc5x83_dev_t handler, int16_t out[3]) { - pios_hmc5x83_data_ready = false; + pios_hmc5x83_dev_data_t *dev = dev_validate(handler); + + dev->data_ready = false; uint8_t buffer[6]; int32_t temp; int32_t sensitivity; - if (dev_cfg->Driver->Read(PIOS_HMC5x83_DATAOUT_XMSB_REG, buffer, 6) != 0) { + if (dev->cfg->Driver->Read(handler, PIOS_HMC5x83_DATAOUT_XMSB_REG, buffer, 6) != 0) { return -1; } - switch (CTRLB & 0xE0) { + switch (dev->CTRLB & 0xE0) { case 0x00: sensitivity = PIOS_HMC5x83_Sensitivity_0_88Ga; break; @@ -213,7 +250,7 @@ int32_t PIOS_HMC5x83_ReadMag(int16_t out[3]) out[1] = temp; // This should not be necessary but for some reason it is coming out of continuous conversion mode - dev_cfg->Driver->Write(PIOS_HMC5x83_MODE_REG, PIOS_HMC5x83_MODE_CONTINUOUS); + dev->cfg->Driver->Write(handler, PIOS_HMC5x83_MODE_REG, PIOS_HMC5x83_MODE_CONTINUOUS); return 0; } @@ -224,9 +261,10 @@ int32_t PIOS_HMC5x83_ReadMag(int16_t out[3]) * \param[out] uint8_t array of size 4 to store HMC5x83 ID. * \return 0 if successful, -1 if not */ -uint8_t PIOS_HMC5x83_ReadID(uint8_t out[4]) +uint8_t PIOS_HMC5x83_ReadID(pios_hmc5x83_dev_t handler, uint8_t out[4]) { - uint8_t retval = dev_cfg->Driver->Read(PIOS_HMC5x83_DATAOUT_IDA_REG, out, 3); + pios_hmc5x83_dev_data_t *dev = dev_validate(handler); + uint8_t retval = dev->cfg->Driver->Read(handler, PIOS_HMC5x83_DATAOUT_IDA_REG, out, 3); out[3] = '\0'; return retval; @@ -237,16 +275,18 @@ uint8_t PIOS_HMC5x83_ReadID(uint8_t out[4]) * \return true if new data is available * \return false if new data is not available */ -bool PIOS_HMC5x83_NewDataAvailable(void) +bool PIOS_HMC5x83_NewDataAvailable(pios_hmc5x83_dev_t handler) { - return pios_hmc5x83_data_ready; + pios_hmc5x83_dev_data_t *dev = dev_validate(handler); + + return dev->data_ready; } /** * @brief Run self-test operation. Do not call this during operational use!! * \return 0 if success, -1 if test failed */ -int32_t PIOS_HMC5x83_Test(void) +int32_t PIOS_HMC5x83_Test(pios_hmc5x83_dev_t handler) { int32_t failed = 0; uint8_t registers[3] = { 0, 0, 0 }; @@ -255,30 +295,30 @@ int32_t PIOS_HMC5x83_Test(void) uint8_t ctrl_b_read; uint8_t mode_read; int16_t values[3]; - + pios_hmc5x83_dev_data_t *dev = dev_validate(handler); /* Verify that ID matches (HMC5x83 ID is null-terminated ASCII string "H43") */ char id[4]; - PIOS_HMC5x83_ReadID((uint8_t *)id); + PIOS_HMC5x83_ReadID(handler, (uint8_t *)id); if ((id[0] != 'H') || (id[1] != '4') || (id[2] != '3')) { // Expect H43 return -1; } /* Backup existing configuration */ - if (dev_cfg->Driver->Read(PIOS_HMC5x83_CONFIG_REG_A, registers, 3) != 0) { + if (dev->cfg->Driver->Read(handler, PIOS_HMC5x83_CONFIG_REG_A, registers, 3) != 0) { return -1; } /* Stop the device and read out last value */ PIOS_DELAY_WaitmS(10); - if (dev_cfg->Driver->Write(PIOS_HMC5x83_MODE_REG, PIOS_HMC5x83_MODE_IDLE) != 0) { + if (dev->cfg->Driver->Write(handler, PIOS_HMC5x83_MODE_REG, PIOS_HMC5x83_MODE_IDLE) != 0) { return -1; } - if (dev_cfg->Driver->Read(PIOS_HMC5x83_DATAOUT_STATUS_REG, &status, 1) != 0) { + if (dev->cfg->Driver->Read(handler, PIOS_HMC5x83_DATAOUT_STATUS_REG, &status, 1) != 0) { return -1; } - if (PIOS_HMC5x83_ReadMag(values) != 0) { + if (PIOS_HMC5x83_ReadMag(handler, values) != 0) { return -1; } @@ -294,51 +334,42 @@ int32_t PIOS_HMC5x83_Test(void) * Changing measurement config back to PIOS_HMC5x83_MEASCONF_NORMAL will leave self-test mode. */ PIOS_DELAY_WaitmS(10); - if (dev_cfg->Driver->Write(PIOS_HMC5x83_CONFIG_REG_A, PIOS_HMC5x83_MEASCONF_BIAS_POS | PIOS_HMC5x83_ODR_15) != 0) { + if (dev->cfg->Driver->Write(handler, PIOS_HMC5x83_CONFIG_REG_A, PIOS_HMC5x83_MEASCONF_BIAS_POS | PIOS_HMC5x83_ODR_15) != 0) { return -1; } PIOS_DELAY_WaitmS(10); - if (dev_cfg->Driver->Write(PIOS_HMC5x83_CONFIG_REG_B, PIOS_HMC5x83_GAIN_8_1) != 0) { + if (dev->cfg->Driver->Write(handler, PIOS_HMC5x83_CONFIG_REG_B, PIOS_HMC5x83_GAIN_8_1) != 0) { return -1; } PIOS_DELAY_WaitmS(10); - if (dev_cfg->Driver->Write(PIOS_HMC5x83_MODE_REG, PIOS_HMC5x83_MODE_SINGLE) != 0) { + if (dev->cfg->Driver->Write(handler, PIOS_HMC5x83_MODE_REG, PIOS_HMC5x83_MODE_SINGLE) != 0) { return -1; } /* Must wait for value to be updated */ PIOS_DELAY_WaitmS(200); - if (PIOS_HMC5x83_ReadMag(values) != 0) { + if (PIOS_HMC5x83_ReadMag(handler, values) != 0) { return -1; } - /* - if(abs(values[0] - 766) > 20) - failed |= 1; - if(abs(values[1] - 766) > 20) - failed |= 1; - if(abs(values[2] - 713) > 20) - failed |= 1; - */ - - dev_cfg->Driver->Read(PIOS_HMC5x83_CONFIG_REG_A, &ctrl_a_read, 1); - dev_cfg->Driver->Read(PIOS_HMC5x83_CONFIG_REG_B, &ctrl_b_read, 1); - dev_cfg->Driver->Read(PIOS_HMC5x83_MODE_REG, &mode_read, 1); - dev_cfg->Driver->Read(PIOS_HMC5x83_DATAOUT_STATUS_REG, &status, 1); + dev->cfg->Driver->Read(handler, PIOS_HMC5x83_CONFIG_REG_A, &ctrl_a_read, 1); + dev->cfg->Driver->Read(handler, PIOS_HMC5x83_CONFIG_REG_B, &ctrl_b_read, 1); + dev->cfg->Driver->Read(handler, PIOS_HMC5x83_MODE_REG, &mode_read, 1); + dev->cfg->Driver->Read(handler, PIOS_HMC5x83_DATAOUT_STATUS_REG, &status, 1); /* Restore backup configuration */ PIOS_DELAY_WaitmS(10); - if (dev_cfg->Driver->Write(PIOS_HMC5x83_CONFIG_REG_A, registers[0]) != 0) { + if (dev->cfg->Driver->Write(handler, PIOS_HMC5x83_CONFIG_REG_A, registers[0]) != 0) { return -1; } PIOS_DELAY_WaitmS(10); - if (dev_cfg->Driver->Write(PIOS_HMC5x83_CONFIG_REG_B, registers[1]) != 0) { + if (dev->cfg->Driver->Write(handler, PIOS_HMC5x83_CONFIG_REG_B, registers[1]) != 0) { return -1; } PIOS_DELAY_WaitmS(10); - if (dev_cfg->Driver->Write(PIOS_HMC5x83_MODE_REG, registers[2]) != 0) { + if (dev->cfg->Driver->Write(handler, PIOS_HMC5x83_MODE_REG, registers[2]) != 0) { return -1; } @@ -348,35 +379,36 @@ int32_t PIOS_HMC5x83_Test(void) /** * @brief IRQ Handler */ -bool PIOS_HMC5x83_IRQHandler(void) +bool PIOS_HMC5x83_IRQHandler(pios_hmc5x83_dev_t handler) { - pios_hmc5x83_data_ready = true; + pios_hmc5x83_dev_data_t *dev = dev_validate(handler); + dev->data_ready = true; return false; } #ifdef PIOS_INCLUDE_SPI -int32_t PIOS_HMC5x83_SPI_Read(uint8_t address, uint8_t *buffer, uint8_t len); -int32_t PIOS_HMC5x83_SPI_Write(uint8_t address, uint8_t buffer); +int32_t PIOS_HMC5x83_SPI_Read(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t *buffer, uint8_t len); +int32_t PIOS_HMC5x83_SPI_Write(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t buffer); const struct pios_hmc5x83_io_driver PIOS_HMC5x83_SPI_DRIVER = { .Read = PIOS_HMC5x83_SPI_Read, .Write = PIOS_HMC5x83_SPI_Write, }; -static int32_t pios_hmc5x83_spi_claim_bus() +static int32_t pios_hmc5x83_spi_claim_bus(pios_hmc5x83_dev_data_t *dev) { - if (PIOS_SPI_ClaimBus(dev_port_id) < 0) { + if (PIOS_SPI_ClaimBus(dev->port_id) < 0) { return -1; } - PIOS_SPI_RC_PinSet(dev_port_id, dev_slave_num, 0); + PIOS_SPI_RC_PinSet(dev->port_id, dev->slave_num, 0); return 0; } -static void pios_hmc5x83_spi_release_bus() +static void pios_hmc5x83_spi_release_bus(pios_hmc5x83_dev_data_t *dev) { - PIOS_SPI_RC_PinSet(dev_port_id, dev_slave_num, 1); - PIOS_SPI_ReleaseBus(dev_port_id); + PIOS_SPI_RC_PinSet(dev->port_id, dev->slave_num, 1); + PIOS_SPI_ReleaseBus(dev->port_id); } /** * @brief Reads one or more bytes into a buffer @@ -387,22 +419,24 @@ static void pios_hmc5x83_spi_release_bus() * \return -1 if error during I2C transfer * \return -2 if unable to claim i2c device */ -int32_t PIOS_HMC5x83_SPI_Read(uint8_t address, uint8_t *buffer, uint8_t len) +int32_t PIOS_HMC5x83_SPI_Read(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t *buffer, uint8_t len) { - if (pios_hmc5x83_spi_claim_bus() < 0) { + pios_hmc5x83_dev_data_t *dev = dev_validate(handler); + + if (pios_hmc5x83_spi_claim_bus(dev) < 0) { return -1; } memset(buffer, 0xA5, len); - PIOS_SPI_TransferByte(dev_port_id, address | PIOS_HMC5x83_SPI_AUTOINCR_FLAG | PIOS_HMC5x83_SPI_READ_FLAG); + PIOS_SPI_TransferByte(dev->port_id, address | PIOS_HMC5x83_SPI_AUTOINCR_FLAG | PIOS_HMC5x83_SPI_READ_FLAG); // buffer[0] = address | PIOS_HMC5x83_SPI_AUTOINCR_FLAG | PIOS_HMC5x83_SPI_READ_FLAG; /* Copy the transfer data to the buffer */ - if (PIOS_SPI_TransferBlock(dev_port_id, NULL, buffer, len, NULL) < 0) { - pios_hmc5x83_spi_release_bus(); + if (PIOS_SPI_TransferBlock(dev->port_id, NULL, buffer, len, NULL) < 0) { + pios_hmc5x83_spi_release_bus(dev); return -3; } - pios_hmc5x83_spi_release_bus(); + pios_hmc5x83_spi_release_bus(dev); return 0; } @@ -414,9 +448,11 @@ int32_t PIOS_HMC5x83_SPI_Read(uint8_t address, uint8_t *buffer, uint8_t len) * \return -1 if error during I2C transfer * \return -2 if unable to claim spi device */ -int32_t PIOS_HMC5x83_SPI_Write(uint8_t address, uint8_t buffer) +int32_t PIOS_HMC5x83_SPI_Write(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t buffer) { - if (pios_hmc5x83_spi_claim_bus() < 0) { + pios_hmc5x83_dev_data_t *dev = dev_validate(handler); + + if (pios_hmc5x83_spi_claim_bus(dev) < 0) { return -1; } uint8_t data[] = { @@ -424,19 +460,19 @@ int32_t PIOS_HMC5x83_SPI_Write(uint8_t address, uint8_t buffer) buffer, }; - if (PIOS_SPI_TransferBlock(dev_port_id, data, NULL, sizeof(data), NULL) < 0) { - pios_hmc5x83_spi_release_bus(); + if (PIOS_SPI_TransferBlock(dev->port_id, data, NULL, sizeof(data), NULL) < 0) { + pios_hmc5x83_spi_release_bus(dev); return -2; } - pios_hmc5x83_spi_release_bus(); + pios_hmc5x83_spi_release_bus(dev); return 0; } -#endif /* ifdef PIOS_INCLUDE_SPI */ +#endif /* PIOS_INCLUDE_SPI */ #ifdef PIOS_INCLUDE_I2C -int32_t PIOS_HMC5x83_I2C_Read(uint8_t address, uint8_t *buffer, uint8_t len); -int32_t PIOS_HMC5x83_I2C_Write(uint8_t address, uint8_t buffer); +int32_t PIOS_HMC5x83_I2C_Read(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t *buffer, uint8_t len); +int32_t PIOS_HMC5x83_I2C_Write(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t buffer); const struct pios_hmc5x83_io_driver PIOS_HMC5x83_I2C_DRIVER = { .Read = PIOS_HMC5x83_I2C_Read, @@ -452,8 +488,9 @@ const struct pios_hmc5x83_io_driver PIOS_HMC5x83_I2C_DRIVER = { * \return -1 if error during I2C transfer * \return -2 if unable to claim i2c device */ -int32_t PIOS_HMC5x83_I2C_Read(uint8_t address, uint8_t *buffer, uint8_t len) +int32_t PIOS_HMC5x83_I2C_Read(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t *buffer, uint8_t len) { + pios_hmc5x83_dev_data_t *dev = dev_validate(handler); uint8_t addr_buffer[] = { address, }; @@ -476,7 +513,7 @@ int32_t PIOS_HMC5x83_I2C_Read(uint8_t address, uint8_t *buffer, uint8_t len) } }; - return PIOS_I2C_Transfer(dev_port_id, txn_list, NELEMENTS(txn_list)); + return PIOS_I2C_Transfer(dev->port_id, txn_list, NELEMENTS(txn_list)); } /** @@ -487,8 +524,9 @@ int32_t PIOS_HMC5x83_I2C_Read(uint8_t address, uint8_t *buffer, uint8_t len) * \return -1 if error during I2C transfer * \return -2 if unable to claim i2c device */ -int32_t PIOS_HMC5x83_I2C_Write(uint8_t address, uint8_t buffer) +int32_t PIOS_HMC5x83_I2C_Write(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t buffer) { + pios_hmc5x83_dev_data_t *dev = dev_validate(handler); uint8_t data[] = { address, buffer, @@ -506,9 +544,9 @@ int32_t PIOS_HMC5x83_I2C_Write(uint8_t address, uint8_t buffer) }; ; - return PIOS_I2C_Transfer(dev_port_id, txn_list, NELEMENTS(txn_list)); + return PIOS_I2C_Transfer(dev->port_id, txn_list, NELEMENTS(txn_list)); } -#endif /* ifdef PIOS_INCLUDE_I2C */ +#endif /* PIOS_INCLUDE_I2C */ #endif /* PIOS_INCLUDE_HMC5x83 */ diff --git a/flight/pios/inc/pios_hmc5x83.h b/flight/pios/inc/pios_hmc5x83.h index 949966ef5..dc15c35bf 100644 --- a/flight/pios/inc/pios_hmc5x83.h +++ b/flight/pios/inc/pios_hmc5x83.h @@ -30,7 +30,7 @@ #ifndef PIOS_HMC5x83_H #define PIOS_HMC5x83_H - +#include /* HMC5x83 Addresses */ #define PIOS_HMC5x83_I2C_ADDR 0x1E #define PIOS_HMC5x83_I2C_READ_ADDR 0x3D @@ -94,9 +94,11 @@ #define PIOS_HMC5x83_Sensitivity_5_6Ga 330 // LSB/Ga #define PIOS_HMC5x83_Sensitivity_8_1Ga 230 // LSB/Ga --> NOT RECOMMENDED +typedef uintptr_t pios_hmc5x83_dev_t; + struct pios_hmc5x83_io_driver { - int32_t (*Write)(uint8_t address, uint8_t buffer); - int32_t (*Read)(uint8_t address, uint8_t *buffer, uint8_t len); + int32_t (*Write)(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t buffer); + int32_t (*Read)(pios_hmc5x83_dev_t handler, uint8_t address, uint8_t *buffer, uint8_t len); }; #ifdef PIOS_INCLUDE_SPI @@ -120,12 +122,12 @@ struct pios_hmc5x83_cfg { }; /* Public Functions */ -extern void PIOS_HMC5x83_Init(const struct pios_hmc5x83_cfg *cfg, uint32_t port_id, uint8_t device_num); -extern bool PIOS_HMC5x83_NewDataAvailable(void); -extern int32_t PIOS_HMC5x83_ReadMag(int16_t out[3]); -extern uint8_t PIOS_HMC5x83_ReadID(uint8_t out[4]); -extern int32_t PIOS_HMC5x83_Test(void); -extern bool PIOS_HMC5x83_IRQHandler(); +extern pios_hmc5x83_dev_t PIOS_HMC5x83_Init(const struct pios_hmc5x83_cfg *cfg, uint32_t port_id, uint8_t device_num); +extern bool PIOS_HMC5x83_NewDataAvailable(pios_hmc5x83_dev_t handler); +extern int32_t PIOS_HMC5x83_ReadMag(pios_hmc5x83_dev_t handler, int16_t out[3]); +extern uint8_t PIOS_HMC5x83_ReadID(pios_hmc5x83_dev_t handler, uint8_t out[4]); +extern int32_t PIOS_HMC5x83_Test(pios_hmc5x83_dev_t handler); +extern bool PIOS_HMC5x83_IRQHandler(pios_hmc5x83_dev_t handler); #endif /* PIOS_HMC5x83_H */ From d856bbdb5015ed7d340a0f1779a6aa172bb69e0e Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sat, 19 Jul 2014 18:34:24 +0200 Subject: [PATCH 25/26] OP-1403 - Update targets --- flight/modules/Extensions/MagBaro/magbaro.c | 7 ++++--- flight/modules/Sensors/sensors.c | 11 ++++++++--- .../targets/boards/revolution/firmware/pios_board.c | 11 +++++++++-- flight/targets/boards/revoproto/firmware/pios_board.c | 10 ++++++++-- 4 files changed, 29 insertions(+), 10 deletions(-) diff --git a/flight/modules/Extensions/MagBaro/magbaro.c b/flight/modules/Extensions/MagBaro/magbaro.c index aa956c57e..73e2a37db 100644 --- a/flight/modules/Extensions/MagBaro/magbaro.c +++ b/flight/modules/Extensions/MagBaro/magbaro.c @@ -65,6 +65,7 @@ static int alt_ds_count = 0; #endif #if defined(PIOS_INCLUDE_HMC5X83) +pios_hmc5x83_dev_t mag_handle = 0; int32_t mag_test; static float mag_bias[3] = { 0, 0, 0 }; static float mag_scale[3] = { 1, 1, 1 }; @@ -151,7 +152,7 @@ static void magbaroTask(__attribute__((unused)) void *parameters) #if defined(PIOS_INCLUDE_HMC5X83) MagSensorData mag; - PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, PIOS_I2C_MAIN_ADAPTER, 0); + mag_handle = PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, PIOS_I2C_MAIN_ADAPTER, 0); uint32_t mag_update_time = PIOS_DELAY_GetRaw(); #endif @@ -199,9 +200,9 @@ static void magbaroTask(__attribute__((unused)) void *parameters) #endif /* if defined(PIOS_INCLUDE_BMP085) */ #if defined(PIOS_INCLUDE_HMC5X83) - if (PIOS_HMC5x83_NewDataAvailable() || PIOS_DELAY_DiffuS(mag_update_time) > 100000) { + if (PIOS_HMC5x83_NewDataAvailable(mag_handle) || PIOS_DELAY_DiffuS(mag_update_time) > 100000) { int16_t values[3]; - PIOS_HMC5x83_ReadMag(values); + PIOS_HMC5x83_ReadMag(mag_handle, values); float mags[3] = { (float)values[1] * mag_scale[0] - mag_bias[0], (float)values[0] * mag_scale[1] - mag_bias[1], -(float)values[2] * mag_scale[2] - mag_bias[2] }; diff --git a/flight/modules/Sensors/sensors.c b/flight/modules/Sensors/sensors.c index c93dc64f8..b24e34317 100644 --- a/flight/modules/Sensors/sensors.c +++ b/flight/modules/Sensors/sensors.c @@ -82,6 +82,11 @@ static xTaskHandle sensorsTaskHandle; RevoCalibrationData cal; AccelGyroSettingsData agcal; +#ifdef PIOS_INCLUDE_HMC5X83 +#include +extern pios_hmc5x83_dev_t onboard_mag; +#endif + // These values are initialized by settings but can be updated by the attitude algorithm static float mag_bias[3] = { 0, 0, 0 }; @@ -201,7 +206,7 @@ static void SensorsTask(__attribute__((unused)) void *parameters) } #if defined(PIOS_INCLUDE_HMC5X83) - mag_test = PIOS_HMC5x83_Test(); + mag_test = PIOS_HMC5x83_Test(onboard_mag); #else mag_test = 0; #endif @@ -411,9 +416,9 @@ static void SensorsTask(__attribute__((unused)) void *parameters) #if defined(PIOS_INCLUDE_HMC5X83) MagSensorData mag; - if (PIOS_HMC5x83_NewDataAvailable() || PIOS_DELAY_DiffuS(mag_update_time) > 150000) { + if (PIOS_HMC5x83_NewDataAvailable(onboard_mag) || PIOS_DELAY_DiffuS(mag_update_time) > 150000) { int16_t values[3]; - PIOS_HMC5x83_ReadMag(values); + PIOS_HMC5x83_ReadMag(onboard_mag, values); float mags[3] = { (float)values[1] - mag_bias[0], (float)values[0] - mag_bias[1], -(float)values[2] - mag_bias[2] }; diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c index b97adb07a..b26f75a8c 100644 --- a/flight/targets/boards/revolution/firmware/pios_board.c +++ b/flight/targets/boards/revolution/firmware/pios_board.c @@ -57,6 +57,7 @@ */ #if defined(PIOS_INCLUDE_ADC) + #include "pios_adc_priv.h" void PIOS_ADC_DMC_irq_handler(void); void DMA2_Stream4_IRQHandler(void) __attribute__((alias("PIOS_ADC_DMC_irq_handler"))); @@ -93,8 +94,14 @@ void PIOS_ADC_DMC_irq_handler(void) #if defined(PIOS_INCLUDE_HMC5X83) #include "pios_hmc5x83.h" +pios_hmc5x83_dev_t onboard_mag = 0; + +bool pios_board_internal_mag_handler() +{ + return PIOS_HMC5x83_IRQHandler(onboard_mag); +} static const struct pios_exti_cfg pios_exti_hmc5x83_cfg __exti_config = { - .vector = PIOS_HMC5x83_IRQHandler, + .vector = pios_board_internal_mag_handler, .line = EXTI_Line7, .pin = { .gpio = GPIOB, @@ -946,7 +953,7 @@ void PIOS_Board_Init(void) #endif #if defined(PIOS_INCLUDE_HMC5X83) - PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, pios_i2c_mag_pressure_adapter_id, 0); + onboard_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, pios_i2c_mag_pressure_adapter_id, 0); #endif #if defined(PIOS_INCLUDE_MS5611) diff --git a/flight/targets/boards/revoproto/firmware/pios_board.c b/flight/targets/boards/revoproto/firmware/pios_board.c index de2c5982d..d748265bb 100644 --- a/flight/targets/boards/revoproto/firmware/pios_board.c +++ b/flight/targets/boards/revoproto/firmware/pios_board.c @@ -82,9 +82,15 @@ void PIOS_ADC_DMC_irq_handler(void) #endif /* if defined(PIOS_INCLUDE_ADC) */ #if defined(PIOS_INCLUDE_HMC5X83) +pios_hmc5x83_dev_t onboard_mag = 0; + +bool pios_board_internal_mag_handler() +{ + return PIOS_HMC5x83_IRQHandler(onboard_mag); +} #include "pios_hmc5x83.h" static const struct pios_exti_cfg pios_exti_hmc5x83_cfg __exti_config = { - .vector = PIOS_HMC5x83_IRQHandler, + .vector = pios_board_internal_mag_handler, .line = EXTI_Line5, .pin = { .gpio = GPIOB, @@ -940,7 +946,7 @@ void PIOS_Board_Init(void) #endif #if defined(PIOS_INCLUDE_HMC5X83) - PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, pios_i2c_pressure_adapter_id, 0); + onboard_mag = PIOS_HMC5x83_Init(&pios_hmc5x83_cfg, pios_i2c_pressure_adapter_id, 0); #endif #if defined(PIOS_INCLUDE_MS5611) From 016ba6940db8a9533d8c35e1e889d28656cbcc52 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sat, 19 Jul 2014 18:43:57 +0200 Subject: [PATCH 26/26] OP-1403 - drop attitude/revolution as it has been supersede by StateEstimation module --- flight/modules/Attitude/revolution/attitude.c | 1348 ----------------- .../Attitude/revolution/inc/attitude.h | 37 - 2 files changed, 1385 deletions(-) delete mode 100644 flight/modules/Attitude/revolution/attitude.c delete mode 100644 flight/modules/Attitude/revolution/inc/attitude.h diff --git a/flight/modules/Attitude/revolution/attitude.c b/flight/modules/Attitude/revolution/attitude.c deleted file mode 100644 index 2ee8901e0..000000000 --- a/flight/modules/Attitude/revolution/attitude.c +++ /dev/null @@ -1,1348 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup OpenPilotModules OpenPilot Modules - * @{ - * @addtogroup Attitude Copter Control Attitude Estimation - * @brief Acquires sensor data and computes attitude estimate - * Specifically updates the the @ref AttitudeState "AttitudeState" and @ref AttitudeRaw "AttitudeRaw" settings objects - * @{ - * - * @file attitude.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Module to handle all comms to the AHRS on a periodic basis. - * - * @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 - */ - -/** - * Input objects: None, takes sensor data via pios - * Output objects: @ref AttitudeRaw @ref AttitudeState - * - * This module computes an attitude estimate from the sensor data - * - * The module executes in its own thread. - * - * 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 -#include -#include "attitude.h" -#include "accelsensor.h" -#include "accelstate.h" -#include "airspeedsensor.h" -#include "airspeedstate.h" -#include "attitudestate.h" -#include "attitudesettings.h" -#include "barosensor.h" -#include "flightstatus.h" -#include "gpspositionsensor.h" -#include "gpsvelocitysensor.h" -#include "gyrostate.h" -#include "gyrosensor.h" -#include "homelocation.h" -#include "magsensor.h" -#include "magstate.h" -#include "positionstate.h" -#include "ekfconfiguration.h" -#include "ekfstatevariance.h" -#include "revocalibration.h" -#include "revosettings.h" -#include "velocitystate.h" -#include "taskinfo.h" - -#include "CoordinateConversions.h" - -// Private constants -#define STACK_SIZE_BYTES 2048 -#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) -#define FAILSAFE_TIMEOUT_MS 10 - -#define CALIBRATION_DELAY 4000 -#define CALIBRATION_DURATION 6000 -// low pass filter configuration to calculate offset -// of barometric altitude sensor -// reasoning: updates at: 10 Hz, tau= 300 s settle time -// exp(-(1/f) / tau ) ~=~ 0.9997 -#define BARO_OFFSET_LOWPASS_ALPHA 0.9997f - -// simple IAS to TAS aproximation - 2% increase per 1000ft -// since we do not have flowing air temperature information -#define IAS2TAS(alt) (1.0f + (0.02f * (alt) / 304.8f)) - -// Private types - -// Private variables -static xTaskHandle attitudeTaskHandle; - -static xQueueHandle gyroQueue; -static xQueueHandle accelQueue; -static xQueueHandle magQueue; -static xQueueHandle airspeedQueue; -static xQueueHandle baroQueue; -static xQueueHandle gpsQueue; -static xQueueHandle gpsVelQueue; - -static AttitudeSettingsData attitudeSettings; -static HomeLocationData homeLocation; -static RevoCalibrationData revoCalibration; -static EKFConfigurationData ekfConfiguration; -static RevoSettingsData revoSettings; -static FlightStatusData flightStatus; -const uint32_t SENSOR_QUEUE_SIZE = 10; - -static bool volatile variance_error = true; -static bool volatile initialization_required = true; -static uint32_t volatile running_algorithm = 0xffffffff; // we start with no algorithm running -static float rollPitchBiasRate = 0; - -// Accel filtering -static float accel_alpha = 0; -static bool accel_filter_enabled = false; -static float accels_filtered[3]; -static float grot_filtered[3]; - -// Private functions -static void AttitudeTask(void *parameters); - -static int32_t updateAttitudeComplementary(bool first_run); -static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode); -static void settingsUpdatedCb(UAVObjEvent *objEv); - -static int32_t getNED(GPSPositionSensorData *gpsPosition, float *NED); - -static void magOffsetEstimation(MagSensorData *mag); - -// check for invalid values -static inline bool invalid(float data) -{ - if (isnan(data) || isinf(data)) { - return true; - } - return false; -} - -// check for invalid variance values -static inline bool invalid_var(float data) -{ - if (invalid(data)) { - return true; - } - if (data < 1e-15f) { // var should not be close to zero. And not negative either. - return true; - } - return false; -} - -/** - * API for sensor fusion algorithms: - * Configure(xQueueHandle gyro, xQueueHandle accel, xQueueHandle mag, xQueueHandle baro) - * Stores all the queues the algorithm will pull data from - * FinalizeSensors() -- before saving the sensors modifies them based on internal state (gyro bias) - * Update() -- queries queues and updates the attitude estiamte - */ - - -/** - * Initialise the module. Called before the start function - * \returns 0 on success or -1 if initialisation failed - */ -int32_t AttitudeInitialize(void) -{ - GyroSensorInitialize(); - GyroStateInitialize(); - AccelSensorInitialize(); - AccelStateInitialize(); - MagSensorInitialize(); - MagStateInitialize(); - AirspeedSensorInitialize(); - AirspeedStateInitialize(); - BaroSensorInitialize(); - GPSPositionSensorInitialize(); - GPSVelocitySensorInitialize(); - AttitudeSettingsInitialize(); - AttitudeStateInitialize(); - PositionStateInitialize(); - VelocityStateInitialize(); - RevoSettingsInitialize(); - RevoCalibrationInitialize(); - EKFConfigurationInitialize(); - EKFStateVarianceInitialize(); - FlightStatusInitialize(); - - // Initialize this here while we aren't setting the homelocation in GPS - HomeLocationInitialize(); - - // Initialize quaternion - AttitudeStateData attitude; - AttitudeStateGet(&attitude); - attitude.q1 = 1.0f; - attitude.q2 = 0.0f; - attitude.q3 = 0.0f; - attitude.q4 = 0.0f; - AttitudeStateSet(&attitude); - - AttitudeSettingsConnectCallback(&settingsUpdatedCb); - RevoSettingsConnectCallback(&settingsUpdatedCb); - RevoCalibrationConnectCallback(&settingsUpdatedCb); - HomeLocationConnectCallback(&settingsUpdatedCb); - EKFConfigurationConnectCallback(&settingsUpdatedCb); - FlightStatusConnectCallback(&settingsUpdatedCb); - - return 0; -} - -/** - * Start the task. Expects all objects to be initialized by this point. - * \returns 0 on success or -1 if initialisation failed - */ -int32_t AttitudeStart(void) -{ - // Create the queues for the sensors - gyroQueue = xQueueCreate(1, sizeof(UAVObjEvent)); - accelQueue = xQueueCreate(1, sizeof(UAVObjEvent)); - magQueue = xQueueCreate(1, sizeof(UAVObjEvent)); - airspeedQueue = xQueueCreate(1, sizeof(UAVObjEvent)); - baroQueue = xQueueCreate(1, sizeof(UAVObjEvent)); - gpsQueue = xQueueCreate(1, sizeof(UAVObjEvent)); - gpsVelQueue = xQueueCreate(1, sizeof(UAVObjEvent)); - - // Start main task - xTaskCreate(AttitudeTask, "Attitude", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &attitudeTaskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_ATTITUDE, attitudeTaskHandle); -#ifdef PIOS_INCLUDE_WDG - PIOS_WDG_RegisterFlag(PIOS_WDG_ATTITUDE); -#endif - - GyroSensorConnectQueue(gyroQueue); - AccelSensorConnectQueue(accelQueue); - MagSensorConnectQueue(magQueue); - AirspeedSensorConnectQueue(airspeedQueue); - BaroSensorConnectQueue(baroQueue); - GPSPositionSensorConnectQueue(gpsQueue); - GPSVelocitySensorConnectQueue(gpsVelQueue); - - return 0; -} - -MODULE_INITCALL(AttitudeInitialize, AttitudeStart); - -/** - * Module thread, should not return. - */ -static void AttitudeTask(__attribute__((unused)) void *parameters) -{ - AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); - - // Force settings update to make sure rotation loaded - settingsUpdatedCb(NULL); - - // Wait for all the sensors be to read - vTaskDelay(100); - - // Main task loop - TODO: make it run as delayed callback - while (1) { - int32_t ret_val = -1; - - bool first_run = false; - if (initialization_required) { - initialization_required = false; - first_run = true; - } - - // This function blocks on data queue - switch (running_algorithm) { - case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARY: - ret_val = updateAttitudeComplementary(first_run); - break; - case REVOSETTINGS_FUSIONALGORITHM_INS13GPSOUTDOOR: - ret_val = updateAttitudeINSGPS(first_run, true); - break; - case REVOSETTINGS_FUSIONALGORITHM_INS13INDOOR: - ret_val = updateAttitudeINSGPS(first_run, false); - break; - default: - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL); - break; - } - - if (ret_val != 0) { - initialization_required = true; - } - -#ifdef PIOS_INCLUDE_WDG - PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE); -#endif - } -} - -static inline void apply_accel_filter(const float *raw, float *filtered) -{ - if (accel_filter_enabled) { - filtered[0] = filtered[0] * accel_alpha + raw[0] * (1 - accel_alpha); - filtered[1] = filtered[1] * accel_alpha + raw[1] * (1 - accel_alpha); - filtered[2] = filtered[2] * accel_alpha + raw[2] * (1 - accel_alpha); - } else { - filtered[0] = raw[0]; - filtered[1] = raw[1]; - filtered[2] = raw[2]; - } -} - -float accel_mag; -float qmag; -float attitudeDt; -float mag_err[3]; - -static int32_t updateAttitudeComplementary(bool first_run) -{ - UAVObjEvent ev; - GyroSensorData gyroSensorData; - GyroStateData gyroStateData; - AccelSensorData accelSensorData; - static int32_t timeval; - float dT; - static uint8_t init = 0; - static float gyro_bias[3] = { 0, 0, 0 }; - static bool magCalibrated = true; - static uint32_t initStartupTime = 0; - - // Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe - if (xQueueReceive(gyroQueue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE || - xQueueReceive(accelQueue, &ev, 1 / portTICK_RATE_MS) != pdTRUE) { - // When one of these is updated so should the other - // Do not set attitude timeout warnings in simulation mode - if (!AttitudeStateReadOnly()) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING); - return -1; - } - } - - AccelSensorGet(&accelSensorData); - -// TODO: put in separate filter - AccelStateData accelState; - accelState.x = accelSensorData.x; - accelState.y = accelSensorData.y; - accelState.z = accelSensorData.z; - AccelStateSet(&accelState); - - // During initialization and - if (first_run) { -#if defined(PIOS_INCLUDE_HMC5X83) - // To initialize we need a valid mag reading - if (xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) != pdTRUE) { - return -1; - } - MagSensorData magData; - MagSensorGet(&magData); -#else - MagSensorData magData; - magData.x = 100.0f; - magData.y = 0.0f; - magData.z = 0.0f; -#endif - float magBias[3]; - RevoCalibrationmag_biasArrayGet(magBias); - // don't trust Mag for initial orientation if it has not been calibrated - if (magBias[0] < 1e-6f && magBias[1] < 1e-6f && magBias[2] < 1e-6f) { - magCalibrated = false; - magData.x = 100.0f; - magData.y = 0.0f; - magData.z = 0.0f; - } - AttitudeStateData attitudeState; - AttitudeStateGet(&attitudeState); - init = 0; - - // Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly, - // so pseudo "north" vector can be estimated even if the board is not level - attitudeState.Roll = atan2f(-accelSensorData.y, -accelSensorData.z); - float zn = cosf(attitudeState.Roll) * magData.z + sinf(attitudeState.Roll) * magData.y; - float yn = cosf(attitudeState.Roll) * magData.y - sinf(attitudeState.Roll) * magData.z; - - // rotate accels z vector according to roll - float azn = cosf(attitudeState.Roll) * accelSensorData.z + sinf(attitudeState.Roll) * accelSensorData.y; - attitudeState.Pitch = atan2f(accelSensorData.x, -azn); - - float xn = cosf(attitudeState.Pitch) * magData.x + sinf(attitudeState.Pitch) * zn; - - attitudeState.Yaw = atan2f(-yn, xn); - // TODO: This is still a hack - // Put this in a proper generic function in CoordinateConversion.c - // should take 4 vectors: g (0,0,-9.81), accels, Be (or 1,0,0 if no home loc) and magnetometers (or 1,0,0 if no mags) - // should calculate the rotation in 3d space using proper cross product math - // SUBTODO: formulate the math required - - attitudeState.Roll = RAD2DEG(attitudeState.Roll); - attitudeState.Pitch = RAD2DEG(attitudeState.Pitch); - attitudeState.Yaw = RAD2DEG(attitudeState.Yaw); - - RPY2Quaternion(&attitudeState.Roll, &attitudeState.q1); - AttitudeStateSet(&attitudeState); - - timeval = PIOS_DELAY_GetRaw(); - - // wait calibration_delay only at powerup - if (xTaskGetTickCount() < 3000) { - initStartupTime = 0; - } else { - initStartupTime = xTaskGetTickCount() - CALIBRATION_DELAY; - } - - // Zero gyro bias - // This is really needed after updating calibration settings. - gyro_bias[0] = 0.0f; - gyro_bias[1] = 0.0f; - gyro_bias[2] = 0.0f; - return 0; - } - - if ((xTaskGetTickCount() - initStartupTime < CALIBRATION_DURATION + CALIBRATION_DELAY) && - (xTaskGetTickCount() - initStartupTime > CALIBRATION_DELAY)) { - // For first CALIBRATION_DURATION seconds after CALIBRATION_DELAY from startup - // Zero gyro bias assuming it is steady, smoothing the gyro input value applying rollPitchBiasRate. - attitudeSettings.AccelKp = 1.0f; - attitudeSettings.AccelKi = 0.0f; - attitudeSettings.YawBiasRate = 0.23f; - accel_filter_enabled = false; - rollPitchBiasRate = 0.01f; - attitudeSettings.MagKp = magCalibrated ? 1.0f : 0.0f; - init = 0; - } else if ((attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) { - attitudeSettings.AccelKp = 1.0f; - attitudeSettings.AccelKi = 0.0f; - attitudeSettings.YawBiasRate = 0.23f; - accel_filter_enabled = false; - rollPitchBiasRate = 0.01f; - attitudeSettings.MagKp = magCalibrated ? 1.0f : 0.0f; - init = 0; - } else if (init == 0) { - // Reload settings (all the rates) - AttitudeSettingsGet(&attitudeSettings); - rollPitchBiasRate = 0.0f; - if (accel_alpha > 0.0f) { - accel_filter_enabled = true; - } - init = 1; - } - - GyroSensorGet(&gyroSensorData); - gyroStateData.x = gyroSensorData.x; - gyroStateData.y = gyroSensorData.y; - gyroStateData.z = gyroSensorData.z; - - // Compute the dT using the cpu clock - dT = PIOS_DELAY_DiffuS(timeval) / 1000000.0f; - timeval = PIOS_DELAY_GetRaw(); - - float q[4]; - - AttitudeStateData attitudeState; - AttitudeStateGet(&attitudeState); - - float grot[3]; - float accel_err[3]; - - // Get the current attitude estimate - quat_copy(&attitudeState.q1, q); - - // Apply smoothing to accel values, to reduce vibration noise before main calculations. - apply_accel_filter((const float *)&accelSensorData.x, accels_filtered); - - // Rotate gravity to body frame and cross with accels - grot[0] = -(2.0f * (q[1] * q[3] - q[0] * q[2])); - grot[1] = -(2.0f * (q[2] * q[3] + q[0] * q[1])); - grot[2] = -(q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]); - - apply_accel_filter(grot, grot_filtered); - - CrossProduct((const float *)accels_filtered, (const float *)grot_filtered, accel_err); - - // Account for accel magnitude - accel_mag = accels_filtered[0] * accels_filtered[0] + accels_filtered[1] * accels_filtered[1] + accels_filtered[2] * accels_filtered[2]; - accel_mag = sqrtf(accel_mag); - - float grot_mag; - if (accel_filter_enabled) { - grot_mag = sqrtf(grot_filtered[0] * grot_filtered[0] + grot_filtered[1] * grot_filtered[1] + grot_filtered[2] * grot_filtered[2]); - } else { - grot_mag = 1.0f; - } - - // TODO! check grot_mag & accel vector magnitude values for correctness. - - accel_err[0] /= (accel_mag * grot_mag); - accel_err[1] /= (accel_mag * grot_mag); - accel_err[2] /= (accel_mag * grot_mag); - - - if (xQueueReceive(magQueue, &ev, 0) != pdTRUE) { - // Rotate gravity to body frame and cross with accels - float brot[3]; - float Rbe[3][3]; - MagSensorData mag; - - Quaternion2R(q, Rbe); - MagSensorGet(&mag); - -// TODO: separate filter! - if (revoCalibration.MagBiasNullingRate > 0) { - magOffsetEstimation(&mag); - } - MagStateData mags; - mags.x = mag.x; - mags.y = mag.y; - mags.z = mag.z; - MagStateSet(&mags); - - // If the mag is producing bad data don't use it (normally bad calibration) - if (!isnan(mag.x) && !isinf(mag.x) && !isnan(mag.y) && !isinf(mag.y) && !isnan(mag.z) && !isinf(mag.z)) { - rot_mult(Rbe, homeLocation.Be, brot); - - float mag_len = sqrtf(mag.x * mag.x + mag.y * mag.y + mag.z * mag.z); - mag.x /= mag_len; - mag.y /= mag_len; - mag.z /= mag_len; - - float bmag = sqrtf(brot[0] * brot[0] + brot[1] * brot[1] + brot[2] * brot[2]); - brot[0] /= bmag; - brot[1] /= bmag; - brot[2] /= bmag; - - // Only compute if neither vector is null - if (bmag < 1.0f || mag_len < 1.0f) { - mag_err[0] = mag_err[1] = mag_err[2] = 0.0f; - } else { - CrossProduct((const float *)&mag.x, (const float *)brot, mag_err); - } - } - } else { - mag_err[0] = mag_err[1] = mag_err[2] = 0.0f; - } - - // Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s - // Correct rates based on integral coefficient - gyroStateData.x -= gyro_bias[0]; - gyroStateData.y -= gyro_bias[1]; - gyroStateData.z -= gyro_bias[2]; - - gyro_bias[0] -= accel_err[0] * attitudeSettings.AccelKi - (gyroStateData.x) * rollPitchBiasRate; - gyro_bias[1] -= accel_err[1] * attitudeSettings.AccelKi - (gyroStateData.y) * rollPitchBiasRate; - gyro_bias[2] -= -mag_err[2] * attitudeSettings.MagKi - (gyroStateData.z) * rollPitchBiasRate; - - // save gyroscope state - GyroStateSet(&gyroStateData); - - // Correct rates based on proportional coefficient - gyroStateData.x += accel_err[0] * attitudeSettings.AccelKp / dT; - gyroStateData.y += accel_err[1] * attitudeSettings.AccelKp / dT; - gyroStateData.z += accel_err[2] * attitudeSettings.AccelKp / dT + mag_err[2] * attitudeSettings.MagKp / dT; - - // Work out time derivative from INSAlgo writeup - // Also accounts for the fact that gyros are in deg/s - float qdot[4]; - qdot[0] = DEG2RAD(-q[1] * gyroStateData.x - q[2] * gyroStateData.y - q[3] * gyroStateData.z) * dT / 2; - qdot[1] = DEG2RAD(q[0] * gyroStateData.x - q[3] * gyroStateData.y + q[2] * gyroStateData.z) * dT / 2; - qdot[2] = DEG2RAD(q[3] * gyroStateData.x + q[0] * gyroStateData.y - q[1] * gyroStateData.z) * dT / 2; - qdot[3] = DEG2RAD(-q[2] * gyroStateData.x + q[1] * gyroStateData.y + q[0] * gyroStateData.z) * dT / 2; - - // Take a time step - q[0] = q[0] + qdot[0]; - q[1] = q[1] + qdot[1]; - q[2] = q[2] + qdot[2]; - q[3] = q[3] + qdot[3]; - - if (q[0] < 0.0f) { - q[0] = -q[0]; - q[1] = -q[1]; - q[2] = -q[2]; - q[3] = -q[3]; - } - - // Renomalize - qmag = sqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); - q[0] = q[0] / qmag; - q[1] = q[1] / qmag; - q[2] = q[2] / qmag; - q[3] = q[3] / qmag; - - // If quaternion has become inappropriately short or is nan reinit. - // THIS SHOULD NEVER ACTUALLY HAPPEN - if ((fabsf(qmag) < 1.0e-3f) || isnan(qmag)) { - q[0] = 1.0f; - q[1] = 0.0f; - q[2] = 0.0f; - q[3] = 0.0f; - } - - quat_copy(q, &attitudeState.q1); - - // Convert into eueler degrees (makes assumptions about RPY order) - Quaternion2RPY(&attitudeState.q1, &attitudeState.Roll); - - AttitudeStateSet(&attitudeState); - - // Flush these queues for avoid errors - xQueueReceive(baroQueue, &ev, 0); - if (xQueueReceive(gpsQueue, &ev, 0) == pdTRUE && homeLocation.Set == HOMELOCATION_SET_TRUE) { - float NED[3]; - // Transform the GPS position into NED coordinates - GPSPositionSensorData gpsPosition; - GPSPositionSensorGet(&gpsPosition); - getNED(&gpsPosition, NED); - - PositionStateData positionState; - PositionStateGet(&positionState); - positionState.North = NED[0]; - positionState.East = NED[1]; - positionState.Down = NED[2]; - PositionStateSet(&positionState); - } - - if (xQueueReceive(gpsVelQueue, &ev, 0) == pdTRUE) { - // Transform the GPS position into NED coordinates - GPSVelocitySensorData gpsVelocity; - GPSVelocitySensorGet(&gpsVelocity); - - VelocityStateData velocityState; - VelocityStateGet(&velocityState); - velocityState.North = gpsVelocity.North; - velocityState.East = gpsVelocity.East; - velocityState.Down = gpsVelocity.Down; - VelocityStateSet(&velocityState); - } - - if (xQueueReceive(airspeedQueue, &ev, 0) == pdTRUE) { - // Calculate true airspeed from indicated airspeed - AirspeedSensorData airspeedSensor; - AirspeedSensorGet(&airspeedSensor); - - AirspeedStateData airspeed; - AirspeedStateGet(&airspeed); - - PositionStateData positionState; - PositionStateGet(&positionState); - - if (airspeedSensor.SensorConnected == AIRSPEEDSENSOR_SENSORCONNECTED_TRUE) { - // we have airspeed available - airspeed.CalibratedAirspeed = airspeedSensor.CalibratedAirspeed; - airspeed.TrueAirspeed = (airspeedSensor.TrueAirspeed < 0.f) ? airspeed.CalibratedAirspeed *IAS2TAS(homeLocation.Altitude - positionState.Down) : airspeedSensor.TrueAirspeed; - AirspeedStateSet(&airspeed); - } - } - - if (!init && flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); - } else if (variance_error) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL); - } else { - AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); - } - - - return 0; -} - -#include "insgps.h" -int32_t ins_failed = 0; -extern struct NavStruct Nav; -int32_t init_stage = 0; - -/** - * @brief Use the INSGPS fusion algorithm in either indoor or outdoor mode (use GPS) - * @params[in] first_run This is the first run so trigger reinitialization - * @params[in] outdoor_mode If true use the GPS for position, if false weakly pull to (0,0) - * @return 0 for success, -1 for failure - */ -static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) -{ - UAVObjEvent ev; - GyroSensorData gyroSensorData; - AccelSensorData accelSensorData; - MagStateData magData; - AirspeedSensorData airspeedData; - BaroSensorData baroData; - GPSPositionSensorData gpsData; - GPSVelocitySensorData gpsVelData; - - static bool mag_updated = false; - static bool baro_updated; - static bool airspeed_updated; - static bool gps_updated; - static bool gps_vel_updated; - - static bool value_error = false; - - static float baroOffset = 0.0f; - - static uint32_t ins_last_time = 0; - static bool inited; - - float NED[3] = { 0.0f, 0.0f, 0.0f }; - float vel[3] = { 0.0f, 0.0f, 0.0f }; - float zeros[3] = { 0.0f, 0.0f, 0.0f }; - - // Perform the update - uint16_t sensors = 0; - float dT; - - // Wait until the gyro and accel object is updated, if a timeout then go to failsafe - if ((xQueueReceive(gyroQueue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE) || - (xQueueReceive(accelQueue, &ev, 1 / portTICK_RATE_MS) != pdTRUE)) { - // Do not set attitude timeout warnings in simulation mode - if (!AttitudeStateReadOnly()) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_WARNING); - return -1; - } - } - - if (inited) { - mag_updated = 0; - baro_updated = 0; - airspeed_updated = 0; - gps_updated = 0; - gps_vel_updated = 0; - } - - if (first_run) { - inited = false; - init_stage = 0; - - mag_updated = 0; - baro_updated = 0; - airspeed_updated = 0; - gps_updated = 0; - gps_vel_updated = 0; - - ins_last_time = PIOS_DELAY_GetRaw(); - - return 0; - } - - mag_updated |= (xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE); - baro_updated |= xQueueReceive(baroQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE; - airspeed_updated |= xQueueReceive(airspeedQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE; - - // Check if we are running simulation - if (!GPSPositionSensorReadOnly()) { - gps_updated |= (xQueueReceive(gpsQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode; - } else { - gps_updated |= pdTRUE && outdoor_mode; - } - - if (!GPSVelocitySensorReadOnly()) { - gps_vel_updated |= (xQueueReceive(gpsVelQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode; - } else { - gps_vel_updated |= pdTRUE && outdoor_mode; - } - - // Get most recent data - GyroSensorGet(&gyroSensorData); - AccelSensorGet(&accelSensorData); -// TODO: separate filter! - if (mag_updated) { - MagSensorData mags; - MagSensorGet(&mags); - if (revoCalibration.MagBiasNullingRate > 0) { - magOffsetEstimation(&mags); - } - magData.x = mags.x; - magData.y = mags.y; - magData.z = mags.z; - MagStateSet(&magData); - } else { - MagStateGet(&magData); - } - - BaroSensorGet(&baroData); - AirspeedSensorGet(&airspeedData); - GPSPositionSensorGet(&gpsData); - GPSVelocitySensorGet(&gpsVelData); - -// TODO: put in separate filter - AccelStateData accelState; - accelState.x = accelSensorData.x; - accelState.y = accelSensorData.y; - accelState.z = accelSensorData.z; - AccelStateSet(&accelState); - - - value_error = false; - // safety checks - if (invalid(gyroSensorData.x) || - invalid(gyroSensorData.y) || - invalid(gyroSensorData.z) || - invalid(accelSensorData.x) || - invalid(accelSensorData.y) || - invalid(accelSensorData.z)) { - // cannot run process update, raise error! - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); - return 0; - } - - if (invalid(magData.x) || - invalid(magData.y) || - invalid(magData.z)) { - // magnetometers can be ignored for a while - mag_updated = false; - value_error = true; - } - - // Don't require HomeLocation.Set to be true but at least require a mag configuration (allows easily - // switching between indoor and outdoor mode with Set = false) - if ((homeLocation.Be[0] * homeLocation.Be[0] + homeLocation.Be[1] * homeLocation.Be[1] + homeLocation.Be[2] * homeLocation.Be[2] < 1e-5f)) { - mag_updated = false; - value_error = true; - } - - if (invalid(baroData.Altitude)) { - baro_updated = false; - value_error = true; - } - - if (invalid(airspeedData.CalibratedAirspeed)) { - airspeed_updated = false; - value_error = true; - } - - if (invalid(gpsData.Altitude)) { - gps_updated = false; - value_error = true; - } - - if (invalid_var(ekfConfiguration.R.GPSPosNorth) || - invalid_var(ekfConfiguration.R.GPSPosEast) || - invalid_var(ekfConfiguration.R.GPSPosDown) || - invalid_var(ekfConfiguration.R.GPSVelNorth) || - invalid_var(ekfConfiguration.R.GPSVelEast) || - invalid_var(ekfConfiguration.R.GPSVelDown)) { - gps_updated = false; - value_error = true; - } - - if (invalid(gpsVelData.North) || - invalid(gpsVelData.East) || - invalid(gpsVelData.Down)) { - gps_vel_updated = false; - value_error = true; - } - - // Discard airspeed if sensor not connected - if (airspeedData.SensorConnected != AIRSPEEDSENSOR_SENSORCONNECTED_TRUE) { - airspeed_updated = false; - } - - // Have a minimum requirement for gps usage - if ((gpsData.Satellites < 7) || - (gpsData.PDOP > 4.0f) || - (gpsData.Latitude == 0 && gpsData.Longitude == 0) || - (homeLocation.Set != HOMELOCATION_SET_TRUE)) { - gps_updated = false; - gps_vel_updated = false; - } - - if (!inited) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); - } else if (value_error) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL); - } else if (variance_error) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL); - } else if (outdoor_mode && gpsData.Satellites < 7) { - AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR); - } else { - AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); - } - - dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f; - ins_last_time = PIOS_DELAY_GetRaw(); - - // This should only happen at start up or at mode switches - if (dT > 0.01f) { - dT = 0.01f; - } else if (dT <= 0.001f) { - dT = 0.001f; - } - - if (!inited && mag_updated && baro_updated && (gps_updated || !outdoor_mode) && !variance_error) { - // Don't initialize until all sensors are read - if (init_stage == 0) { - // Reset the INS algorithm - INSGPSInit(); - INSSetMagVar((float[3]) { ekfConfiguration.R.MagX, - ekfConfiguration.R.MagY, - ekfConfiguration.R.MagZ } - ); - INSSetAccelVar((float[3]) { ekfConfiguration.Q.AccelX, - ekfConfiguration.Q.AccelY, - ekfConfiguration.Q.AccelZ } - ); - INSSetGyroVar((float[3]) { ekfConfiguration.Q.GyroX, - ekfConfiguration.Q.GyroY, - ekfConfiguration.Q.GyroZ } - ); - INSSetGyroBiasVar((float[3]) { ekfConfiguration.Q.GyroDriftX, - ekfConfiguration.Q.GyroDriftY, - ekfConfiguration.Q.GyroDriftZ } - ); - INSSetBaroVar(ekfConfiguration.R.BaroZ); - - // Initialize the gyro bias - float gyro_bias[3] = { 0.0f, 0.0f, 0.0f }; - INSSetGyroBias(gyro_bias); - - float pos[3] = { 0.0f, 0.0f, 0.0f }; - - if (outdoor_mode) { - GPSPositionSensorData gpsPosition; - GPSPositionSensorGet(&gpsPosition); - - // Transform the GPS position into NED coordinates - getNED(&gpsPosition, pos); - - // Initialize barometric offset to current GPS NED coordinate - baroOffset = -pos[2] - baroData.Altitude; - } else { - // Initialize barometric offset to homelocation altitude - baroOffset = -baroData.Altitude; - pos[2] = -(baroData.Altitude + baroOffset); - } - - // xQueueReceive(magQueue, &ev, 100 / portTICK_RATE_MS); - // MagSensorGet(&magData); - - AttitudeStateData attitudeState; - AttitudeStateGet(&attitudeState); - - // Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly, - // so pseudo "north" vector can be estimated even if the board is not level - attitudeState.Roll = atan2f(-accelSensorData.y, -accelSensorData.z); - float zn = cosf(attitudeState.Roll) * magData.z + sinf(attitudeState.Roll) * magData.y; - float yn = cosf(attitudeState.Roll) * magData.y - sinf(attitudeState.Roll) * magData.z; - - // rotate accels z vector according to roll - float azn = cosf(attitudeState.Roll) * accelSensorData.z + sinf(attitudeState.Roll) * accelSensorData.y; - attitudeState.Pitch = atan2f(accelSensorData.x, -azn); - - float xn = cosf(attitudeState.Pitch) * magData.x + sinf(attitudeState.Pitch) * zn; - - attitudeState.Yaw = atan2f(-yn, xn); - // TODO: This is still a hack - // Put this in a proper generic function in CoordinateConversion.c - // should take 4 vectors: g (0,0,-9.81), accels, Be (or 1,0,0 if no home loc) and magnetometers (or 1,0,0 if no mags) - // should calculate the rotation in 3d space using proper cross product math - // SUBTODO: formulate the math required - - attitudeState.Roll = RAD2DEG(attitudeState.Roll); - attitudeState.Pitch = RAD2DEG(attitudeState.Pitch); - attitudeState.Yaw = RAD2DEG(attitudeState.Yaw); - - RPY2Quaternion(&attitudeState.Roll, &attitudeState.q1); - AttitudeStateSet(&attitudeState); - - float q[4] = { attitudeState.q1, attitudeState.q2, attitudeState.q3, attitudeState.q4 }; - INSSetState(pos, zeros, q, zeros, zeros); - - INSResetP(cast_struct_to_array(ekfConfiguration.P, ekfConfiguration.P.AttitudeQ1)); - } else { - // Run prediction a bit before any corrections - - // Because the sensor module remove the bias we need to add it - // back in here so that the INS algorithm can track it correctly - float gyros[3] = { DEG2RAD(gyroSensorData.x), DEG2RAD(gyroSensorData.y), DEG2RAD(gyroSensorData.z) }; - INSStatePrediction(gyros, &accelSensorData.x, dT); - - AttitudeStateData attitude; - AttitudeStateGet(&attitude); - attitude.q1 = Nav.q[0]; - attitude.q2 = Nav.q[1]; - attitude.q3 = Nav.q[2]; - attitude.q4 = Nav.q[3]; - Quaternion2RPY(&attitude.q1, &attitude.Roll); - AttitudeStateSet(&attitude); - } - - init_stage++; - if (init_stage > 10) { - inited = true; - } - - return 0; - } - - if (!inited) { - return 0; - } - - // Because the sensor module remove the bias we need to add it - // back in here so that the INS algorithm can track it correctly - float gyros[3] = { DEG2RAD(gyroSensorData.x), DEG2RAD(gyroSensorData.y), DEG2RAD(gyroSensorData.z) }; - - // Advance the state estimate - INSStatePrediction(gyros, &accelSensorData.x, dT); - - // Copy the attitude into the UAVO - AttitudeStateData attitude; - AttitudeStateGet(&attitude); - attitude.q1 = Nav.q[0]; - attitude.q2 = Nav.q[1]; - attitude.q3 = Nav.q[2]; - attitude.q4 = Nav.q[3]; - Quaternion2RPY(&attitude.q1, &attitude.Roll); - AttitudeStateSet(&attitude); - - // Advance the covariance estimate - INSCovariancePrediction(dT); - - if (mag_updated) { - sensors |= MAG_SENSORS; - } - - if (baro_updated) { - sensors |= BARO_SENSOR; - } - - INSSetMagNorth(homeLocation.Be); - - if (gps_updated && outdoor_mode) { - INSSetPosVelVar((float[3]) { ekfConfiguration.R.GPSPosNorth, - ekfConfiguration.R.GPSPosEast, - ekfConfiguration.R.GPSPosDown }, - (float[3]) { ekfConfiguration.R.GPSVelNorth, - ekfConfiguration.R.GPSVelEast, - ekfConfiguration.R.GPSVelDown } - ); - sensors |= POS_SENSORS; - - if (0) { // Old code to take horizontal velocity from GPS Position update - sensors |= HORIZ_SENSORS; - vel[0] = gpsData.Groundspeed * cosf(DEG2RAD(gpsData.Heading)); - vel[1] = gpsData.Groundspeed * sinf(DEG2RAD(gpsData.Heading)); - vel[2] = 0.0f; - } - // Transform the GPS position into NED coordinates - getNED(&gpsData, NED); - - // Track barometric altitude offset with a low pass filter - baroOffset = BARO_OFFSET_LOWPASS_ALPHA * baroOffset + - (1.0f - BARO_OFFSET_LOWPASS_ALPHA) - * (-NED[2] - baroData.Altitude); - } else if (!outdoor_mode) { - INSSetPosVelVar((float[3]) { ekfConfiguration.FakeR.FakeGPSPosIndoor, - ekfConfiguration.FakeR.FakeGPSPosIndoor, - ekfConfiguration.FakeR.FakeGPSPosIndoor }, - (float[3]) { ekfConfiguration.FakeR.FakeGPSVelIndoor, - ekfConfiguration.FakeR.FakeGPSVelIndoor, - ekfConfiguration.FakeR.FakeGPSVelIndoor } - ); - vel[0] = vel[1] = vel[2] = 0.0f; - NED[0] = NED[1] = 0.0f; - NED[2] = -(baroData.Altitude + baroOffset); - sensors |= HORIZ_SENSORS | HORIZ_POS_SENSORS; - sensors |= POS_SENSORS | VERT_SENSORS; - } - - if (gps_vel_updated && outdoor_mode) { - sensors |= HORIZ_SENSORS | VERT_SENSORS; - vel[0] = gpsVelData.North; - vel[1] = gpsVelData.East; - vel[2] = gpsVelData.Down; - } - - // Copy the position into the UAVO - PositionStateData positionState; - PositionStateGet(&positionState); - positionState.North = Nav.Pos[0]; - positionState.East = Nav.Pos[1]; - positionState.Down = Nav.Pos[2]; - PositionStateSet(&positionState); - - // airspeed correction needs current positionState - if (airspeed_updated) { - // we have airspeed available - AirspeedStateData airspeed; - AirspeedStateGet(&airspeed); - - airspeed.CalibratedAirspeed = airspeedData.CalibratedAirspeed; - airspeed.TrueAirspeed = (airspeedData.TrueAirspeed < 0.f) ? airspeed.CalibratedAirspeed *IAS2TAS(homeLocation.Altitude - positionState.Down) : airspeedData.TrueAirspeed; - - AirspeedStateSet(&airspeed); - - if (!gps_vel_updated && !gps_updated) { - // feed airspeed into EKF, treat wind as 1e2 variance - sensors |= HORIZ_SENSORS | VERT_SENSORS; - INSSetPosVelVar((float[3]) { ekfConfiguration.FakeR.FakeGPSPosIndoor, - ekfConfiguration.FakeR.FakeGPSPosIndoor, - ekfConfiguration.FakeR.FakeGPSPosIndoor }, - (float[3]) { ekfConfiguration.FakeR.FakeGPSVelAirspeed, - ekfConfiguration.FakeR.FakeGPSVelAirspeed, - ekfConfiguration.FakeR.FakeGPSVelAirspeed } - ); - // rotate airspeed vector into NED frame - airspeed is measured in X axis only - float R[3][3]; - Quaternion2R(Nav.q, R); - float vtas[3] = { airspeed.TrueAirspeed, 0.0f, 0.0f }; - rot_mult(R, vtas, vel); - } - } - - /* - * TODO: Need to add a general sanity check for all the inputs to make sure their kosher - * although probably should occur within INS itself - */ - if (sensors) { - INSCorrection(&magData.x, NED, vel, (baroData.Altitude + baroOffset), sensors); - } - - // Copy the velocity into the UAVO - VelocityStateData velocityState; - VelocityStateGet(&velocityState); - velocityState.North = Nav.Vel[0]; - velocityState.East = Nav.Vel[1]; - velocityState.Down = Nav.Vel[2]; - VelocityStateSet(&velocityState); - - GyroStateData gyroState; - gyroState.x = RAD2DEG(gyros[0] - RAD2DEG(Nav.gyro_bias[0])); - gyroState.y = RAD2DEG(gyros[1] - RAD2DEG(Nav.gyro_bias[1])); - gyroState.z = RAD2DEG(gyros[2] - RAD2DEG(Nav.gyro_bias[2])); - GyroStateSet(&gyroState); - - EKFStateVarianceData vardata; - EKFStateVarianceGet(&vardata); - INSGetP(cast_struct_to_array(vardata.P, vardata.P.AttitudeQ1)); - EKFStateVarianceSet(&vardata); - - return 0; -} - -/** - * @brief Convert the GPS LLA position into NED coordinates - * @note this method uses a taylor expansion around the home coordinates - * to convert to NED which allows it to be done with all floating - * calculations - * @param[in] Current GPS coordinates - * @param[out] NED frame coordinates - * @returns 0 for success, -1 for failure - */ -float T[3]; -static int32_t getNED(GPSPositionSensorData *gpsPosition, float *NED) -{ - float dL[3] = { DEG2RAD((gpsPosition->Latitude - homeLocation.Latitude) / 10.0e6f), - DEG2RAD((gpsPosition->Longitude - homeLocation.Longitude) / 10.0e6f), - (gpsPosition->Altitude + gpsPosition->GeoidSeparation - homeLocation.Altitude) }; - - NED[0] = T[0] * dL[0]; - NED[1] = T[1] * dL[1]; - NED[2] = T[2] * dL[2]; - - return 0; -} - -static void settingsUpdatedCb(UAVObjEvent *ev) -{ - if (ev == NULL || ev->obj == FlightStatusHandle()) { - FlightStatusGet(&flightStatus); - } - if (ev == NULL || ev->obj == RevoCalibrationHandle()) { - RevoCalibrationGet(&revoCalibration); - } - // change of these settings require reinitialization of the EKF - // when an error flag has been risen, we also listen to flightStatus updates, - // since we are waiting for the system to get disarmed so we can reinitialize safely. - if (ev == NULL || - ev->obj == EKFConfigurationHandle() || - ev->obj == RevoSettingsHandle() || - (variance_error == true && ev->obj == FlightStatusHandle()) - ) { - bool error = false; - - EKFConfigurationGet(&ekfConfiguration); - int t; - for (t = 0; t < EKFCONFIGURATION_P_NUMELEM; t++) { - if (invalid_var(cast_struct_to_array(ekfConfiguration.P, ekfConfiguration.P.AttitudeQ1)[t])) { - error = true; - } - } - for (t = 0; t < EKFCONFIGURATION_Q_NUMELEM; t++) { - if (invalid_var(cast_struct_to_array(ekfConfiguration.Q, ekfConfiguration.Q.AccelX)[t])) { - error = true; - } - } - for (t = 0; t < EKFCONFIGURATION_R_NUMELEM; t++) { - if (invalid_var(cast_struct_to_array(ekfConfiguration.R, ekfConfiguration.R.BaroZ)[t])) { - error = true; - } - } - - RevoSettingsGet(&revoSettings); - - // Reinitialization of the EKF is not desired during flight. - // It will be delayed until the board is disarmed by raising the error flag. - // We will not prevent the initial initialization though, since the board could be in always armed mode. - if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED && !initialization_required) { - error = true; - } - - if (error) { - variance_error = true; - } else { - // trigger reinitialization - possibly with new algorithm - running_algorithm = revoSettings.FusionAlgorithm; - variance_error = false; - initialization_required = true; - } - } - if (ev == NULL || ev->obj == HomeLocationHandle()) { - HomeLocationGet(&homeLocation); - // Compute matrix to convert deltaLLA to NED - float lat, alt; - lat = DEG2RAD(homeLocation.Latitude / 10.0e6f); - alt = homeLocation.Altitude; - - T[0] = alt + 6.378137E6f; - T[1] = cosf(lat) * (alt + 6.378137E6f); - T[2] = -1.0f; - - // TODO: convert positionState to new reference frame and gracefully update EKF state! - // needed for long range flights where the reference coordinate is adjusted in flight - } - if (ev == NULL || ev->obj == AttitudeSettingsHandle()) { - AttitudeSettingsGet(&attitudeSettings); - - // Calculate accel filter alpha, in the same way as for gyro data in stabilization module. - const float fakeDt = 0.0015f; - if (attitudeSettings.AccelTau < 0.0001f) { - accel_alpha = 0; // not trusting this to resolve to 0 - accel_filter_enabled = false; - } else { - accel_alpha = expf(-fakeDt / attitudeSettings.AccelTau); - accel_filter_enabled = true; - } - } -} - -/** - * Perform an update of the @ref MagBias based on - * Magmeter Offset Cancellation: Theory and Implementation, - * revisited William Premerlani, October 14, 2011 - */ -static void magOffsetEstimation(MagSensorData *mag) -{ - #if 0 - // Constants, to possibly go into a UAVO - static const float MIN_NORM_DIFFERENCE = 50; - - static float B2[3] = { 0, 0, 0 }; - - MagBiasData magBias; - MagBiasGet(&magBias); - - // Remove the current estimate of the bias - mag->x -= magBias.x; - mag->y -= magBias.y; - mag->z -= magBias.z; - - // First call - if (B2[0] == 0 && B2[1] == 0 && B2[2] == 0) { - B2[0] = mag->x; - B2[1] = mag->y; - B2[2] = mag->z; - return; - } - - float B1[3] = { mag->x, mag->y, mag->z }; - float norm_diff = sqrtf(powf(B2[0] - B1[0], 2) + powf(B2[1] - B1[1], 2) + powf(B2[2] - B1[2], 2)); - if (norm_diff > MIN_NORM_DIFFERENCE) { - float norm_b1 = sqrtf(B1[0] * B1[0] + B1[1] * B1[1] + B1[2] * B1[2]); - float norm_b2 = sqrtf(B2[0] * B2[0] + B2[1] * B2[1] + B2[2] * B2[2]); - float scale = cal.MagBiasNullingRate * (norm_b2 - norm_b1) / norm_diff; - float b_error[3] = { (B2[0] - B1[0]) * scale, (B2[1] - B1[1]) * scale, (B2[2] - B1[2]) * scale }; - - magBias.x += b_error[0]; - magBias.y += b_error[1]; - magBias.z += b_error[2]; - - MagBiasSet(&magBias); - - // Store this value to compare against next update - B2[0] = B1[0]; B2[1] = B1[1]; B2[2] = B1[2]; - } - #else // if 0 - static float magBias[3] = { 0 }; - - // Remove the current estimate of the bias - mag->x -= magBias[0]; - mag->y -= magBias[1]; - mag->z -= magBias[2]; - - AttitudeStateData attitude; - AttitudeStateGet(&attitude); - - const float Rxy = sqrtf(homeLocation.Be[0] * homeLocation.Be[0] + homeLocation.Be[1] * homeLocation.Be[1]); - const float Rz = homeLocation.Be[2]; - - const float rate = revoCalibration.MagBiasNullingRate; - float Rot[3][3]; - float B_e[3]; - float xy[2]; - float delta[3]; - - // Get the rotation matrix - Quaternion2R(&attitude.q1, Rot); - - // Rotate the mag into the NED frame - B_e[0] = Rot[0][0] * mag->x + Rot[1][0] * mag->y + Rot[2][0] * mag->z; - B_e[1] = Rot[0][1] * mag->x + Rot[1][1] * mag->y + Rot[2][1] * mag->z; - B_e[2] = Rot[0][2] * mag->x + Rot[1][2] * mag->y + Rot[2][2] * mag->z; - - float cy = cosf(DEG2RAD(attitude.Yaw)); - float sy = sinf(DEG2RAD(attitude.Yaw)); - - xy[0] = cy * B_e[0] + sy * B_e[1]; - xy[1] = -sy * B_e[0] + cy * B_e[1]; - - float xy_norm = sqrtf(xy[0] * xy[0] + xy[1] * xy[1]); - - delta[0] = -rate * (xy[0] / xy_norm * Rxy - xy[0]); - delta[1] = -rate * (xy[1] / xy_norm * Rxy - xy[1]); - delta[2] = -rate * (Rz - B_e[2]); - - if (!isnan(delta[0]) && !isinf(delta[0]) && - !isnan(delta[1]) && !isinf(delta[1]) && - !isnan(delta[2]) && !isinf(delta[2])) { - magBias[0] += delta[0]; - magBias[1] += delta[1]; - magBias[2] += delta[2]; - } - #endif // if 0 -} - - -/** - * @} - * @} - */ diff --git a/flight/modules/Attitude/revolution/inc/attitude.h b/flight/modules/Attitude/revolution/inc/attitude.h deleted file mode 100644 index 64c556a8e..000000000 --- a/flight/modules/Attitude/revolution/inc/attitude.h +++ /dev/null @@ -1,37 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup OpenPilotModules OpenPilot Modules - * @{ - * @addtogroup Attitude Attitude Module - * @{ - * - * @file attitude.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. - * @brief Acquires sensor data and fuses it into attitude estimate for CC - * - * @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 ATTITUDE_H -#define ATTITUDE_H - -#include "openpilot.h" - -int32_t AttitudeInitialize(void); - -#endif // ATTITUDE_H