Merge Android Pie into master
Bug: 112104996
Change-Id: I5786617e5db720dc171326bfbf52920d6b88abf3
diff --git a/Android.bp b/Android.bp
deleted file mode 100644
index 1e0d8c8..0000000
--- a/Android.bp
+++ /dev/null
@@ -1,3 +0,0 @@
-subdirs = [
- "lib",
-]
diff --git a/contexthubhal/Android.bp b/contexthubhal/Android.bp
new file mode 100644
index 0000000..130f7de
--- /dev/null
+++ b/contexthubhal/Android.bp
@@ -0,0 +1,88 @@
+//
+// Copyright (C) 2018 The Android Open Source Project
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+cc_defaults {
+ name: "contexthub_libs_default",
+ relative_install_path: "hw",
+ srcs: [
+ "nanohubhal.cpp",
+ "nanohubhal_default.cpp",
+ "system_comms.cpp",
+ ],
+ cflags: ["-Wall", "-Werror", "-Wextra"],
+ shared_libs: [
+ "liblog",
+ "libcutils",
+ "libutils",
+ "libstagefright_foundation",
+ ],
+ static_libs: [
+ "libjsoncpp",
+ "libhubutilcommon",
+ ],
+ header_libs: [
+ "libnanohub_common_headers",
+ "libhardware_headers",
+ "libutils_headers",
+ ],
+ vendor: true,
+}
+
+cc_defaults {
+ name: "contexthub_hidl_libs_default",
+ srcs: [
+ "NanohubHidlAdapter.cpp",
+ ],
+ shared_libs: [
+ "libhidlbase",
+ "libhidltransport",
+ "android.hardware.contexthub@1.0",
+ ],
+}
+
+cc_library {
+ name: "context_hub.default",
+ srcs: [
+ "legacyhal.cpp",
+ ],
+ defaults: [
+ "contexthub_libs_default",
+ ],
+}
+
+cc_library_shared {
+ name: "android.hardware.contexthub@1.0-impl.nanohub",
+ shared_libs: [
+ "libbase",
+ ],
+ defaults: [
+ "contexthub_libs_default",
+ "contexthub_hidl_libs_default",
+ ],
+}
+
+cc_binary {
+ name: "android.hardware.contexthub@1.0-service.nanohub",
+ init_rc: ["android.hardware.contexthub@1.0-service.nanohub.rc"],
+ srcs: [
+ "service.cpp",
+ ],
+ shared_libs: [
+ "libhwbinder",
+ ],
+ defaults: [
+ "contexthub_libs_default",
+ "contexthub_hidl_libs_default",
+ ],
+}
diff --git a/contexthubhal/Android.mk b/contexthubhal/Android.mk
deleted file mode 100644
index be27d47..0000000
--- a/contexthubhal/Android.mk
+++ /dev/null
@@ -1,26 +0,0 @@
-LOCAL_PATH := $(call my-dir)
-
-# HAL module implemenation stored in
-# hw/<CONTEXT_HUB_MODULE_ID>.<ro.hardware>.so
-include $(CLEAR_VARS)
-
-LOCAL_MODULE_RELATIVE_PATH := hw
-LOCAL_MULTILIB := both
-LOCAL_SHARED_LIBRARIES := liblog libcutils
-LOCAL_SRC_FILES := nanohubhal.cpp system_comms.cpp
-LOCAL_CFLAGS := -Wall -Werror -Wextra
-LOCAL_MODULE_OWNER := google
-
-# Include target-specific files.
-LOCAL_SRC_FILES += nanohubhal_default.cpp
-
-LOCAL_HEADER_LIBRARIES := \
- libhardware_headers \
- libnanohub_common_headers \
- libutils_headers
-
-LOCAL_MODULE := context_hub.default
-LOCAL_MODULE_TAGS := optional
-LOCAL_PROPRIETARY_MODULE := true
-
-include $(BUILD_SHARED_LIBRARY)
diff --git a/contexthubhal/NanohubHidlAdapter.cpp b/contexthubhal/NanohubHidlAdapter.cpp
new file mode 100644
index 0000000..9efca2a
--- /dev/null
+++ b/contexthubhal/NanohubHidlAdapter.cpp
@@ -0,0 +1,591 @@
+/*
+ * Copyright (C) 2018 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+ /*
+ * This file is based on:
+ * hardware/interfaces/contexthub/1.0/default/Contexthub.cpp
+ * with modifications to connect directly to the NanohubHAL and
+ * support endpoints.
+ */
+
+#include "NanohubHidlAdapter.h"
+#include "nanohub_perdevice.h"
+
+#include <inttypes.h>
+
+#include <log/log.h>
+#include <utils/String8.h>
+#include <sys/stat.h>
+
+#include <android/hardware/contexthub/1.0/IContexthub.h>
+#include <hardware/context_hub.h>
+#include <sys/endian.h>
+
+#undef LOG_TAG
+#define LOG_TAG "NanohubHidlAdapter"
+
+using namespace android::nanohub;
+
+namespace android {
+namespace hardware {
+namespace contexthub {
+namespace V1_0 {
+namespace implementation {
+
+static constexpr uint64_t ALL_APPS = UINT64_C(0xFFFFFFFFFFFFFFFF);
+
+Contexthub::Contexthub()
+ : mDeathRecipient(new DeathRecipient(this)),
+ mIsTransactionPending(false) {
+}
+
+bool Contexthub::setOsAppAsDestination(hub_message_t *msg, int hubId) {
+ if (!isValidHubId(hubId)) {
+ ALOGW("%s: Hub information is null for hubHandle %d",
+ __FUNCTION__,
+ hubId);
+ return false;
+ } else {
+ msg->app_name = mCachedHubInfo[hubId].osAppName;
+ return true;
+ }
+}
+
+Return<void> Contexthub::getHubs(getHubs_cb _hidl_cb) {
+ std::vector<ContextHub> hubs;
+ const context_hub_t *hub = nanohub::get_hub_info();
+
+ mCachedHubInfo.clear();
+
+ CachedHubInformation info;
+ ContextHub c;
+
+ c.name = hub->name;
+ c.vendor = hub->vendor;
+ c.toolchain = hub->toolchain;
+ c.platformVersion = hub->platform_version;
+ c.toolchainVersion = hub->toolchain_version;
+ c.hubId = hub->hub_id;
+ c.peakMips = hub->peak_mips;
+ c.stoppedPowerDrawMw = hub->stopped_power_draw_mw;
+ c.sleepPowerDrawMw = hub->sleep_power_draw_mw;
+ c.peakPowerDrawMw = hub->peak_power_draw_mw;
+ // c.connectedSensors =
+ c.maxSupportedMsgLen = hub->max_supported_msg_len;
+ // TODO: get this information from nanohub
+ c.chrePlatformId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0);
+ c.chreApiMajorVersion = 0x01;
+ c.chreApiMinorVersion = 0x02;
+ c.chrePatchVersion = NANOHUB_OS_PATCH_LEVEL;
+
+ info.callback = nullptr;
+ info.osAppName = hub->os_app_name;
+ mCachedHubInfo[hub->hub_id] = info;
+
+ hubs.push_back(c);
+
+ _hidl_cb(hubs);
+ return Void();
+}
+
+Contexthub::DeathRecipient::DeathRecipient(sp<Contexthub> contexthub)
+ : mContexthub(contexthub) {}
+
+void Contexthub::DeathRecipient::serviceDied(
+ uint64_t cookie,
+ const wp<::android::hidl::base::V1_0::IBase>& /*who*/) {
+ uint32_t hubId = static_cast<uint32_t>(cookie);
+ mContexthub->handleServiceDeath(hubId);
+}
+
+bool Contexthub::isValidHubId(uint32_t hubId) {
+ if (!mCachedHubInfo.count(hubId)) {
+ ALOGW("Hub information not found for hubId %" PRIu32, hubId);
+ return false;
+ } else {
+ return true;
+ }
+}
+
+sp<IContexthubCallback> Contexthub::getCallBackForHubId(uint32_t hubId) {
+ if (!isValidHubId(hubId)) {
+ return nullptr;
+ } else {
+ return mCachedHubInfo[hubId].callback;
+ }
+}
+
+Return<Result> Contexthub::sendMessageToHub(uint32_t hubId,
+ const ContextHubMsg &msg) {
+ if (!isValidHubId(hubId) || msg.msg.size() > UINT32_MAX) {
+ return Result::BAD_PARAMS;
+ }
+
+ hub_message_t txMsg = {
+ .app_name.id = msg.appName,
+ .message_type = msg.msgType,
+ .message_len = static_cast<uint32_t>(msg.msg.size()), // Note the check above
+ .message = static_cast<const uint8_t *>(msg.msg.data()),
+ };
+
+ // Use a dummy to prevent send_message with empty message from failing prematurely
+ static uint8_t dummy;
+ if (txMsg.message_len == 0 && txMsg.message == nullptr) {
+ txMsg.message = &dummy;
+ }
+
+ ALOGI("Sending msg of type %" PRIu32 ", size %" PRIu32 " to app 0x%" PRIx64,
+ txMsg.message_type,
+ txMsg.message_len,
+ txMsg.app_name.id);
+
+ if(NanoHub::sendToNanohub(hubId, &txMsg, 0, msg.hostEndPoint) != 0) {
+ return Result::TRANSACTION_FAILED;
+ }
+
+ return Result::OK;
+}
+
+Return<Result> Contexthub::registerCallback(uint32_t hubId,
+ const sp<IContexthubCallback> &cb) {
+ Return<Result> retVal = Result::BAD_PARAMS;
+
+ if (!isValidHubId(hubId)) {
+ // Initialized, but hubId is not valid
+ retVal = Result::BAD_PARAMS;
+ } else if (NanoHub::subscribeMessages(hubId,
+ contextHubCb,
+ this) == 0) {
+ // Initialized && valid hub && subscription successful
+ if (mCachedHubInfo[hubId].callback != nullptr) {
+ ALOGD("Modifying callback for hubId %" PRIu32, hubId);
+ mCachedHubInfo[hubId].callback->unlinkToDeath(mDeathRecipient);
+ }
+
+ mCachedHubInfo[hubId].callback = cb;
+ if (cb != nullptr) {
+ Return<bool> linkResult = cb->linkToDeath(mDeathRecipient, hubId);
+ bool linkSuccess = linkResult.isOk() ?
+ static_cast<bool>(linkResult) : false;
+ if (!linkSuccess) {
+ ALOGW("Couldn't link death recipient for hubId %" PRIu32,
+ hubId);
+ }
+ }
+ retVal = Result::OK;
+ } else {
+ // Initalized && valid hubId - but subscription unsuccessful
+ // This is likely an internal error in the HAL implementation, but we
+ // cannot add more information.
+ ALOGW("Could not subscribe to the hub for callback");
+ retVal = Result::UNKNOWN_FAILURE;
+ }
+
+ return retVal;
+}
+
+static bool isValidOsStatus(const uint8_t *msg,
+ size_t msgLen,
+ status_response_t *rsp) {
+ // Workaround a bug in some HALs
+ if (msgLen == 1) {
+ rsp->result = msg[0];
+ return true;
+ }
+
+ if (msg == nullptr || msgLen != sizeof(*rsp)) {
+ ALOGI("Received invalid response (is null : %d, size %zu)",
+ msg == nullptr ? 1 : 0,
+ msgLen);
+ return false;
+ }
+
+ memcpy(rsp, msg, sizeof(*rsp));
+
+ // No sanity checks on return values
+ return true;
+}
+
+int Contexthub::handleOsMessage(sp<IContexthubCallback> cb,
+ uint32_t msgType,
+ const uint8_t *msg,
+ int msgLen,
+ uint32_t transactionId) {
+ int retVal = -1;
+
+
+ switch(msgType) {
+ case CONTEXT_HUB_APPS_ENABLE:
+ case CONTEXT_HUB_APPS_DISABLE:
+ case CONTEXT_HUB_LOAD_APP:
+ case CONTEXT_HUB_UNLOAD_APP:
+ {
+ struct status_response_t rsp;
+ TransactionResult result;
+ if (isValidOsStatus(msg, msgLen, &rsp) && rsp.result == 0) {
+ retVal = 0;
+ result = TransactionResult::SUCCESS;
+ } else {
+ result = TransactionResult::FAILURE;
+ }
+
+ mIsTransactionPending = false;
+ if (cb != nullptr) {
+ cb->handleTxnResult(transactionId, result);
+ }
+ retVal = 0;
+ break;
+ }
+
+ case CONTEXT_HUB_QUERY_APPS:
+ {
+ std::vector<HubAppInfo> apps;
+ int numApps = msgLen / sizeof(hub_app_info);
+ const hub_app_info *unalignedInfoAddr = reinterpret_cast<const hub_app_info *>(msg);
+
+ for (int i = 0; i < numApps; i++) {
+ hub_app_info query_info;
+ memcpy(&query_info, &unalignedInfoAddr[i], sizeof(query_info));
+ HubAppInfo app;
+ app.appId = query_info.app_name.id;
+ app.version = query_info.version;
+ // TODO :: Add memory ranges
+
+ apps.push_back(app);
+ }
+
+ if (cb != nullptr) {
+ cb->handleAppsInfo(apps);
+ }
+ retVal = 0;
+ break;
+ }
+
+ case CONTEXT_HUB_QUERY_MEMORY:
+ {
+ // Deferring this use
+ retVal = 0;
+ break;
+ }
+
+ case CONTEXT_HUB_OS_REBOOT:
+ {
+ mIsTransactionPending = false;
+ if (cb != nullptr) {
+ cb->handleHubEvent(AsyncEventType::RESTARTED);
+ }
+ retVal = 0;
+ break;
+ }
+
+ default:
+ {
+ retVal = -1;
+ break;
+ }
+ }
+
+ return retVal;
+}
+
+void Contexthub::handleServiceDeath(uint32_t hubId) {
+ ALOGI("Callback/service died for hubId %" PRIu32, hubId);
+ int ret = NanoHub::subscribeMessages(hubId, nullptr, nullptr);
+ if (ret != 0) {
+ ALOGW("Failed to unregister callback from hubId %" PRIu32 ": %d",
+ hubId, ret);
+ }
+ mCachedHubInfo[hubId].callback.clear();
+}
+
+int Contexthub::contextHubCb(uint32_t hubId,
+ const nanohub::HubMessage &rxMsg,
+ void *cookie) {
+ Contexthub *obj = static_cast<Contexthub *>(cookie);
+
+ if (!obj->isValidHubId(hubId)) {
+ ALOGW("Invalid hub Id %" PRIu32, hubId);
+ return -1;
+ }
+
+ sp<IContexthubCallback> cb = obj->getCallBackForHubId(hubId);
+
+ if (cb == nullptr) {
+ // This should not ever happen
+ ALOGW("No callback registered, returning");
+ return -1;
+ }
+
+ if (rxMsg.message_type < CONTEXT_HUB_TYPE_PRIVATE_MSG_BASE) {
+ obj->handleOsMessage(cb,
+ rxMsg.message_type,
+ static_cast<const uint8_t *>(rxMsg.message),
+ rxMsg.message_len,
+ rxMsg.message_transaction_id);
+ } else {
+ ContextHubMsg msg;
+
+ msg.appName = rxMsg.app_name.id;
+ msg.msgType = rxMsg.message_type;
+ msg.hostEndPoint = rxMsg.message_endpoint;
+ msg.msg = std::vector<uint8_t>(static_cast<const uint8_t *>(rxMsg.message),
+ static_cast<const uint8_t *>(rxMsg.message) +
+ rxMsg.message_len);
+
+ cb->handleClientMsg(msg);
+ }
+
+ return 0;
+}
+
+Return<Result> Contexthub::unloadNanoApp(uint32_t hubId,
+ uint64_t appId,
+ uint32_t transactionId) {
+ if (mIsTransactionPending) {
+ return Result::TRANSACTION_PENDING;
+ }
+
+ hub_message_t msg;
+
+ if (setOsAppAsDestination(&msg, hubId) == false) {
+ return Result::BAD_PARAMS;
+ }
+
+ struct apps_disable_request_t req;
+
+ msg.message_type = CONTEXT_HUB_UNLOAD_APP;
+ msg.message_len = sizeof(req);
+ msg.message = &req;
+ req.app_name.id = appId;
+
+ if(NanoHub::sendToNanohub(hubId,
+ &msg,
+ transactionId,
+ static_cast<uint16_t>(HostEndPoint::UNSPECIFIED)) != 0) {
+ return Result::TRANSACTION_FAILED;
+ } else {
+ mIsTransactionPending = true;
+ return Result::OK;
+ }
+}
+
+Return<Result> Contexthub::loadNanoApp(uint32_t hubId,
+ const NanoAppBinary& appBinary,
+ uint32_t transactionId) {
+ if (mIsTransactionPending) {
+ return Result::TRANSACTION_PENDING;
+ }
+
+ hub_message_t hubMsg;
+
+ if (setOsAppAsDestination(&hubMsg, hubId) == false) {
+ return Result::BAD_PARAMS;
+ }
+
+ // Data from the nanoapp header is passed through HIDL as explicit fields,
+ // but the legacy HAL expects it prepended to the binary, therefore we must
+ // reconstruct it here prior to passing to the legacy HAL.
+ const struct nano_app_binary_t header = {
+ .header_version = htole32(1),
+ .magic = htole32(NANOAPP_MAGIC),
+ .app_id.id = htole64(appBinary.appId),
+ .app_version = htole32(appBinary.appVersion),
+ .flags = htole32(appBinary.flags),
+ .hw_hub_type = htole64(0),
+ .target_chre_api_major_version = appBinary.targetChreApiMajorVersion,
+ .target_chre_api_minor_version = appBinary.targetChreApiMinorVersion,
+ };
+ const uint8_t *headerBytes = reinterpret_cast<const uint8_t *>(&header);
+
+ std::vector<uint8_t> binaryWithHeader(appBinary.customBinary);
+ binaryWithHeader.insert(binaryWithHeader.begin(),
+ headerBytes,
+ headerBytes + sizeof(header));
+
+ hubMsg.message_type = CONTEXT_HUB_LOAD_APP;
+ hubMsg.message_len = binaryWithHeader.size();
+ hubMsg.message = binaryWithHeader.data();
+
+ if(NanoHub::sendToNanohub(hubId,
+ &hubMsg,
+ transactionId,
+ static_cast<uint16_t>(HostEndPoint::UNSPECIFIED)) != 0) {
+ return Result::TRANSACTION_FAILED;
+ } else {
+ mIsTransactionPending = true;
+ return Result::OK;
+ }
+}
+
+Return<Result> Contexthub::enableNanoApp(uint32_t hubId,
+ uint64_t appId,
+ uint32_t transactionId) {
+ if (mIsTransactionPending) {
+ return Result::TRANSACTION_PENDING;
+ }
+
+ hub_message_t msg;
+
+ if (setOsAppAsDestination(&msg, hubId) == false) {
+ return Result::BAD_PARAMS;
+ }
+
+ struct apps_enable_request_t req;
+
+ msg.message_type = CONTEXT_HUB_APPS_ENABLE;
+ msg.message_len = sizeof(req);
+ req.app_name.id = appId;
+ msg.message = &req;
+
+ if(NanoHub::sendToNanohub(hubId,
+ &msg,
+ transactionId,
+ static_cast<uint16_t>(HostEndPoint::UNSPECIFIED)) != 0) {
+ return Result::TRANSACTION_FAILED;
+ } else {
+ mIsTransactionPending = true;
+ return Result::OK;
+ }
+}
+
+Return<Result> Contexthub::disableNanoApp(uint32_t hubId,
+ uint64_t appId,
+ uint32_t transactionId) {
+ if (mIsTransactionPending) {
+ return Result::TRANSACTION_PENDING;
+ }
+
+ hub_message_t msg;
+
+ if (setOsAppAsDestination(&msg, hubId) == false) {
+ return Result::BAD_PARAMS;
+ }
+
+ struct apps_disable_request_t req;
+
+ msg.message_type = CONTEXT_HUB_APPS_DISABLE;
+ msg.message_len = sizeof(req);
+ req.app_name.id = appId;
+ msg.message = &req;
+
+ if(NanoHub::sendToNanohub(hubId,
+ &msg,
+ transactionId,
+ static_cast<uint16_t>(HostEndPoint::UNSPECIFIED)) != 0) {
+ return Result::TRANSACTION_FAILED;
+ } else {
+ mIsTransactionPending = true;
+ return Result::OK;
+ }
+}
+
+Return<Result> Contexthub::queryApps(uint32_t hubId) {
+ hub_message_t msg;
+
+ if (setOsAppAsDestination(&msg, hubId) == false) {
+ ALOGW("Could not find hubId %" PRIu32, hubId);
+ return Result::BAD_PARAMS;
+ }
+
+ query_apps_request_t payload;
+ payload.app_name.id = ALL_APPS; // TODO : Pass this in as a parameter
+ msg.message = &payload;
+ msg.message_len = sizeof(payload);
+ msg.message_type = CONTEXT_HUB_QUERY_APPS;
+
+ if(NanoHub::sendToNanohub(hubId,
+ &msg,
+ 0,
+ static_cast<uint16_t>(HostEndPoint::UNSPECIFIED)) != 0) {
+ ALOGW("Query Apps sendMessage failed");
+ return Result::TRANSACTION_FAILED;
+ }
+
+ return Result::OK;
+}
+
+IContexthub *HIDL_FETCH_IContexthub(const char *) {
+ return new Contexthub();
+}
+
+static bool readApp(const char *file, NanoAppBinary *appBinary)
+{
+ bool success = false;
+ int fd = open(file, O_RDONLY);
+
+ if (fd >= 0) {
+ struct stat sb;
+ if (fstat(fd, &sb) == 0) {
+ void *buf = malloc(sb.st_size);
+ if (buf != nullptr && read(fd, buf, sb.st_size) == sb.st_size) {
+ success = true;
+ const struct nano_app_binary_t *header = static_cast<const struct nano_app_binary_t *>(buf);
+ appBinary->appId = header->app_id.id;
+ appBinary->appVersion = header->app_version;
+ appBinary->flags = header->flags;
+ appBinary->targetChreApiMajorVersion = header->target_chre_api_major_version;
+ appBinary->targetChreApiMinorVersion = header->target_chre_api_minor_version;
+ appBinary->customBinary = std::vector<uint8_t>(static_cast<const uint8_t *>(buf) + sizeof(struct nano_app_binary_t), static_cast<const uint8_t *>(buf) + sb.st_size);
+ }
+ free(buf);
+ }
+ close(fd);
+ }
+ return success;
+}
+
+Return<void> Contexthub::debug(const hidl_handle& hh_fd,
+ const hidl_vec<hidl_string>& hh_data) {
+ if (hh_fd == nullptr || hh_fd->numFds < 1) {
+ return Void();
+ }
+
+ String8 result;
+ int fd = hh_fd.getNativeHandle()->data[0];
+
+ if (hh_data.size() == 0) {
+ result.appendFormat("debug: %d\n", NanoHub::getDebugFlags());
+ std::string appInfo;
+ NanoHub::dumpAppInfo(appInfo);
+ result.append(appInfo.c_str());
+ } else if (hh_data.size() == 1) {
+ NanoHub::setDebugFlags(atoi(hh_data[0].c_str()));
+ result.appendFormat("debug: %d\n", NanoHub::getDebugFlags());
+ } else if (hh_data.size() == 2) {
+ if (strncmp(hh_data[0].c_str(), "load", 4) == 0) {
+ NanoAppBinary appBinary;
+ if (readApp(hh_data[1].c_str(), &appBinary))
+ loadNanoApp(0, appBinary, 0);
+ } else if (strncmp(hh_data[0].c_str(), "unload", 6) == 0) {
+ unloadNanoApp(0, strtoul(hh_data[1].c_str(), NULL, 16), 0);
+ } else if (strncmp(hh_data[0].c_str(), "enable", 6) == 0) {
+ enableNanoApp(0, strtoul(hh_data[1].c_str(), NULL, 16), 0);
+ } else if (strncmp(hh_data[0].c_str(), "disable", 7) == 0) {
+ disableNanoApp(0, strtoul(hh_data[1].c_str(), NULL, 16), 0);
+ }
+ } else {
+ result.appendFormat("unknown debug options");
+ }
+ write(fd, result.string(), result.size());
+
+ return Void();
+}
+
+} // namespace implementation
+} // namespace V1_0
+} // namespace contexthub
+} // namespace hardware
+} // namespace android
diff --git a/contexthubhal/NanohubHidlAdapter.h b/contexthubhal/NanohubHidlAdapter.h
new file mode 100644
index 0000000..79d6242
--- /dev/null
+++ b/contexthubhal/NanohubHidlAdapter.h
@@ -0,0 +1,132 @@
+/*
+ * Copyright (C) 2018 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+ /*
+ * This file is based on:
+ * hardware/interfaces/contexthub/1.0/default/Contexthub.h
+ * with modifications to connect directly to the NanohubHAL and
+ * support endpoints.
+ */
+
+#ifndef _NANOHUB_HIDL_ADAPTER_H_
+#define _NANOHUB_HIDL_ADAPTER_H_
+
+#include <unordered_map>
+
+#include <android-base/macros.h>
+#include <android/hardware/contexthub/1.0/IContexthub.h>
+#include <hardware/context_hub.h>
+
+#include "nanohubhal.h"
+
+namespace android {
+namespace hardware {
+namespace contexthub {
+namespace V1_0 {
+namespace implementation {
+
+struct Contexthub : public ::android::hardware::contexthub::V1_0::IContexthub {
+ Contexthub();
+
+ Return<void> getHubs(getHubs_cb _hidl_cb) override;
+
+ Return<Result> registerCallback(uint32_t hubId,
+ const sp<IContexthubCallback> &cb) override;
+
+ Return<Result> sendMessageToHub(uint32_t hubId,
+ const ContextHubMsg &msg) override;
+
+ Return<Result> loadNanoApp(uint32_t hubId,
+ const NanoAppBinary& appBinary,
+ uint32_t transactionId) override;
+
+ Return<Result> unloadNanoApp(uint32_t hubId,
+ uint64_t appId,
+ uint32_t transactionId) override;
+
+ Return<Result> enableNanoApp(uint32_t hubId,
+ uint64_t appId,
+ uint32_t transactionId) override;
+
+ Return<Result> disableNanoApp(uint32_t hubId,
+ uint64_t appId,
+ uint32_t transactionId) override;
+
+ Return<Result> queryApps(uint32_t hubId) override;
+
+ Return<Result> reboot(uint32_t hubId);
+
+ Return<void> debug(const hidl_handle& fd, const hidl_vec<hidl_string>& options) override;
+
+ bool isInitialized();
+
+
+private:
+
+ struct CachedHubInformation{
+ struct hub_app_name_t osAppName;
+ sp<IContexthubCallback> callback;
+ };
+
+ class DeathRecipient : public hidl_death_recipient {
+ public:
+ DeathRecipient(const sp<Contexthub> contexthub);
+
+ void serviceDied(
+ uint64_t cookie,
+ const wp<::android::hidl::base::V1_0::IBase>& who) override;
+
+ private:
+ sp<Contexthub> mContexthub;
+ };
+
+ std::unordered_map<uint32_t, CachedHubInformation> mCachedHubInfo;
+
+ sp<DeathRecipient> mDeathRecipient;
+ bool mIsTransactionPending;
+ uint32_t mTransactionId;
+
+ bool isValidHubId(uint32_t hubId);
+
+ sp<IContexthubCallback> getCallBackForHubId(uint32_t hubId);
+
+ int handleOsMessage(sp<IContexthubCallback> cb,
+ uint32_t msgType,
+ const uint8_t *msg,
+ int msgLen,
+ uint32_t transactionId);
+
+ // Handle the case where the callback registered for the given hub ID dies
+ void handleServiceDeath(uint32_t hubId);
+
+ static int contextHubCb(uint32_t hubId,
+ const nanohub::HubMessage &rxMsg,
+ void *cookie);
+
+ bool setOsAppAsDestination(hub_message_t *msg, int hubId);
+
+ DISALLOW_COPY_AND_ASSIGN(Contexthub);
+};
+
+extern "C" IContexthub *HIDL_FETCH_IContexthub(const char *);
+
+} // namespace implementation
+} // namespace V1_0
+} // namespace contexthub
+} // namespace hardware
+} // namespace android
+
+#endif // _NANOHUB_HIDL_ADAPTER_H_
diff --git a/contexthubhal/android.hardware.contexthub@1.0-service.nanohub.rc b/contexthubhal/android.hardware.contexthub@1.0-service.nanohub.rc
new file mode 100644
index 0000000..a60545e
--- /dev/null
+++ b/contexthubhal/android.hardware.contexthub@1.0-service.nanohub.rc
@@ -0,0 +1,4 @@
+service contexthub-1-0 /vendor/bin/hw/android.hardware.contexthub@1.0-service.nanohub
+ class hal
+ user system
+ group system
diff --git a/contexthubhal/legacyhal.cpp b/contexthubhal/legacyhal.cpp
new file mode 100644
index 0000000..80ceedb
--- /dev/null
+++ b/contexthubhal/legacyhal.cpp
@@ -0,0 +1,63 @@
+/*
+ * Copyright (C) 2018 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "nanohub_perdevice.h"
+#include "nanohubhal.h"
+
+using namespace android::nanohub;
+
+static context_hub_callback *mSavedCbk = nullptr;
+
+static int legacy_cbk(uint32_t hub_id, const HubMessage &rxMsg, void *cookie)
+{
+ return mSavedCbk(hub_id, &rxMsg, cookie);
+}
+
+static int legacy_subscribe_messages(uint32_t hub_id, context_hub_callback *cbk, void *cookie)
+{
+ mSavedCbk = cbk;
+ if (cbk)
+ return NanoHub::subscribeMessages(hub_id, legacy_cbk, cookie);
+ else
+ return NanoHub::subscribeMessages(hub_id, nullptr, nullptr);
+}
+
+static int legacy_send_message(uint32_t hub_id, const hub_message_t *msg)
+{
+ return NanoHub::sendToNanohub(hub_id, msg, 0, ENDPOINT_UNSPECIFIED);
+}
+
+static int legacy_get_hubs(context_hub_module_t*, const context_hub_t ** list)
+{
+ *list = get_hub_info();
+
+ return 1; /* we have one hub */
+}
+
+context_hub_module_t HAL_MODULE_INFO_SYM = {
+ .common = {
+ .tag = HARDWARE_MODULE_TAG,
+ .module_api_version = CONTEXT_HUB_DEVICE_API_VERSION_1_0,
+ .hal_api_version = HARDWARE_HAL_API_VERSION,
+ .id = CONTEXT_HUB_MODULE_ID,
+ .name = "Nanohub HAL",
+ .author = "Google",
+ },
+
+ .get_hubs = legacy_get_hubs,
+ .subscribe_messages = legacy_subscribe_messages,
+ .send_message = legacy_send_message,
+};
diff --git a/contexthubhal/nanohubhal.cpp b/contexthubhal/nanohubhal.cpp
index 21b6374..2f750a6 100644
--- a/contexthubhal/nanohubhal.cpp
+++ b/contexthubhal/nanohubhal.cpp
@@ -65,13 +65,15 @@
return os;
}
-void dumpBuffer(const char *pfx, const hub_app_name_t &appId, uint32_t evtId, const void *data, size_t len, int status)
+void dumpBuffer(const char *pfx, const hub_app_name_t &appId, uint32_t evtId, uint16_t endpoint, const void *data, size_t len, int status)
{
std::ostringstream os;
const uint8_t *p = static_cast<const uint8_t *>(data);
os << pfx << ": [ID=" << appId << "; SZ=" << std::dec << len;
if (evtId)
os << "; EVT=" << std::hex << evtId;
+ if (endpoint)
+ os << "; EPT=" << std::hex << endpoint;
os << "]:" << std::hex;
for (size_t i = 0; i < len; ++i) {
os << " " << std::setfill('0') << std::setw(2) << (unsigned int)p[i];
@@ -160,7 +162,7 @@
}
}
-int NanoHub::doSendToDevice(const hub_app_name_t name, const void *data, uint32_t len, uint32_t messageType)
+int NanoHub::doSendToDevice(const hub_app_name_t name, const void *data, uint32_t len, uint32_t messageType, uint16_t endpoint)
{
if (len > MAX_RX_PACKET) {
return -EINVAL;
@@ -173,6 +175,7 @@
.appId = name.id,
.len = static_cast<uint8_t>(len),
.appEventId = messageType,
+ .endpoint = endpoint,
},
};
@@ -189,6 +192,11 @@
mAppTxCond.notify_all();
}
+void NanoHub::doDumpAppInfo(std::string &result)
+{
+ SystemComm::dumpAppInfo(result);
+}
+
void* NanoHub::runAppTx()
{
std::unique_lock<std::mutex> lk(mAppTxLock);
@@ -199,7 +207,7 @@
}
HubMessage &m = mAppTxQueue.front();
lk.unlock();
- mMsgCbkFunc(0, &m, mMsgCbkData);
+ mMsgCbkFunc(0, m, mMsgCbkData);
lk.lock();
mAppTxQueue.pop_front();
};
@@ -250,34 +258,45 @@
ALOGE("read failed with %d", ret);
break;
}
- if (ret < (int)sizeof(msg.hdr)) {
+ if (ret < (int)sizeof(msg.raw.hdr)) {
ALOGE("Only read %d bytes", ret);
break;
}
- uint32_t len = msg.hdr.len;
+ uint32_t len = msg.raw.hdr.len;
- if (len > sizeof(msg.data)) {
+ if (len > MAX_RX_PACKET) {
ALOGE("malformed packet with len %" PRIu32, len);
break;
}
// receive message from FW in legacy format
- if (ret != (int)(sizeof(msg.hdr) + len)) {
- ALOGE("Expected %zu bytes, read %d bytes", sizeof(msg.hdr) + len, ret);
+ if (ret == (int)(sizeof(msg.raw.hdr) + len)) {
+ ret = SystemComm::handleRx(&msg.raw);
+ if (ret > 0) {
+ hub_app_name_t app_name = { .id = msg.raw.hdr.appId };
+ if (messageTracingEnabled()) {
+ dumpBuffer("(RAW) DEV -> APP", app_name, msg.raw.hdr.eventId, 0, &msg.raw.data[0], len);
+ }
+ doSendToApp(HubMessage(&app_name, msg.raw.hdr.eventId, ENDPOINT_BROADCAST, &msg.raw.data[0], len));
+ }
+ // receive message from FW in chre format
+ } else if (ret == (int)(sizeof(msg.chre.hdr) + len)) {
+ ret = SystemComm::handleRx(&msg.chre);
+ if (ret > 0) {
+ hub_app_name_t app_name = { .id = msg.chre.hdr.appId };
+ if (messageTracingEnabled()) {
+ dumpBuffer("(CHRE) DEV -> APP", app_name, msg.chre.hdr.appEventId, msg.chre.hdr.endpoint, &msg.chre.data[0], len);
+ }
+ doSendToApp(HubMessage(&app_name, msg.chre.hdr.appEventId, msg.chre.hdr.endpoint, &msg.chre.data[0], len));
+ }
+ } else {
+ ALOGE("Expected (%zu|%zu) bytes, read %d bytes", sizeof(msg.raw.hdr) + len, sizeof(msg.chre.hdr) + len, ret);
break;
}
- ret = SystemComm::handleRx(&msg);
- if (ret < 0) {
+ if (ret < 0)
ALOGE("SystemComm::handleRx() returned %d", ret);
- } else if (ret) {
- hub_app_name_t app_name = { .id = msg.hdr.appId };
- if (messageTracingEnabled()) {
- dumpBuffer("DEV -> APP", app_name, msg.hdr.eventId, &msg.data[0], msg.hdr.len);
- }
- doSendToApp(HubMessage(&app_name, msg.hdr.eventId, &msg.data[0], msg.hdr.len));
- }
}
if (myFds[IDX_CLOSE_PIPE].revents & POLLIN) { // we have been asked to die
@@ -354,7 +373,7 @@
return 0;
}
-int NanoHub::doSubscribeMessages(uint32_t hub_id, context_hub_callback *cbk, void *cookie)
+int NanoHub::doSubscribeMessages(uint32_t hub_id, Contexthub_callback *cbk, void *cookie)
{
if (hub_id) {
return -ENODEV;
@@ -386,7 +405,7 @@
return ret;
}
-int NanoHub::doSendToNanohub(uint32_t hub_id, const hub_message_t *msg)
+int NanoHub::doSendToNanohub(uint32_t hub_id, const hub_message_t *msg, uint32_t transaction_id, uint16_t endpoint)
{
if (hub_id) {
return -ENODEV;
@@ -405,45 +424,23 @@
} else if (get_hub_info()->os_app_name == msg->app_name) {
//messages to the "system" app are special - hal handles them
if (messageTracingEnabled()) {
- dumpBuffer("APP -> HAL", msg->app_name, msg->message_type, msg->message, msg->message_len);
+ dumpBuffer("APP -> HAL", msg->app_name, msg->message_type, 0, msg->message, msg->message_len);
}
- ret = SystemComm::handleTx(msg);
+ ret = SystemComm::handleTx(msg, transaction_id);
} else if (msg->message_len > MAX_RX_PACKET) {
ALOGW("not sending invalid message 2");
ret = -EINVAL;
} else {
if (messageTracingEnabled()) {
- dumpBuffer("APP -> DEV", msg->app_name, msg->message_type, msg->message, msg->message_len);
+ dumpBuffer("APP -> DEV", msg->app_name, msg->message_type, endpoint, msg->message, msg->message_len);
}
- ret = doSendToDevice(msg->app_name, msg->message, msg->message_len, msg->message_type);
+ ret = doSendToDevice(msg->app_name, msg->message, msg->message_len, msg->message_type, endpoint);
}
}
return ret;
}
-static int hal_get_hubs(context_hub_module_t*, const context_hub_t ** list)
-{
- *list = get_hub_info();
-
- return 1; /* we have one hub */
-}
-
}; // namespace nanohub
}; // namespace android
-
-context_hub_module_t HAL_MODULE_INFO_SYM = {
- .common = {
- .tag = HARDWARE_MODULE_TAG,
- .module_api_version = CONTEXT_HUB_DEVICE_API_VERSION_1_0,
- .hal_api_version = HARDWARE_HAL_API_VERSION,
- .id = CONTEXT_HUB_MODULE_ID,
- .name = "Nanohub HAL",
- .author = "Google",
- },
-
- .get_hubs = android::nanohub::hal_get_hubs,
- .subscribe_messages = android::nanohub::NanoHub::subscribeMessages,
- .send_message = android::nanohub::NanoHub::sendToNanohub,
-};
diff --git a/contexthubhal/nanohubhal.h b/contexthubhal/nanohubhal.h
index 6a0432c..b9cc172 100644
--- a/contexthubhal/nanohubhal.h
+++ b/contexthubhal/nanohubhal.h
@@ -27,36 +27,50 @@
//as per protocol
#define MAX_RX_PACKET 128
+#define MAX_TX_PACKET 128
#define APP_FROM_HOST_EVENT_ID 0x000000F8
#define APP_FROM_HOST_CHRE_EVENT_ID 0x000000F9
+#define ENDPOINT_UNSPECIFIED 0xFFFE
+#define ENDPOINT_BROADCAST 0xFFFF
+
namespace android {
namespace nanohub {
-void dumpBuffer(const char *pfx, const hub_app_name_t &appId, uint32_t evtId, const void *data, size_t len, int status = 0);
+void dumpBuffer(const char *pfx, const hub_app_name_t &appId, uint32_t evtId, uint16_t endpoint, const void *data, size_t len, int status = 0);
struct nano_message_chre {
- HostMsgHdrChreV10 hdr;
+ HostMsgHdrChre hdr;
uint8_t data[MAX_RX_PACKET];
} __attribute__((packed));
-struct nano_message {
+struct nano_message_raw {
HostMsgHdr hdr;
uint8_t data[MAX_RX_PACKET];
} __attribute__((packed));
+union nano_message {
+ struct nano_message_chre chre;
+ struct nano_message_raw raw;
+} __attribute__((packed));
+
class HubMessage : public hub_message_t {
std::unique_ptr<uint8_t> data_;
public:
+ uint32_t message_transaction_id;
+ uint16_t message_endpoint;
HubMessage(const HubMessage &other) = delete;
HubMessage &operator = (const HubMessage &other) = delete;
- HubMessage(const hub_app_name_t *name, uint32_t typ, const void *data, uint32_t len) {
+ HubMessage(const hub_app_name_t *name, uint32_t typ, uint32_t transaction_id,
+ uint16_t endpoint, const void *data, uint32_t len) {
app_name = *name;
message_type = typ;
message_len = len;
message = data;
+ message_transaction_id = transaction_id;
+ message_endpoint = endpoint;
if (len > 0 && data != nullptr) {
data_ = std::unique_ptr<uint8_t>(new uint8_t[len]);
memcpy(data_.get(), data, len);
@@ -64,12 +78,31 @@
}
}
+ HubMessage(const hub_app_name_t *name, uint32_t typ, uint16_t endpoint, const void *data,
+ uint32_t len) : HubMessage(name, typ, 0, endpoint, data, len) { }
+
+ HubMessage(const hub_message_t *msg, uint32_t transaction_id, uint16_t endpoint) {
+ app_name = msg->app_name;
+ message_type = msg->message_type;
+ message_len = msg->message_len;
+ message = msg->message;
+ message_transaction_id = transaction_id;
+ message_endpoint = endpoint;
+ if (msg->message_len > 0 && msg->message != nullptr) {
+ data_ = std::unique_ptr<uint8_t>(new uint8_t[msg->message_len]);
+ memcpy(data_.get(), msg->message, msg->message_len);
+ message = data_.get();
+ }
+ }
+
HubMessage(HubMessage &&other) {
*this = (HubMessage &&)other;
}
HubMessage &operator = (HubMessage &&other) {
*static_cast<hub_message_t *>(this) = static_cast<hub_message_t>(other);
+ message_transaction_id = other.message_transaction_id;
+ message_endpoint = other.message_endpoint;
data_ = std::move(other.data_);
other.message = nullptr;
other.message_len = 0;
@@ -77,6 +110,8 @@
}
};
+typedef int Contexthub_callback(uint32_t hub_id, const HubMessage &rxed_msg, void *cookie);
+
class NanoHub {
std::mutex mLock;
bool mAppQuit;
@@ -85,7 +120,7 @@
std::list<HubMessage> mAppTxQueue;
std::thread mPollThread;
std::thread mAppThread;
- context_hub_callback *mMsgCbkFunc;
+ Contexthub_callback *mMsgCbkFunc;
int mThreadClosingPipe[2];
int mFd; // [0] is read end
void * mMsgCbkData;
@@ -113,10 +148,13 @@
return &theHub;
}
- int doSubscribeMessages(uint32_t hub_id, context_hub_callback *cbk, void *cookie);
- int doSendToNanohub(uint32_t hub_id, const hub_message_t *msg);
- int doSendToDevice(const hub_app_name_t name, const void *data, uint32_t len, uint32_t messageType = 0);
+ int doSubscribeMessages(uint32_t hub_id, Contexthub_callback *cbk, void *cookie);
+ int doSendToNanohub(uint32_t hub_id, const hub_message_t *msg,
+ uint32_t transaction_id, uint16_t endpoint);
+ int doSendToDevice(const hub_app_name_t name, const void *data, uint32_t len,
+ uint32_t messageType = 0, uint16_t endpoint = ENDPOINT_UNSPECIFIED);
void doSendToApp(HubMessage &&msg);
+ void doDumpAppInfo(std::string &result);
static constexpr unsigned int FL_MESSAGE_TRACING = 1;
@@ -135,20 +173,25 @@
static void setDebugFlags(unsigned int flags) {
hubInstance()->mFlags = flags;
}
+ static void dumpAppInfo(std::string &result) {
+ hubInstance()->doDumpAppInfo(result);
+ }
// messaging interface
// define callback to invoke for APP messages
- static int subscribeMessages(uint32_t hub_id, context_hub_callback *cbk, void *cookie) {
+ static int subscribeMessages(uint32_t hub_id, Contexthub_callback *cbk, void *cookie) {
return hubInstance()->doSubscribeMessages(hub_id, cbk, cookie);
}
// all messages from APP go here
- static int sendToNanohub(uint32_t hub_id, const hub_message_t *msg) {
- return hubInstance()->doSendToNanohub(hub_id, msg);
+ static int sendToNanohub(uint32_t hub_id, const hub_message_t *msg,
+ uint32_t transaction_id, uint16_t endpoint) {
+ return hubInstance()->doSendToNanohub(hub_id, msg, transaction_id, endpoint);
}
// passes message to kernel driver directly
- static int sendToDevice(const hub_app_name_t *name, const void *data, uint32_t len) {
- return hubInstance()->doSendToDevice(*name, data, len);
+ static int sendToDevice(const hub_app_name_t *name, const void *data, uint32_t len,
+ uint32_t transactionId) {
+ return hubInstance()->doSendToDevice(*name, data, len, transactionId, ENDPOINT_UNSPECIFIED);
}
// passes message to APP via callback
static void sendToApp(HubMessage &&msg) {
diff --git a/contexthubhal/service.cpp b/contexthubhal/service.cpp
new file mode 100644
index 0000000..645a269
--- /dev/null
+++ b/contexthubhal/service.cpp
@@ -0,0 +1,44 @@
+/*
+ * Copyright (C) 2018 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#define LOG_TAG "android.hardware.contexthub@1.0-service.nanohub"
+
+#include <android/hardware/contexthub/1.0/IContexthub.h>
+#include <hidl/HidlSupport.h>
+#include <hidl/HidlTransportSupport.h>
+
+#include "NanohubHidlAdapter.h"
+
+using android::hardware::configureRpcThreadpool;
+using android::hardware::joinRpcThreadpool;
+using android::hardware::contexthub::V1_0::IContexthub;
+using android::hardware::contexthub::V1_0::implementation::Contexthub;
+using namespace android;
+
+status_t registerContexthubService()
+{
+ sp<IContexthub> contexthub = new Contexthub();
+ return contexthub->registerAsService();
+}
+
+int main() {
+ configureRpcThreadpool(1, true);
+ status_t status = registerContexthubService();
+ if (status != OK) {
+ return status;
+ }
+ joinRpcThreadpool();
+}
diff --git a/contexthubhal/system_comms.cpp b/contexthubhal/system_comms.cpp
index b0e4bc9..6edd0cb 100644
--- a/contexthubhal/system_comms.cpp
+++ b/contexthubhal/system_comms.cpp
@@ -16,9 +16,13 @@
#define LOG_TAG "NanohubHAL"
+#include "file.h"
+#include <json/json.h>
+
#include <cassert>
#include <cerrno>
#include <cinttypes>
+#include <string>
#include <endian.h>
@@ -27,12 +31,20 @@
#include <log/log.h>
#include <endian.h>
+#include <sys/stat.h>
+
+#include <media/stagefright/foundation/ADebug.h>
#include <hardware/context_hub.h>
#include "nanohub_perdevice.h"
#include "system_comms.h"
#include "nanohubhal.h"
+#define CHRE_APP_DIR "/data/vendor/sensor/chre"
+#define CHRE_APP_DIR_PERMS (S_IRUSR | S_IWUSR | S_IXUSR)
+#define CHRE_APP_FILE_PERMS (S_IRUSR | S_IWUSR)
+#define CHRE_APP_SETTINGS CHRE_APP_DIR "/apps.json"
+
namespace android {
namespace nanohub {
@@ -47,74 +59,134 @@
buf.writeU64(name.id);
}
-static void readNanohubAppInfo(MessageBuf &buf, NanohubAppInfo &info)
+static void readNanohubMemInfo(MessageBuf &buf, NanohubMemInfo &mi)
{
- size_t pos = buf.getPos();
- readAppName(buf, info.name);
- info.version = buf.readU32();
- info.flashUse = buf.readU32();
- info.ramUse = buf.readU32();
- if ((buf.getPos() - pos) != sizeof(info)) {
- ALOGE("%s: failed to read object", __func__);
+ uint8_t type, len;
+ uint32_t ramFree = NANOHUB_MEM_SZ_UNKNOWN;
+ uint32_t eeFree = NANOHUB_MEM_SZ_UNKNOWN;
+ uint32_t sharedFree = NANOHUB_MEM_SZ_UNKNOWN;
+ uint32_t osFree = NANOHUB_MEM_SZ_UNKNOWN;
+
+ mi.flashSz = NANOHUB_MEM_SZ_UNKNOWN;
+ mi.blSz = NANOHUB_MEM_SZ_UNKNOWN;
+ mi.osSz = NANOHUB_MEM_SZ_UNKNOWN;
+ mi.sharedSz = NANOHUB_MEM_SZ_UNKNOWN;
+ mi.eeSz = NANOHUB_MEM_SZ_UNKNOWN;
+ mi.ramSz = NANOHUB_MEM_SZ_UNKNOWN;
+
+ mi.blUse = NANOHUB_MEM_SZ_UNKNOWN;
+ mi.osUse = NANOHUB_MEM_SZ_UNKNOWN;
+ mi.sharedUse = NANOHUB_MEM_SZ_UNKNOWN;
+ mi.eeUse = NANOHUB_MEM_SZ_UNKNOWN;
+ mi.ramUse = NANOHUB_MEM_SZ_UNKNOWN;
+
+ while (buf.getRoom() >= 2) {
+ type = buf.readU8();
+ len = buf.readU8();
+ if (buf.getRoom() >= len) {
+ switch(type) {
+ case NANOHUB_HAL_SYS_INFO_HEAP_FREE:
+ if (len == sizeof(ramFree))
+ ramFree = buf.readU32();
+ else
+ buf.readRaw(len);
+ break;
+ case NANOHUB_HAL_SYS_INFO_RAM_SIZE:
+ if (len == sizeof(mi.ramSz))
+ mi.ramSz = buf.readU32();
+ else
+ buf.readRaw(len);
+ break;
+ case NANOHUB_HAL_SYS_INFO_EEDATA_SIZE:
+ if (len == sizeof(mi.ramSz))
+ mi.eeSz = buf.readU32();
+ else
+ buf.readRaw(len);
+ break;
+ case NANOHUB_HAL_SYS_INFO_EEDATA_FREE:
+ if (len == sizeof(eeFree))
+ eeFree = buf.readU32();
+ else
+ buf.readRaw(len);
+ break;
+ case NANOHUB_HAL_SYS_INFO_CODE_SIZE:
+ if (len == sizeof(mi.osSz))
+ mi.osSz = buf.readU32();
+ else
+ buf.readRaw(len);
+ break;
+ case NANOHUB_HAL_SYS_INFO_CODE_FREE:
+ if (len == sizeof(osFree))
+ osFree = buf.readU32();
+ else
+ buf.readRaw(len);
+ break;
+ case NANOHUB_HAL_SYS_INFO_SHARED_SIZE:
+ if (len == sizeof(mi.sharedSz))
+ mi.sharedSz = buf.readU32();
+ else
+ buf.readRaw(len);
+ break;
+ case NANOHUB_HAL_SYS_INFO_SHARED_FREE:
+ if (len == sizeof(sharedFree))
+ sharedFree = buf.readU32();
+ else
+ buf.readRaw(len);
+ break;
+ case NANOHUB_HAL_SYS_INFO_END:
+ if (len != 0 || buf.getRoom() != 0) {
+ ALOGE("%s: failed to read object", __func__);
+ return;
+ }
+ break;
+ default:
+ ALOGI("%s: unsupported type: %d", __func__, type);
+ buf.readRaw(len);
+ break;
+ }
+ } else {
+ ALOGE("%s: failed to read object", __func__);
+ return;
+ }
}
+
+ if (buf.getRoom() != 0) {
+ ALOGE("%s: failed to read object", __func__);
+ return;
+ }
+
+ if (mi.ramSz != NANOHUB_MEM_SZ_UNKNOWN && ramFree != NANOHUB_MEM_SZ_UNKNOWN)
+ mi.ramUse = mi.ramSz - ramFree;
+ if (mi.eeSz != NANOHUB_MEM_SZ_UNKNOWN && eeFree != NANOHUB_MEM_SZ_UNKNOWN)
+ mi.eeUse = mi.eeSz - eeFree;
+ if (mi.osSz != NANOHUB_MEM_SZ_UNKNOWN && osFree != NANOHUB_MEM_SZ_UNKNOWN)
+ mi.osUse = mi.osSz - osFree;
+ if (mi.sharedSz != NANOHUB_MEM_SZ_UNKNOWN && sharedFree != NANOHUB_MEM_SZ_UNKNOWN)
+ mi.sharedUse = mi.sharedSz - sharedFree;
}
-static void readNanohubMemInfo(MessageBuf &buf, NanohubMemInfo &mi)
+NanohubRsp::NanohubRsp(MessageBuf &buf, uint32_t transactionId, bool chre)
{
- size_t pos = buf.getPos();
- mi.flashSz = buf.readU32();
- mi.blSz = buf.readU32();
- mi.osSz = buf.readU32();
- mi.sharedSz = buf.readU32();
- mi.eeSz = buf.readU32();
- mi.ramSz = buf.readU32();
-
- mi.blUse = buf.readU32();
- mi.osUse = buf.readU32();
- mi.sharedUse = buf.readU32();
- mi.eeUse = buf.readU32();
- mi.ramUse = buf.readU32();
- if ((buf.getPos() - pos) != sizeof(mi)) {
- ALOGE("%s: failed to read object", __func__);
- }
-}
-
-NanohubRsp::NanohubRsp(MessageBuf &buf, bool no_status)
-{
- // all responses start with command
- // most of them have 4-byte status (result code)
+ // all responses start with command and have a 4-byte status (result code)
buf.reset();
- cmd = buf.readU8();
- if (!buf.getSize()) {
- status = -EINVAL;
- } else if (no_status) {
- status = 0;
+ if (buf.getSize() < 5) {
+ mStatus = -EINVAL;
} else {
- if (cmd == NANOHUB_START_UPLOAD || cmd == NANOHUB_CONT_UPLOAD || cmd == NANOHUB_FINISH_UPLOAD)
- status = buf.readU8();
+ mCmd = buf.readU8();
+ mStatus = buf.readU32();
+ if (chre)
+ mTransactionId = transactionId;
else
- status = buf.readU32();
+ mTransactionId = 0;
}
}
-int SystemComm::sendToSystem(const void *data, size_t len)
+int SystemComm::sendToSystem(const void *data, size_t len, uint32_t transactionId)
{
if (NanoHub::messageTracingEnabled()) {
- dumpBuffer("HAL -> SYS", getSystem()->mHostIfAppName, 0, data, len);
+ dumpBuffer("HAL -> SYS", getSystem()->mHostIfAppName, transactionId, 0, data, len);
}
- return NanoHub::sendToDevice(&getSystem()->mHostIfAppName, data, len);
-}
-
-int SystemComm::AppInfoSession::setup(const hub_message_t *)
-{
- std::lock_guard<std::mutex> _l(mLock);
- int suggestedSize = mAppInfo.size() ? mAppInfo.size() : 20;
-
- mAppInfo.clear();
- mAppInfo.reserve(suggestedSize);
- setState(SESSION_USER);
-
- return requestNext();
+ return NanoHub::sendToDevice(&getSystem()->mHostIfAppName, data, len, transactionId);
}
inline hub_app_name_t deviceAppNameToHost(const hub_app_name_t src)
@@ -129,143 +201,112 @@
return res;
}
-int SystemComm::AppInfoSession::handleRx(MessageBuf &buf)
+const uint8_t app_info_tags[] =
{
- std::lock_guard<std::mutex> _l(mLock);
+ NANOHUB_HAL_APP_INFO_APPID,
+ NANOHUB_HAL_APP_INFO_CRC,
+ NANOHUB_HAL_APP_INFO_TID,
+ NANOHUB_HAL_APP_INFO_VERSION,
+ NANOHUB_HAL_APP_INFO_ADDR,
+ NANOHUB_HAL_APP_INFO_SIZE,
+ NANOHUB_HAL_APP_INFO_HEAP,
+ NANOHUB_HAL_APP_INFO_DATA,
+ NANOHUB_HAL_APP_INFO_BSS,
+ NANOHUB_HAL_APP_INFO_CHRE_MAJOR,
+ NANOHUB_HAL_APP_INFO_CHRE_MINOR,
+ NANOHUB_HAL_APP_INFO_END,
+};
- NanohubRsp rsp(buf, true);
- if (rsp.cmd != NANOHUB_QUERY_APPS) {
- return 1;
- }
- size_t len = buf.getRoom();
- if (len != sizeof(NanohubAppInfo) && len) {
- ALOGE("%s: Invalid data size; have %zu, need %zu", __func__,
- len, sizeof(NanohubAppInfo));
- return -EINVAL;
- }
- if (getState() != SESSION_USER) {
- ALOGE("%s: Invalid state; have %d, need %d", __func__, getState(), SESSION_USER);
- return -EINVAL;
- }
- if (len) {
- NanohubAppInfo info;
- readNanohubAppInfo(buf, info);
- hub_app_info appInfo;
- appInfo.num_mem_ranges = 0;
- if (info.flashUse != NANOHUB_MEM_SZ_UNKNOWN) {
- mem_range_t &range = appInfo.mem_usage[appInfo.num_mem_ranges++];
- range.type = HUB_MEM_TYPE_MAIN;
- range.total_bytes = info.flashUse;
- }
- if (info.ramUse != NANOHUB_MEM_SZ_UNKNOWN) {
- mem_range_t &range = appInfo.mem_usage[appInfo.num_mem_ranges++];
- range.type = HUB_MEM_TYPE_RAM;
- range.total_bytes = info.ramUse;
- }
-
- appInfo.app_name = info.name;
- appInfo.version = info.version;
-
- mAppInfo.push_back(appInfo);
- return requestNext();
- } else {
- sendToApp(CONTEXT_HUB_QUERY_APPS,
- static_cast<const void *>(mAppInfo.data()),
- mAppInfo.size() * sizeof(mAppInfo[0]));
- complete();
- }
-
- return 0;
-}
-
-int SystemComm::AppInfoSession::requestNext()
+const uint8_t sys_info_tags[] =
{
- char data[MAX_RX_PACKET];
- MessageBuf buf(data, sizeof(data));
- buf.writeU8(NANOHUB_QUERY_APPS);
- buf.writeU32(mAppInfo.size());
- return sendToSystem(buf.getData(), buf.getPos());
-}
+ NANOHUB_HAL_SYS_INFO_HEAP_FREE,
+ NANOHUB_HAL_SYS_INFO_RAM_SIZE,
+ NANOHUB_HAL_SYS_INFO_EEDATA_SIZE,
+ NANOHUB_HAL_SYS_INFO_EEDATA_FREE,
+ NANOHUB_HAL_SYS_INFO_CODE_SIZE,
+ NANOHUB_HAL_SYS_INFO_CODE_FREE,
+ NANOHUB_HAL_SYS_INFO_SHARED_SIZE,
+ NANOHUB_HAL_SYS_INFO_SHARED_FREE,
+ NANOHUB_HAL_SYS_INFO_END,
+};
-int SystemComm::MemInfoSession::setup(const hub_message_t *)
+int SystemComm::MemInfoSession::setup(const hub_message_t *, uint32_t transactionId, AppManager &)
{
std::lock_guard<std::mutex> _l(mLock);
char data[MAX_RX_PACKET];
MessageBuf buf(data, sizeof(data));
- buf.writeU8(NANOHUB_QUERY_MEMINFO);
+ buf.writeU8(NANOHUB_HAL_SYS_INFO);
+ buf.writeRaw(sys_info_tags, sizeof(sys_info_tags));
setState(SESSION_USER);
- return sendToSystem(buf.getData(), buf.getPos());
+ return sendToSystem(buf.getData(), buf.getPos(), transactionId);
}
-int SystemComm::MemInfoSession::handleRx(MessageBuf &buf)
+int SystemComm::MemInfoSession::handleRx(MessageBuf &buf, uint32_t transactionId, AppManager &, bool chre)
{
std::lock_guard<std::mutex> _l(mLock);
- NanohubRsp rsp(buf, true);
+ NanohubRsp rsp(buf, transactionId, chre);
- if (rsp.cmd != NANOHUB_QUERY_MEMINFO)
+ if (rsp.mCmd != NANOHUB_HAL_SYS_INFO)
return 1;
size_t len = buf.getRoom();
- if (len != sizeof(NanohubMemInfo)) {
- ALOGE("%s: Invalid data size: %zu", __func__, len);
- return -EINVAL;
- }
if (getState() != SESSION_USER) {
ALOGE("%s: Invalid state; have %d, need %d", __func__, getState(), SESSION_USER);
return -EINVAL;
}
- NanohubMemInfo mi;
- readNanohubMemInfo(buf, mi);
std::vector<mem_range_t> ranges;
ranges.reserve(4);
+ if (len) {
+ NanohubMemInfo mi;
+ readNanohubMemInfo(buf, mi);
- //if each is valid, copy to output area
- if (mi.sharedSz != NANOHUB_MEM_SZ_UNKNOWN &&
- mi.sharedUse != NANOHUB_MEM_SZ_UNKNOWN)
- ranges.push_back({
- .type = HUB_MEM_TYPE_MAIN,
- .total_bytes = mi.sharedSz,
- .free_bytes = mi.sharedSz - mi.sharedUse,
- });
+ //if each is valid, copy to output area
+ if (mi.sharedSz != NANOHUB_MEM_SZ_UNKNOWN &&
+ mi.sharedUse != NANOHUB_MEM_SZ_UNKNOWN)
+ ranges.push_back({
+ .type = HUB_MEM_TYPE_MAIN,
+ .total_bytes = mi.sharedSz,
+ .free_bytes = mi.sharedSz - mi.sharedUse,
+ });
- if (mi.osSz != NANOHUB_MEM_SZ_UNKNOWN &&
- mi.osUse != NANOHUB_MEM_SZ_UNKNOWN)
- ranges.push_back({
- .type = HUB_MEM_TYPE_OS,
- .total_bytes = mi.osSz,
- .free_bytes = mi.osSz - mi.osUse,
- });
+ if (mi.osSz != NANOHUB_MEM_SZ_UNKNOWN &&
+ mi.osUse != NANOHUB_MEM_SZ_UNKNOWN)
+ ranges.push_back({
+ .type = HUB_MEM_TYPE_OS,
+ .total_bytes = mi.osSz,
+ .free_bytes = mi.osSz - mi.osUse,
+ });
- if (mi.eeSz != NANOHUB_MEM_SZ_UNKNOWN &&
- mi.eeUse != NANOHUB_MEM_SZ_UNKNOWN)
- ranges.push_back({
- .type = HUB_MEM_TYPE_EEDATA,
- .total_bytes = mi.eeSz,
- .free_bytes = mi.eeSz - mi.eeUse,
- });
+ if (mi.eeSz != NANOHUB_MEM_SZ_UNKNOWN &&
+ mi.eeUse != NANOHUB_MEM_SZ_UNKNOWN)
+ ranges.push_back({
+ .type = HUB_MEM_TYPE_EEDATA,
+ .total_bytes = mi.eeSz,
+ .free_bytes = mi.eeSz - mi.eeUse,
+ });
- if (mi.ramSz != NANOHUB_MEM_SZ_UNKNOWN &&
- mi.ramUse != NANOHUB_MEM_SZ_UNKNOWN)
- ranges.push_back({
- .type = HUB_MEM_TYPE_RAM,
- .total_bytes = mi.ramSz,
- .free_bytes = mi.ramSz - mi.ramUse,
- });
+ if (mi.ramSz != NANOHUB_MEM_SZ_UNKNOWN &&
+ mi.ramUse != NANOHUB_MEM_SZ_UNKNOWN)
+ ranges.push_back({
+ .type = HUB_MEM_TYPE_RAM,
+ .total_bytes = mi.ramSz,
+ .free_bytes = mi.ramSz - mi.ramUse,
+ });
+ }
//send it out
- sendToApp(CONTEXT_HUB_QUERY_MEMORY,
+ sendToApp(CONTEXT_HUB_QUERY_MEMORY, transactionId,
static_cast<const void *>(ranges.data()),
ranges.size() * sizeof(ranges[0]));
complete();
-
return 0;
}
-int SystemComm::AppMgmtSession::setup(const hub_message_t *appMsg)
+int SystemComm::AppMgmtSession::setup(const hub_message_t *appMsg, uint32_t transactionId, AppManager &appManager)
{
std::lock_guard<std::mutex> _l(mLock);
@@ -281,203 +322,468 @@
switch (mCmd) {
case CONTEXT_HUB_APPS_ENABLE:
- return setupMgmt(appMsg, NANOHUB_EXT_APPS_ON);
+ return setupMgmt(appMsg, transactionId, NANOHUB_HAL_APP_MGMT_START, appManager);
case CONTEXT_HUB_APPS_DISABLE:
- return setupMgmt(appMsg, NANOHUB_EXT_APPS_OFF);
+ return setupMgmt(appMsg, transactionId, NANOHUB_HAL_APP_MGMT_STOP, appManager);
case CONTEXT_HUB_UNLOAD_APP:
- return setupMgmt(appMsg, NANOHUB_EXT_APP_DELETE);
+ return setupMgmt(appMsg, transactionId, NANOHUB_HAL_APP_MGMT_UNLOAD, appManager);
case CONTEXT_HUB_LOAD_APP:
{
- mData.clear();
- mData = std::vector<uint8_t>(msgData, msgData + mLen);
const load_app_request_t *appReq = static_cast<const load_app_request_t*>(appMsg->message);
if (appReq == nullptr || mLen <= sizeof(*appReq)) {
ALOGE("%s: Invalid app header: too short\n", __func__);
return -EINVAL;
}
mAppName = appReq->app_binary.app_id;
- setState(TRANSFER);
+ if (!appManager.isAppLoaded(mAppName)) {
+ appManager.addNewApp(mAppName, appReq->app_binary.app_version);
+ appManager.writeApp(mAppName, msgData, mLen);
+ mData.clear();
+ mData = std::vector<uint8_t>(msgData, msgData + mLen);
+ setState(TRANSFER);
- buf.writeU8(NANOHUB_START_UPLOAD);
- buf.writeU8(0);
- buf.writeU32(mLen);
- return sendToSystem(buf.getData(), buf.getPos());
+ buf.writeU8(NANOHUB_HAL_START_UPLOAD);
+ buf.writeU8(0);
+ buf.writeU32(mLen);
+
+ return sendToSystem(buf.getData(), buf.getPos(), transactionId);
+ } else {
+ if (appManager.cmpApp(mAppName, msgData, mLen)) {
+ mFlashAddr = appManager.getFlashAddr(mAppName);
+ if (appManager.isAppRunning(mAppName)) {
+ setState(STOP_RUN);
+
+ buf.writeU8(NANOHUB_HAL_APP_MGMT);
+ writeAppName(buf, mAppName);
+ buf.writeU8(NANOHUB_HAL_APP_MGMT_STOP);
+
+ return sendToSystem(buf.getData(), buf.getPos(), transactionId);
+ } else {
+ setState(RUN);
+
+ buf.writeU8(NANOHUB_HAL_APP_MGMT);
+ writeAppName(buf, mAppName);
+ buf.writeU8(NANOHUB_HAL_APP_MGMT_START);
+
+ return sendToSystem(buf.getData(), buf.getPos(), transactionId);
+ }
+ } else {
+ appManager.setCachedVersion(mAppName, appReq->app_binary.app_version);
+ appManager.writeApp(mAppName, msgData, mLen);
+ mData.clear();
+ mData = std::vector<uint8_t>(msgData, msgData + mLen);
+ if (appManager.isAppRunning(mAppName)) {
+ setState(STOP_TRANSFER);
+
+ buf.writeU8(NANOHUB_HAL_APP_MGMT);
+ writeAppName(buf, mAppName);
+ buf.writeU8(NANOHUB_HAL_APP_MGMT_STOP);
+
+ return sendToSystem(buf.getData(), buf.getPos(), transactionId);
+ } else {
+ setState(TRANSFER);
+
+ buf.writeU8(NANOHUB_HAL_START_UPLOAD);
+ buf.writeU8(0);
+ buf.writeU32(mLen);
+
+ return sendToSystem(buf.getData(), buf.getPos(), transactionId);
+ }
+ }
+ }
}
-
case CONTEXT_HUB_OS_REBOOT:
setState(REBOOT);
- buf.writeU8(NANOHUB_REBOOT);
- return sendToSystem(buf.getData(), buf.getPos());
+
+ buf.writeU8(NANOHUB_HAL_SYS_MGMT);
+ buf.writeU8(NANOHUB_HAL_SYS_MGMT_REBOOT);
+
+ return sendToSystem(buf.getData(), buf.getPos(), transactionId);
+
+ case CONTEXT_HUB_START_APPS:
+ if (mLen == sizeof(mStatus))
+ memcpy(&mStatus, msgData, mLen);
+ appManager.eraseApps();
+ setState(QUERY_START);
+
+ buf.writeU8(NANOHUB_HAL_APP_INFO);
+ buf.writeU32(0);
+ buf.writeRaw(app_info_tags, sizeof(app_info_tags));
+
+ return sendToSystem(buf.getData(), buf.getPos(), transactionId);
}
return -EINVAL;
}
-int SystemComm::AppMgmtSession::setupMgmt(const hub_message_t *appMsg, uint32_t cmd)
+int SystemComm::AppMgmtSession::setupMgmt(const hub_message_t *appMsg, uint32_t transactionId, uint32_t cmd, AppManager &appManager)
{
+ int32_t result = 0;
const hub_app_name_t &appName = *static_cast<const hub_app_name_t*>(appMsg->message);
if (appMsg->message_len != sizeof(appName)) {
return -EINVAL;
}
+ mAppName = appName;
+ switch (cmd) {
+ case NANOHUB_HAL_APP_MGMT_START:
+ if (appManager.isAppRunning(mAppName)) {
+ appManager.setCachedStart(mAppName, true);
+ sendToApp(mCmd, transactionId, &result, sizeof(result));
+ complete();
+ return 0;
+ }
+ break;
+ case NANOHUB_HAL_APP_MGMT_STOP:
+ case NANOHUB_HAL_APP_MGMT_UNLOAD:
+ appManager.setCachedStart(mAppName, false);
+ if (!appManager.isAppRunning(mAppName)) {
+ sendToApp(mCmd, transactionId, &result, sizeof(result));
+ complete();
+ return 0;
+ }
+ break;
+ }
char data[MAX_RX_PACKET];
MessageBuf buf(data, sizeof(data));
- buf.writeU8(cmd);
+ buf.writeU8(NANOHUB_HAL_APP_MGMT);
writeAppName(buf, appName);
+ buf.writeU8(cmd);
setState(MGMT);
- return sendToSystem(buf.getData(), buf.getPos());
+ return sendToSystem(buf.getData(), buf.getPos(), transactionId);
}
-int SystemComm::AppMgmtSession::handleRx(MessageBuf &buf)
+int SystemComm::AppMgmtSession::handleRx(MessageBuf &buf, uint32_t transactionId, AppManager &appManager, bool chre)
{
int ret = 0;
std::lock_guard<std::mutex> _l(mLock);
- NanohubRsp rsp(buf);
+ NanohubRsp rsp(buf, transactionId, chre);
switch (getState()) {
case TRANSFER:
- ret = handleTransfer(rsp);
+ ret = handleTransfer(rsp, buf, appManager);
+ break;
+ case STOP_TRANSFER:
+ ret = handleStopTransfer(rsp, buf, appManager);
+ break;
+ case QUERY_START:
+ ret = handleQueryStart(rsp, buf, appManager);
+ break;
+ case START:
+ ret = handleStart(rsp, buf, appManager);
break;
case FINISH:
- ret = handleFinish(rsp);
+ ret = handleFinish(rsp, buf, appManager);
break;
case RUN:
- ret = handleRun(rsp);
+ ret = handleRun(rsp, buf, appManager);
break;
- case RUN_FAILED:
- ret = handleRunFailed(rsp);
+ case STOP_RUN:
+ ret = handleStopRun(rsp, buf, appManager);
break;
case REBOOT:
- ret = handleReboot(rsp);
+ ret = handleReboot(rsp, buf, appManager);
+ break;
+ case ERASE_TRANSFER:
+ ret = handleEraseTransfer(rsp, buf, appManager);
break;
case MGMT:
- ret = handleMgmt(rsp);
+ ret = handleMgmt(rsp, buf, appManager);
+ break;
+ case INFO:
+ ret = handleInfo(rsp, buf, appManager);
break;
}
return ret;
}
-int SystemComm::AppMgmtSession::handleTransfer(NanohubRsp &rsp)
+int SystemComm::AppMgmtSession::handleTransfer(NanohubRsp &rsp, MessageBuf &, AppManager &appManager)
{
- if (rsp.cmd != NANOHUB_CONT_UPLOAD && rsp.cmd != NANOHUB_START_UPLOAD)
+ if (rsp.mCmd != NANOHUB_HAL_CONT_UPLOAD && rsp.mCmd != NANOHUB_HAL_START_UPLOAD)
return 1;
char data[MAX_RX_PACKET];
MessageBuf buf(data, sizeof(data));
- const bool success = rsp.status != 0;
+ int32_t result = 0;
static_assert(NANOHUB_UPLOAD_CHUNK_SZ_MAX <= (MAX_RX_PACKET-5),
"Invalid chunk size");
- if (success) {
+ if (rsp.mStatus == NANOHUB_HAL_UPLOAD_ACCEPTED) {
mPos = mNextPos;
mErrCnt = 0;
+ } else if (rsp.mStatus == NANOHUB_HAL_UPLOAD_RESEND) {
+ mErrCnt ++;
+ } else if (rsp.mStatus == NANOHUB_HAL_UPLOAD_RESTART) {
+ mPos = 0;
+ mErrCnt ++;
+ } else if (rsp.mStatus == NANOHUB_HAL_UPLOAD_CANCEL ||
+ rsp.mStatus == NANOHUB_HAL_UPLOAD_CANCEL_NO_RETRY) {
+ mPos = mLen;
+ result = NANOHUB_APP_NOT_LOADED;
+ } else if (rsp.mStatus == NANOHUB_HAL_UPLOAD_NO_SPACE) {
+ mPos = 0;
+ mErrCnt = 0;
+ setState(ERASE_TRANSFER);
+
+ buf.writeU8(NANOHUB_HAL_SYS_MGMT);
+ buf.writeU8(NANOHUB_HAL_SYS_MGMT_ERASE);
+
+ return sendToSystem(buf.getData(), buf.getPos(), rsp.mTransactionId);
} else if (mErrCnt > 5) {
mPos = mLen;
+ result = NANOHUB_APP_NOT_LOADED;
} else {
mErrCnt ++;
}
- if (mPos < mLen) {
+ if (result != 0) {
+ appManager.clearCachedApp(mAppName);
+
+ sendToApp(mCmd, rsp.mTransactionId, &result, sizeof(result));
+ complete();
+ return 0;
+ } else if (mPos < mLen) {
uint32_t chunkSize = mLen - mPos;
if (chunkSize > NANOHUB_UPLOAD_CHUNK_SZ_MAX) {
chunkSize = NANOHUB_UPLOAD_CHUNK_SZ_MAX;
}
- buf.writeU8(NANOHUB_CONT_UPLOAD);
+ buf.writeU8(NANOHUB_HAL_CONT_UPLOAD);
buf.writeU32(mPos);
buf.writeRaw(&mData[mPos], chunkSize);
mNextPos = mPos + chunkSize;
} else {
- buf.writeU8(NANOHUB_FINISH_UPLOAD);
+ buf.writeU8(NANOHUB_HAL_FINISH_UPLOAD);
setState(FINISH);
}
- return sendToSystem(buf.getData(), buf.getPos());
+ return sendToSystem(buf.getData(), buf.getPos(), rsp.mTransactionId);
}
-int SystemComm::AppMgmtSession::handleFinish(NanohubRsp &rsp)
+int SystemComm::AppMgmtSession::handleStopTransfer(NanohubRsp &rsp, MessageBuf &buf, AppManager &)
{
- if (rsp.cmd != NANOHUB_FINISH_UPLOAD)
+ if (rsp.mCmd != NANOHUB_HAL_APP_MGMT)
return 1;
- int ret = 0;
- const bool success = rsp.status != 0;
- mData.clear();
+ uint8_t cmd = buf.readU8();
- if (success) {
+ if (cmd != NANOHUB_HAL_APP_MGMT_STOP)
+ return 1;
+
+ MgmtStatus sts = { .value = buf.readU32() };
+
+ ALOGI("Nanohub NEW APP STOP: %08" PRIX32 "\n", sts.value);
+ if (rsp.mStatus == 0) {
char data[MAX_RX_PACKET];
MessageBuf buf(data, sizeof(data));
- buf.writeU8(NANOHUB_EXT_APPS_ON);
- writeAppName(buf, mAppName);
- setState(RUN);
- ret = sendToSystem(buf.getData(), buf.getPos());
+ setState(TRANSFER);
+
+ buf.writeU8(NANOHUB_HAL_START_UPLOAD);
+ buf.writeU8(0);
+ buf.writeU32(mLen);
+ return sendToSystem(buf.getData(), buf.getPos(), rsp.mTransactionId);
} else {
int32_t result = NANOHUB_APP_NOT_LOADED;
- sendToApp(mCmd, &result, sizeof(result));
+ sendToApp(mCmd, rsp.mTransactionId, &result, sizeof(result));
complete();
+ return 0;
}
-
- return ret;
}
-int SystemComm::AppMgmtSession::handleRun(NanohubRsp &rsp)
+int SystemComm::AppMgmtSession::handleQueryStart(NanohubRsp &rsp, MessageBuf &buf, AppManager &appManager)
{
- if (rsp.cmd != NANOHUB_EXT_APPS_ON)
+
+ if (rsp.mCmd != NANOHUB_HAL_APP_INFO)
return 1;
- MgmtStatus sts = { .value = (uint32_t)rsp.status };
+ size_t len = buf.getRoom();
+ if (len) {
+ uint32_t nextAddr = appManager.readNanohubAppInfo(buf);
- // op counter returns number of nanoapps that were started as result of the command
- // for successful start command it must be > 0
- int32_t result = sts.value > 0 && sts.op > 0 && sts.op <= 0x7F ? 0 : -1;
+ if (nextAddr) {
+ char data[MAX_RX_PACKET];
+ MessageBuf buf(data, sizeof(data));
- ALOGI("Nanohub NEW APP START: %08" PRIX32 "\n", rsp.status);
- if (result != 0) {
- // if nanoapp failed to start we have to unload it
+ buf.writeU8(NANOHUB_HAL_APP_INFO);
+ buf.writeU32(nextAddr);
+ buf.writeRaw(app_info_tags, sizeof(app_info_tags));
+
+ return sendToSystem(buf.getData(), buf.getPos(), rsp.mTransactionId);
+ }
+ }
+
+ appManager.getAppsToStart(mAppList);
+ if (mAppList.empty()) {
+ sendToApp(CONTEXT_HUB_OS_REBOOT, 0, &mStatus, sizeof(mStatus));
+ complete();
+ return 0;
+ } else {
char data[MAX_RX_PACKET];
MessageBuf buf(data, sizeof(data));
- buf.writeU8(NANOHUB_EXT_APP_DELETE);
- writeAppName(buf, mAppName);
- if (sendToSystem(buf.getData(), buf.getPos()) == 0) {
- setState(RUN_FAILED);
- return 0;
- }
- ALOGE("%s: failed to send DELETE for failed app\n", __func__);
- }
+ mAppName = mAppList.back();
+ mAppList.pop_back();
+ setState(START);
- // it is either success, and we report it, or
- // it is a failure to load, and also failure to send erase command
- sendToApp(mCmd, &result, sizeof(result));
+ buf.writeU8(NANOHUB_HAL_APP_MGMT);
+ writeAppName(buf, mAppName);
+ buf.writeU8(NANOHUB_HAL_APP_MGMT_START);
+ return sendToSystem(buf.getData(), buf.getPos(), rsp.mTransactionId);
+ }
+}
+
+int SystemComm::AppMgmtSession::handleStart(NanohubRsp &rsp, MessageBuf &buf, AppManager &)
+{
+ if (rsp.mCmd != NANOHUB_HAL_APP_MGMT)
+ return 1;
+
+ uint8_t cmd = buf.readU8();
+
+ if (cmd != NANOHUB_HAL_APP_MGMT_START)
+ return 1;
+
+ MgmtStatus sts = { .value = buf.readU32() };
+
+ ALOGI("Nanohub EXISTING APP START: %08" PRIX32 "\n", sts.value);
+ if (mAppList.empty()) {
+ sendToApp(CONTEXT_HUB_OS_REBOOT, 0, &mStatus, sizeof(mStatus));
+ complete();
+ return 0;
+ } else {
+ char data[MAX_RX_PACKET];
+ MessageBuf buf(data, sizeof(data));
+ mAppName = mAppList.back();
+ mAppList.pop_back();
+
+ buf.writeU8(NANOHUB_HAL_APP_MGMT);
+ writeAppName(buf, mAppName);
+ buf.writeU8(NANOHUB_HAL_APP_MGMT_START);
+ return sendToSystem(buf.getData(), buf.getPos(), rsp.mTransactionId);
+ }
+}
+
+int SystemComm::AppMgmtSession::handleFinish(NanohubRsp &rsp, MessageBuf &buf, AppManager &appManager)
+{
+ if (rsp.mCmd != NANOHUB_HAL_FINISH_UPLOAD)
+ return 1;
+
+ mFlashAddr = buf.readU32();
+ uint32_t crc = buf.readU32();
+ mData.clear();
+
+ if (rsp.mStatus == 0) {
+ appManager.setCachedCrc(mAppName, crc);
+ char data[MAX_RX_PACKET];
+ MessageBuf buf(data, sizeof(data));
+ setState(RUN);
+
+ buf.writeU8(NANOHUB_HAL_APP_MGMT);
+ writeAppName(buf, mAppName);
+ buf.writeU8(NANOHUB_HAL_APP_MGMT_START);
+ return sendToSystem(buf.getData(), buf.getPos(), rsp.mTransactionId);
+ } else {
+ int32_t result = NANOHUB_APP_NOT_LOADED;
+ appManager.clearCachedApp(mAppName);
+
+ sendToApp(mCmd, rsp.mTransactionId, &result, sizeof(result));
+ complete();
+ return 0;
+ }
+}
+
+int SystemComm::AppMgmtSession::handleRun(NanohubRsp &rsp, MessageBuf &buf, AppManager &appManager)
+{
+ if (rsp.mCmd != NANOHUB_HAL_APP_MGMT)
+ return 1;
+
+ uint8_t cmd = buf.readU8();
+
+ if (cmd != NANOHUB_HAL_APP_MGMT_START)
+ return 1;
+
+ MgmtStatus sts = { .value = buf.readU32() };
+
+ ALOGI("Nanohub NEW APP START: %08" PRIX32 "\n", sts.value);
+ if (rsp.mStatus == 0) {
+ appManager.setCachedStart(mAppName, true);
+ char data[MAX_RX_PACKET];
+ MessageBuf buf(data, sizeof(data));
+ setState(INFO);
+
+ buf.writeU8(NANOHUB_HAL_APP_INFO);
+ buf.writeU32(mFlashAddr);
+ buf.writeRaw(app_info_tags, sizeof(app_info_tags));
+
+ return sendToSystem(buf.getData(), buf.getPos(), rsp.mTransactionId);
+ } else {
+ appManager.setCachedStart(mAppName, false);
+ int32_t result = NANOHUB_APP_NOT_LOADED;
+ sendToApp(mCmd, rsp.mTransactionId, &result, sizeof(result));
+ complete();
+ return 0;
+ }
+}
+
+int SystemComm::AppMgmtSession::handleInfo(NanohubRsp &rsp, MessageBuf &buf, AppManager &appManager)
+{
+ if (rsp.mCmd != NANOHUB_HAL_APP_INFO)
+ return 1;
+ int32_t result = 0;
+ size_t len = buf.getRoom();
+ if (len) {
+ appManager.readNanohubAppInfo(buf);
+ appManager.saveApps();
+ }
+ sendToApp(mCmd, rsp.mTransactionId, &result, sizeof(result));
complete();
return 0;
}
-int SystemComm::AppMgmtSession::handleRunFailed(NanohubRsp &rsp)
+int SystemComm::AppMgmtSession::handleStopRun(NanohubRsp &rsp, MessageBuf &buf, AppManager &)
{
- if (rsp.cmd != NANOHUB_EXT_APP_DELETE)
+ if (rsp.mCmd != NANOHUB_HAL_APP_MGMT)
return 1;
- int32_t result = -1;
+ uint8_t cmd = buf.readU8();
- ALOGI("%s: APP DELETE [because it failed]: %08" PRIX32 "\n", __func__, rsp.status);
+ if (cmd != NANOHUB_HAL_APP_MGMT_STOP)
+ return 1;
- sendToApp(mCmd, &result, sizeof(result));
- complete();
+ MgmtStatus sts = { .value = buf.readU32() };
- return 0;
+ ALOGI("Nanohub NEW APP STOP: %08" PRIX32 "\n", sts.value);
+ if (rsp.mStatus == 0) {
+ char data[MAX_RX_PACKET];
+ MessageBuf buf(data, sizeof(data));
+ setState(RUN);
+
+ buf.writeU8(NANOHUB_HAL_APP_MGMT);
+ writeAppName(buf, mAppName);
+ buf.writeU8(NANOHUB_HAL_APP_MGMT_START);
+ return sendToSystem(buf.getData(), buf.getPos(), rsp.mTransactionId);
+ } else {
+ int32_t result = NANOHUB_APP_NOT_LOADED;
+
+ sendToApp(mCmd, rsp.mTransactionId, &result, sizeof(result));
+ complete();
+ return 0;
+ }
}
/* reboot notification, when triggered by App request */
-int SystemComm::AppMgmtSession::handleReboot(NanohubRsp &rsp)
+int SystemComm::AppMgmtSession::handleReboot(NanohubRsp &rsp, MessageBuf &buf, AppManager &)
{
- if (rsp.cmd != NANOHUB_REBOOT)
+ if (rsp.mCmd != NANOHUB_HAL_SYS_MGMT)
return 1;
- ALOGI("Nanohub reboot status [USER REQ]: %08" PRIX32 "\n", rsp.status);
+
+ uint8_t cmd = buf.readU8();
+
+ if (cmd == NANOHUB_HAL_SYS_MGMT_REBOOT)
+ ALOGI("Nanohub reboot status [USER REQ]: %08" PRIX32 "\n", rsp.mStatus);
// reboot notification is sent by SessionManager
complete();
@@ -485,59 +791,102 @@
return 0;
}
-int SystemComm::AppMgmtSession::handleMgmt(NanohubRsp &rsp)
+int SystemComm::AppMgmtSession::handleEraseTransfer(NanohubRsp &rsp, MessageBuf &buf, AppManager &appManager)
{
+ if (rsp.mCmd != NANOHUB_HAL_SYS_MGMT)
+ return 1;
+
+ uint8_t cmd = buf.readU8();
+
+ if (cmd == NANOHUB_HAL_SYS_MGMT_ERASE && rsp.mStatus == 0) {
+ char data[MAX_RX_PACKET];
+ MessageBuf buf(data, sizeof(data));
+ appManager.eraseApps();
+ setState(TRANSFER);
+
+ buf.writeU8(NANOHUB_HAL_START_UPLOAD);
+ buf.writeU8(0);
+ buf.writeU32(mLen);
+ return sendToSystem(buf.getData(), buf.getPos(), rsp.mTransactionId);
+ } else {
+ int32_t result = NANOHUB_APP_NOT_LOADED;
+
+ sendToApp(mCmd, rsp.mTransactionId, &result, sizeof(result));
+ complete();
+ return 0;
+ }
+}
+
+int SystemComm::AppMgmtSession::handleMgmt(NanohubRsp &rsp, MessageBuf &buf, AppManager &appManager)
+{
+ if (rsp.mCmd != NANOHUB_HAL_APP_MGMT)
+ return 1;
+
+ uint8_t cmd = buf.readU8();
+ MgmtStatus sts = { .value = buf.readU32() };
+
bool valid = false;
- int32_t result = rsp.status;
+ int32_t result = rsp.mStatus;
- // TODO: remove this when context hub service can handle non-zero success status
- if (result > 0) {
- // something happened; assume it worked
- result = 0;
- } else if (result == 0) {
- // nothing happened; this is provably an error
+ if (result != 0)
result = -1;
- }
- switch (rsp.cmd) {
- case NANOHUB_EXT_APPS_OFF:
+ switch (cmd) {
+ case NANOHUB_HAL_APP_MGMT_STOP:
valid = mCmd == CONTEXT_HUB_APPS_DISABLE;
+ if (valid && rsp.mStatus == 0)
+ appManager.clearRunning(mAppName);
break;
- case NANOHUB_EXT_APPS_ON:
+ case NANOHUB_HAL_APP_MGMT_START:
valid = mCmd == CONTEXT_HUB_APPS_ENABLE;
+ if (valid && rsp.mStatus == 0) {
+ appManager.setCachedStart(mAppName, true);
+ char data[MAX_RX_PACKET];
+ MessageBuf buf(data, sizeof(data));
+ setState(INFO);
+
+ buf.writeU8(NANOHUB_HAL_APP_INFO);
+ buf.writeU32(appManager.getFlashAddr(mAppName));
+ buf.writeRaw(app_info_tags, sizeof(app_info_tags));
+
+ return sendToSystem(buf.getData(), buf.getPos(), rsp.mTransactionId);
+ }
break;
- case NANOHUB_EXT_APP_DELETE:
+ case NANOHUB_HAL_APP_MGMT_UNLOAD:
valid = mCmd == CONTEXT_HUB_UNLOAD_APP;
+ if (valid && rsp.mStatus == 0)
+ appManager.clearRunning(mAppName);
break;
default:
return 1;
}
- ALOGI("Nanohub MGMT response: CMD=%02X; STATUS=%08" PRIX32, rsp.cmd, rsp.status);
+ ALOGI("Nanohub MGMT response: CMD=%02X; STATUS=%08" PRIX32, rsp.mCmd, sts.value);
if (!valid) {
ALOGE("Invalid response for this state: APP CMD=%02X", mCmd);
return -EINVAL;
}
- sendToApp(mCmd, &result, sizeof(result));
+ sendToApp(mCmd, rsp.mTransactionId, &result, sizeof(result));
complete();
-
return 0;
}
-int SystemComm::KeyInfoSession::setup(const hub_message_t *) {
+int SystemComm::KeyInfoSession::setup(const hub_message_t *, uint32_t transactionId, AppManager &) {
std::lock_guard<std::mutex> _l(mLock);
+ mKeyNum = 0;
+ mKeyOffset = 0;
mRsaKeyData.clear();
setState(SESSION_USER);
mStatus = -EBUSY;
- return requestRsaKeys();
+ return requestRsaKeys(transactionId);
}
-int SystemComm::KeyInfoSession::handleRx(MessageBuf &buf)
+int SystemComm::KeyInfoSession::handleRx(MessageBuf &buf, uint32_t transactionId, AppManager &, bool chre)
{
std::lock_guard<std::mutex> _l(mLock);
- NanohubRsp rsp(buf, true);
+ NanohubRsp rsp(buf, transactionId, chre);
if (getState() != SESSION_USER) {
// invalid state
@@ -545,11 +894,20 @@
return mStatus;
}
- if (buf.getRoom()) {
+ uint32_t keyLen = buf.readU32();
+ uint32_t dataLen = buf.getRoom();
+
+ if (dataLen) {
mRsaKeyData.insert(mRsaKeyData.end(),
buf.getData() + buf.getPos(),
buf.getData() + buf.getSize());
- return requestRsaKeys();
+ if (mKeyOffset + dataLen >= keyLen) {
+ mKeyNum++;
+ mKeyOffset = 0;
+ } else {
+ mKeyOffset += dataLen;
+ }
+ return requestRsaKeys(transactionId);
} else {
mStatus = 0;
complete();
@@ -557,43 +915,500 @@
}
}
-int SystemComm::KeyInfoSession::requestRsaKeys(void)
+int SystemComm::KeyInfoSession::requestRsaKeys(uint32_t transactionId)
{
char data[MAX_RX_PACKET];
MessageBuf buf(data, sizeof(data));
- buf.writeU8(NANOHUB_QUERY_APPS);
- buf.writeU32(mRsaKeyData.size());
+ buf.writeU8(NANOHUB_HAL_KEY_INFO);
+ buf.writeU32(mKeyNum);
+ buf.writeU32(mKeyOffset);
- return sendToSystem(buf.getData(), buf.getPos());
+ return sendToSystem(buf.getData(), buf.getPos(), transactionId);
}
-int SystemComm::doHandleRx(const nano_message *msg)
+void SystemComm::AppManager::dumpAppInfo(std::string &result)
{
+ char buffer[256];
+
+ for (auto &it : apps_) {
+ uint64_t id = it.first;
+ const auto &app = it.second;
+
+ snprintf(buffer, sizeof(buffer), "App: 0x%016" PRIx64 "\n", id);
+ result.append(buffer);
+ if (app->loaded) {
+ snprintf(buffer, sizeof(buffer),
+ " Version: 0x%08" PRIx32 "\n"
+ " flashAddr: 0x%08" PRIx32 "\n"
+ " Running: %s\n",
+ app->version,
+ app->flashAddr,
+ app->running ? "true" : "false");
+ result.append(buffer);
+
+ if (app->flashUse != NANOHUB_MEM_SZ_UNKNOWN) {
+ snprintf(buffer, sizeof(buffer),
+ " flashUse: %d\n"
+ " CRC: 0x%08" PRIx32 "\n",
+ app->flashUse,
+ app->crc);
+ result.append(buffer);
+ }
+
+ if (app->running) {
+ snprintf(buffer, sizeof(buffer),
+ " TID: %04x\n"
+ " ramUse: %d\n",
+ app->tid,
+ app->ramUse);
+ result.append(buffer);
+ }
+
+ if (app->chre) {
+ snprintf(buffer, sizeof(buffer), " CHRE: %d.%d\n",
+ app->chre_major, app->chre_minor);
+ result.append(buffer);
+ }
+ }
+
+ if (app->cached_napp) {
+ snprintf(buffer, sizeof(buffer),
+ " Cached Version: 0x%08" PRIx32 "\n"
+ " Cached Start: %s\n"
+ " Cached CRC: 0x%08" PRIx32 "\n",
+ app->cached_version,
+ app->cached_start ? "true" : "false",
+ app->cached_crc);
+ result.append(buffer);
+ }
+ }
+}
+
+bool SystemComm::AppManager::saveApps()
+{
+ mkdir(CHRE_APP_DIR, CHRE_APP_DIR_PERMS);
+ File saved_apps_file(CHRE_APP_SETTINGS, "w");
+ std::shared_ptr<Json::Value> appsObject(new Json::Value);
+ status_t err;
+
+ if ((err = saved_apps_file.initCheck()) != OK) {
+ ALOGW("saved_apps file open (w) failed %d (%s)",
+ err,
+ strerror(-err));
+ return false;
+ }
+
+ for (auto &it : apps_) {
+ uint64_t id = it.first;
+ const auto &app = it.second;
+
+ if (app->cached_napp) {
+ char hexId[17];
+ snprintf(hexId, sizeof(hexId), "%016" PRIX64, id);
+ Json::Value array(Json::arrayValue);
+ array[0] = app->cached_version;
+ array[1] = app->cached_start;
+ array[2] = app->cached_crc;
+ (*appsObject)[hexId] = array;
+ }
+ }
+
+ // Write the JSON string to disk.
+ Json::StyledWriter writer;
+ std::string serializedSettings(writer.write(*appsObject));
+ size_t size = serializedSettings.size();
+ if ((err = saved_apps_file.write(serializedSettings.c_str(), size)) != (ssize_t)size) {
+ ALOGW("saved_apps file write failed %d (%s)",
+ err,
+ strerror(-err));
+ return false;
+ }
+
+ return true;
+}
+
+bool SystemComm::AppManager::restoreApps()
+{
+ File saved_apps_file(CHRE_APP_SETTINGS, "r");
+ std::shared_ptr<Json::Value> appsObject;
+ status_t err;
+
+ if ((err = saved_apps_file.initCheck()) != OK) {
+ ALOGW("saved_apps file open (r) failed %d (%s)",
+ err,
+ strerror(-err));
+ return false;
+ }
+
+ off64_t size = saved_apps_file.seekTo(0, SEEK_END);
+ saved_apps_file.seekTo(0, SEEK_SET);
+
+ if (size > 0) {
+ char *buf = (char *)malloc(size);
+ CHECK_EQ(saved_apps_file.read(buf, size), (ssize_t)size);
+
+ std::string str(buf);
+ std::shared_ptr<Json::Value> in(new Json::Value);
+ Json::Reader reader;
+ bool valid = reader.parse(str, *in);
+ free(buf);
+
+ if (valid && in->isObject()) {
+ appsObject = in;
+ }
+ }
+
+ if (appsObject == nullptr) {
+ appsObject = std::shared_ptr<Json::Value>(new Json::Value(Json::objectValue));
+ }
+
+ Json::Value::Members apps = appsObject->getMemberNames();
+ for (auto &it : apps) {
+ Json::Value &val = (*appsObject)[it];
+ if (val.isArray()) {
+ uint32_t version = val[0].asUInt();
+ uint32_t start = val[1].asUInt();
+ uint32_t crc = val[2].asUInt();
+
+ uint64_t id = strtoull(it.c_str(), nullptr, 16);
+ apps_[id] = std::unique_ptr<AppData>(new AppData);
+ apps_[id]->loaded = false;
+ apps_[id]->running = false;
+ apps_[id]->chre = false;
+ apps_[id]->cached_napp = true;
+ apps_[id]->cached_version = version;
+ apps_[id]->cached_start = start;
+ apps_[id]->cached_crc = crc;
+ }
+ }
+
+ return true;
+}
+
+bool SystemComm::AppManager::eraseApps()
+{
+ for (auto it=apps_.begin(); it != apps_.end();) {
+ if (!it->second->cached_napp)
+ it = apps_.erase(it);
+ else {
+ it->second->loaded = false;
+ it->second->running = false;
+ it->second->chre = false;
+ ++it;
+ }
+ }
+
+ return true;
+}
+
+bool SystemComm::AppManager::writeApp(hub_app_name_t &appName, const uint8_t *data, int32_t len)
+{
+ mkdir(CHRE_APP_DIR, CHRE_APP_DIR_PERMS);
+ char file[strlen(CHRE_APP_DIR) + strlen("/") + 16 + strlen(".napp") + 1];
+
+ snprintf(file, sizeof(file), "%s/%016" PRIX64 ".napp", CHRE_APP_DIR, appName.id);
+
+ int fd = open(file, O_CREAT | O_WRONLY | O_TRUNC, CHRE_APP_FILE_PERMS);
+ if (fd == -1)
+ return false;
+
+ if (write(fd, data, len) == len) {
+ close(fd);
+ return true;
+ } else {
+ close(fd);
+ return false;
+ }
+}
+
+int32_t SystemComm::AppManager::readApp(hub_app_name_t &appName, void **data)
+{
+ char file[strlen(CHRE_APP_DIR) + strlen("/") + 16 + strlen(".napp") + 1];
+
+ snprintf(file, sizeof(file), "%s/%016" PRIX64 ".napp", CHRE_APP_DIR, appName.id);
+
+ int32_t ret = -1;
+ *data = nullptr;
+ int fd = open(file, O_RDONLY);
+
+ if (fd >= 0) {
+ struct stat sb;
+ if (fstat(fd, &sb) == 0) {
+ *data = malloc(sb.st_size);
+ if (*data != nullptr && read(fd, *data, sb.st_size) == sb.st_size)
+ ret = sb.st_size;
+ else {
+ free(*data);
+ *data = nullptr;
+ }
+ }
+ close(fd);
+ }
+ return ret;
+}
+
+bool SystemComm::AppManager::cmpApp(hub_app_name_t &appName, const uint8_t *data, uint32_t len)
+{
+ char file[strlen(CHRE_APP_DIR) + strlen("/") + 16 + strlen(".napp") + 1];
+
+ snprintf(file, sizeof(file), "%s/%016" PRIX64 ".napp", CHRE_APP_DIR, appName.id);
+
+ if (!isAppLoaded(appName))
+ return false;
+
+ if ((!apps_[appName.id]->cached_napp) ||
+ (apps_[appName.id]->crc != apps_[appName.id]->cached_crc))
+ return false;
+
+ bool success = false;
+ int fd = open(file, O_RDONLY);
+
+ if (fd >= 0) {
+ struct stat sb;
+ if (fstat(fd, &sb) == 0 && sb.st_size == len) {
+ void *buf = malloc(len);
+ if (buf != nullptr && read(fd, buf, sb.st_size) == sb.st_size && memcmp(data, buf, len) == 0)
+ success = true;
+ free(buf);
+ }
+ close(fd);
+ }
+ return success;
+}
+
+uint32_t SystemComm::AppManager::readNanohubAppInfo(MessageBuf &buf)
+{
+ hub_app_name_t name;
+
+ uint8_t tag, len;
+ uint32_t ramUse = 0;
+ bool ramUseValid = true;
+
+ // first tag must be the appid
+ if (buf.getRoom() < 2 + sizeof(name.id)) {
+ ALOGE("%s: failed to read object", __func__);
+ return 0;
+ }
+
+ tag = buf.readU8();
+ len = buf.readU8();
+ if (tag != NANOHUB_HAL_APP_INFO_APPID || len != sizeof(name.id)) {
+ ALOGE("%s: invalid first tag: %d", __func__, tag);
+ return 0;
+ }
+
+ readAppName(buf, name);
+ if (!isAppPresent(name)) {
+ apps_[name.id] = std::unique_ptr<AppData>(new AppData);
+ apps_[name.id]->loaded = false;
+ apps_[name.id]->chre = false;
+ apps_[name.id]->running = false;
+ apps_[name.id]->cached_napp = false;
+ }
+ const auto &app = apps_[name.id];
+
+ while (buf.getRoom() >= 2) {
+ tag = buf.readU8();
+ len = buf.readU8();
+ if (buf.getRoom() >= len) {
+ switch(tag) {
+ case NANOHUB_HAL_APP_INFO_CRC:
+ if (len == 0)
+ app->crc = 0;
+ else if (len == sizeof(app->crc))
+ app->crc = buf.readU32();
+ else
+ buf.readRaw(len);
+ break;
+ case NANOHUB_HAL_APP_INFO_TID:
+ if (len == 0) {
+ app->tid = NANOHUB_TID_UNKNOWN;
+ ramUseValid = false;
+ app->loaded = true;
+ app->running = false;
+ } else if (len == sizeof(app->tid)) {
+ app->tid = buf.readU32();
+ app->loaded = true;
+ app->running = true;
+ } else
+ buf.readRaw(len);
+ break;
+ case NANOHUB_HAL_APP_INFO_VERSION:
+ if (len == sizeof(app->version))
+ app->version = buf.readU32();
+ else
+ buf.readRaw(len);
+ break;
+ case NANOHUB_HAL_APP_INFO_ADDR:
+ if (len == sizeof(app->flashAddr))
+ app->flashAddr = buf.readU32();
+ else
+ buf.readRaw(len);
+ break;
+ case NANOHUB_HAL_APP_INFO_SIZE:
+ if (len == 0)
+ app->flashUse = NANOHUB_MEM_SZ_UNKNOWN;
+ else if (len == sizeof(app->flashUse))
+ app->flashUse = buf.readU32();
+ else
+ buf.readRaw(len);
+ break;
+ case NANOHUB_HAL_APP_INFO_HEAP:
+ case NANOHUB_HAL_APP_INFO_DATA:
+ case NANOHUB_HAL_APP_INFO_BSS:
+ if (len == 0)
+ ramUseValid = false;
+ else if (len == sizeof(uint32_t))
+ ramUse += buf.readU32();
+ else
+ buf.readRaw(len);
+ break;
+ case NANOHUB_HAL_APP_INFO_CHRE_MAJOR:
+ if (len == 0)
+ app->chre = false;
+ else if (len == sizeof(app->chre_major)) {
+ app->chre = true;
+ app->chre_major = buf.readU8();
+ } else
+ buf.readRaw(len);
+ break;
+ case NANOHUB_HAL_APP_INFO_CHRE_MINOR:
+ if (len == 0)
+ app->chre = false;
+ else if (len == sizeof(app->chre_minor)) {
+ app->chre = true;
+ app->chre_minor = buf.readU8();
+ } else
+ buf.readRaw(len);
+ break;
+ case NANOHUB_HAL_APP_INFO_END:
+ if (len != 0 || buf.getRoom() != 0) {
+ ALOGE("%s: failed to read object", __func__);
+ return 0;
+ }
+ break;
+ default:
+ ALOGI("%s: unsupported tag: %d", __func__, tag);
+ buf.readRaw(len);
+ break;
+ }
+ } else {
+ ALOGE("%s: failed to read object", __func__);
+ return 0;
+ }
+ }
+
+ if (buf.getRoom() != 0) {
+ ALOGE("%s: failed to read object", __func__);
+ return 0;
+ }
+
+ if (ramUseValid)
+ app->ramUse = ramUse;
+ else
+ app->ramUse = NANOHUB_MEM_SZ_UNKNOWN;
+
+ return app->flashAddr +
+ (app->flashUse != NANOHUB_MEM_SZ_UNKNOWN ? ((app->flashUse+3)&~3) : 4);
+}
+
+void SystemComm::AppManager::sendAppInfoToApp(uint32_t transactionId) {
+ std::vector<hub_app_info> appInfo;
+ for (auto &it : apps_) {
+ uint64_t id = it.first;
+ const auto &app = it.second;
+
+ // TODO: Still have some non-chre apps that need to be reported
+ // if (!app->chre || !app->running || app->flashUse == NANOHUB_MEM_SZ_UNKNOWN)
+ if (!app->running || app->flashUse == NANOHUB_MEM_SZ_UNKNOWN)
+ continue;
+
+ hub_app_info info;
+ info.app_name = { .id = id };
+ info.version = app->version;
+ info.num_mem_ranges = 0;
+ if (app->flashUse != NANOHUB_MEM_SZ_UNKNOWN) {
+ mem_range_t &range = info.mem_usage[info.num_mem_ranges++];
+ range.type = HUB_MEM_TYPE_MAIN;
+ range.total_bytes = app->flashUse;
+ }
+ if (app->ramUse != NANOHUB_MEM_SZ_UNKNOWN) {
+ mem_range_t &range = info.mem_usage[info.num_mem_ranges++];
+ range.type = HUB_MEM_TYPE_RAM;
+ range.total_bytes = app->ramUse;
+ }
+
+ appInfo.push_back(info);
+ }
+ sendToApp(CONTEXT_HUB_QUERY_APPS, transactionId,
+ static_cast<const void *>(appInfo.data()),
+ appInfo.size() * sizeof(appInfo[0]));
+}
+
+int SystemComm::AppManager::getAppsToStart(std::vector<hub_app_name_t> &apps)
+{
+ int cnt = 0;
+ apps.clear();
+
+ for (auto &it : apps_) {
+ uint64_t id = it.first;
+ const auto &app = it.second;
+
+ if (app->cached_napp && app->cached_start && app->loaded &&
+ !app->running && app->flashUse != NANOHUB_MEM_SZ_UNKNOWN) {
+ apps.push_back({ .id = id });
+ cnt++;
+ }
+ }
+
+ return cnt;
+}
+
+int SystemComm::doHandleRx(uint64_t appId, uint32_t transactionId, const char *data, int len, bool chre)
+{
+ bool reboot = false;
+ uint32_t rebootStatus;
//we only care for messages from HostIF
- if (msg->hdr.appId != mHostIfAppName.id)
+ if (appId != mHostIfAppName.id)
return 1;
//they must all be at least 1 byte long
- if (!msg->hdr.len) {
+ if (!len) {
return -EINVAL;
}
- MessageBuf buf(reinterpret_cast<const char*>(msg->data), msg->hdr.len);
+ MessageBuf buf(data, len);
if (NanoHub::messageTracingEnabled()) {
- dumpBuffer("SYS -> HAL", mHostIfAppName, 0, buf.getData(), buf.getSize());
+ dumpBuffer("SYS -> HAL", mHostIfAppName, transactionId, 0, buf.getData(), buf.getSize());
}
- int status = mSessions.handleRx(buf);
+ int status = mSessions.handleRx(buf, transactionId, mAppManager, chre, reboot, rebootStatus);
if (status) {
// provide default handler for any system message, that is not properly handled
dumpBuffer(status > 0 ? "HAL (not handled)" : "HAL (error)",
- mHostIfAppName, 0, buf.getData(), buf.getSize(), status);
+ mHostIfAppName, transactionId, 0, buf.getData(), buf.getSize(), status);
status = status > 0 ? 0 : status;
}
+ if (reboot) {
+ hub_message_t msg =
+ {
+ .app_name.id = appId,
+ .message_type = CONTEXT_HUB_START_APPS,
+ .message_len = sizeof(rebootStatus),
+ .message = &rebootStatus,
+ };
+
+ status = doHandleTx(&msg, 0xFFFFFFFF);
+ }
return status;
}
-int SystemComm::SessionManager::handleRx(MessageBuf &buf)
+void SystemComm::doDumpAppInfo(std::string &result)
+{
+ mAppManager.dumpAppInfo(result);
+}
+
+int SystemComm::SessionManager::handleRx(MessageBuf &buf, uint32_t transactionId, AppManager &appManager, bool chre, bool &reboot, uint32_t &rebootStatus)
{
int status = 1;
std::unique_lock<std::mutex> lk(lock);
@@ -605,36 +1420,40 @@
continue;
}
Session *session = pos->second;
- status = session->handleRx(buf);
+ status = session->handleRx(buf, transactionId, appManager, chre);
if (status < 0) {
session->complete();
}
}
- NanohubRsp rsp(buf);
- if (rsp.cmd == NANOHUB_REBOOT) {
- // if this is reboot notification, kill all sessions
- for (auto pos = sessions_.begin(); pos != sessions_.end(); next(pos)) {
- if (!isActive(pos)) {
- continue;
+ NanohubRsp rsp(buf, transactionId, chre);
+ if (rsp.mCmd == NANOHUB_HAL_SYS_MGMT) {
+ uint8_t cmd = buf.readU8();
+
+ if (cmd == NANOHUB_HAL_SYS_MGMT_REBOOT) {
+ // if this is reboot notification, kill all sessions
+ for (auto pos = sessions_.begin(); pos != sessions_.end(); next(pos)) {
+ if (!isActive(pos)) {
+ continue;
+ }
+ Session *session = pos->second;
+ session->abort(-EINTR);
}
- Session *session = pos->second;
- session->abort(-EINTR);
+ lk.unlock();
+ // log the reboot event, if not handled
+ if (status > 0) {
+ ALOGW("Nanohub reboot status [UNSOLICITED]: %08" PRIX32, rsp.mStatus);
+ status = 0;
+ }
+ reboot = true;
+ rebootStatus = rsp.mStatus;
}
- lk.unlock();
- // log the reboot event, if not handled
- if (status > 0) {
- ALOGW("Nanohub reboot status [UNSOLICITED]: %08" PRIX32, rsp.status);
- status = 0;
- }
- // report to java apps
- sendToApp(CONTEXT_HUB_OS_REBOOT, &rsp.status, sizeof(rsp.status));
}
return status;
}
-int SystemComm::SessionManager::setup_and_add(int id, Session *session, const hub_message_t *appMsg)
+int SystemComm::SessionManager::setup_and_add(int id, Session *session, const hub_message_t *appMsg, uint32_t transactionId, AppManager &appManager)
{
std::lock_guard<std::mutex> _l(lock);
@@ -645,7 +1464,7 @@
if (sessions_.count(id) == 0 && !session->isRunning()) {
sessions_[id] = session;
- int ret = session->setup(appMsg);
+ int ret = session->setup(appMsg, transactionId, appManager);
if (ret < 0) {
session->complete();
}
@@ -654,14 +1473,14 @@
return -EBUSY;
}
-int SystemComm::doHandleTx(const hub_message_t *appMsg)
+int SystemComm::doHandleTx(const hub_message_t *appMsg, uint32_t transactionId)
{
int status = 0;
switch (appMsg->message_type) {
case CONTEXT_HUB_LOAD_APP:
if (!mKeySession.haveKeys()) {
- status = mSessions.setup_and_add(CONTEXT_HUB_LOAD_APP, &mKeySession, appMsg);
+ status = mSessions.setup_and_add(CONTEXT_HUB_LOAD_APP, &mKeySession, appMsg, transactionId, mAppManager);
if (status < 0) {
break;
}
@@ -671,21 +1490,25 @@
break;
}
}
- status = mSessions.setup_and_add(CONTEXT_HUB_LOAD_APP, &mAppMgmtSession, appMsg);
+ status = mSessions.setup_and_add(CONTEXT_HUB_LOAD_APP, &mAppMgmtSession, appMsg, transactionId, mAppManager);
break;
case CONTEXT_HUB_APPS_ENABLE:
case CONTEXT_HUB_APPS_DISABLE:
case CONTEXT_HUB_UNLOAD_APP:
// all APP-modifying commands share session key, to ensure they can't happen at the same time
- status = mSessions.setup_and_add(CONTEXT_HUB_LOAD_APP, &mAppMgmtSession, appMsg);
+ status = mSessions.setup_and_add(CONTEXT_HUB_LOAD_APP, &mAppMgmtSession, appMsg, transactionId, mAppManager);
break;
case CONTEXT_HUB_QUERY_APPS:
- status = mSessions.setup_and_add(CONTEXT_HUB_QUERY_APPS, &mAppInfoSession, appMsg);
+ mAppManager.sendAppInfoToApp(transactionId);
break;
case CONTEXT_HUB_QUERY_MEMORY:
- status = mSessions.setup_and_add(CONTEXT_HUB_QUERY_MEMORY, &mMemInfoSession, appMsg);
+ status = mSessions.setup_and_add(CONTEXT_HUB_QUERY_MEMORY, &mMemInfoSession, appMsg, transactionId, mAppManager);
+ break;
+
+ case CONTEXT_HUB_START_APPS:
+ status = mSessions.setup_and_add(CONTEXT_HUB_START_APPS, &mAppMgmtSession, appMsg, transactionId, mAppManager);
break;
default:
diff --git a/contexthubhal/system_comms.h b/contexthubhal/system_comms.h
index 6991457..46ee944 100644
--- a/contexthubhal/system_comms.h
+++ b/contexthubhal/system_comms.h
@@ -19,6 +19,7 @@
#include <utils/Condition.h>
+#include <chrono>
#include <condition_variable>
#include <map>
#include <mutex>
@@ -35,35 +36,74 @@
#define MSG_HANDLED 0
//messages to the HostIf nanoapp & their replies (mesages and replies both begin with u8 message_type)
-#define NANOHUB_EXT_APPS_ON 0 // () -> (char success)
-#define NANOHUB_EXT_APPS_OFF 1 // () -> (char success)
-#define NANOHUB_EXT_APP_DELETE 2 // (u64 name) -> (char success) //idempotent
-#define NANOHUB_QUERY_MEMINFO 3 // () -> (mem_info)
-#define NANOHUB_QUERY_APPS 4 // (u32 idxStart) -> (app_info[idxStart] OR EMPTY IF NO MORE)
-#define NANOHUB_QUERY_RSA_KEYS 5 // (u32 byteOffset) -> (u8 data[1 or more bytes] OR EMPTY IF NO MORE)
-#define NANOHUB_START_UPLOAD 6 // (char isOs, u32 totalLenToTx) -> (char success)
-#define NANOHUB_CONT_UPLOAD 7 // (u32 offset, u8 data[]) -> (char success)
-#define NANOHUB_FINISH_UPLOAD 8 // () -> (char success)
-#define NANOHUB_REBOOT 9 // () -> (char success)
+#define NANOHUB_HAL_APP_MGMT 0x10 // (char cmd, u64 appId, u64 appMsk) -> (int errno, u32 results)
+
+#define NANOHUB_HAL_APP_MGMT_START 0
+#define NANOHUB_HAL_APP_MGMT_STOP 1
+#define NANOHUB_HAL_APP_MGMT_UNLOAD 2
+#define NANOHUB_HAL_APP_MGMT_DELETE 3
+
+#define NANOHUB_HAL_SYS_MGMT 0x11 // (char cmd) -> (int errno)
+
+#define NANOHUB_HAL_SYS_MGMT_ERASE 0
+#define NANOHUB_HAL_SYS_MGMT_REBOOT 1
+
+#define NANOHUB_HAL_APP_INFO 0x12
+
+#define NANOHUB_HAL_APP_INFO_APPID 0x00
+#define NANOHUB_HAL_APP_INFO_CRC 0x01
+#define NANOHUB_HAL_APP_INFO_TID 0x02
+#define NANOHUB_HAL_APP_INFO_VERSION 0x03
+#define NANOHUB_HAL_APP_INFO_ADDR 0x04
+#define NANOHUB_HAL_APP_INFO_SIZE 0x05
+#define NANOHUB_HAL_APP_INFO_HEAP 0x06
+#define NANOHUB_HAL_APP_INFO_DATA 0x07
+#define NANOHUB_HAL_APP_INFO_BSS 0x08
+#define NANOHUB_HAL_APP_INFO_CHRE_MAJOR 0x09
+#define NANOHUB_HAL_APP_INFO_CHRE_MINOR 0x0A
+#define NANOHUB_HAL_APP_INFO_END 0xFF
+
+#define NANOHUB_HAL_SYS_INFO 0x13
+
+#define NANOHUB_HAL_SYS_INFO_HEAP_FREE 0x0F
+#define NANOHUB_HAL_SYS_INFO_RAM_SIZE 0x12
+#define NANOHUB_HAL_SYS_INFO_EEDATA_SIZE 0x13
+#define NANOHUB_HAL_SYS_INFO_EEDATA_FREE 0x14
+#define NANOHUB_HAL_SYS_INFO_CODE_SIZE 0x15
+#define NANOHUB_HAL_SYS_INFO_CODE_FREE 0x16
+#define NANOHUB_HAL_SYS_INFO_SHARED_SIZE 0x17
+#define NANOHUB_HAL_SYS_INFO_SHARED_FREE 0x18
+#define NANOHUB_HAL_SYS_INFO_END 0xFF
+
+#define NANOHUB_HAL_KEY_INFO 0x14
+#define NANOHUB_HAL_START_UPLOAD 0x16
+#define NANOHUB_HAL_CONT_UPLOAD 0x17
+#define NANOHUB_HAL_FINISH_UPLOAD 0x18
+
+#define NANOHUB_HAL_UPLOAD_ACCEPTED 0
+#define NANOHUB_HAL_UPLOAD_WAIT 1
+#define NANOHUB_HAL_UPLOAD_RESEND 2
+#define NANOHUB_HAL_UPLOAD_RESTART 3
+#define NANOHUB_HAL_UPLOAD_CANCEL 4
+#define NANOHUB_HAL_UPLOAD_CANCEL_NO_RETRY 5
+#define NANOHUB_HAL_UPLOAD_NO_SPACE 6
#define NANOHUB_APP_NOT_LOADED (-1)
#define NANOHUB_APP_LOADED (0)
#define NANOHUB_UPLOAD_CHUNK_SZ_MAX 64
#define NANOHUB_MEM_SZ_UNKNOWN 0xFFFFFFFFUL
+#define NANOHUB_TID_UNKNOWN 0xFFFFFFFFUL
+
+#define CONTEXT_HUB_START_APPS 8
namespace android {
namespace nanohub {
-int system_comms_handle_rx(const nano_message *msg);
+int system_comms_handle_rx(const nano_message_raw *msg);
int system_comms_handle_tx(const hub_message_t *outMsg);
-struct NanohubAppInfo {
- hub_app_name_t name;
- uint32_t version, flashUse, ramUse;
-} __attribute__((packed));
-
struct MgmtStatus {
union {
uint32_t value;
@@ -87,9 +127,10 @@
} __attribute__((packed));
struct NanohubRsp {
- uint32_t cmd;
- int32_t status;
- explicit NanohubRsp(MessageBuf &buf, bool no_status = false);
+ uint32_t mCmd;
+ uint32_t mTransactionId;
+ int32_t mStatus;
+ explicit NanohubRsp(MessageBuf &buf, uint32_t transactionId, bool chre);
};
inline bool operator == (const hub_app_name_t &a, const hub_app_name_t &b) {
@@ -103,6 +144,8 @@
class SystemComm {
private:
+ class AppManager;
+
/*
* Nanohub HAL sessions
*
@@ -117,8 +160,8 @@
*/
class ISession {
public:
- virtual int setup(const hub_message_t *app_msg) = 0;
- virtual int handleRx(MessageBuf &buf) = 0;
+ virtual int setup(const hub_message_t *app_msg, uint32_t transactionId, AppManager &appManager) = 0;
+ virtual int handleRx(MessageBuf &buf, uint32_t transactionId, AppManager &appManager, bool chre) = 0;
virtual int getState() const = 0; // FSM state
virtual int getStatus() const = 0; // execution status (result code)
virtual void abort(int32_t) = 0;
@@ -173,8 +216,13 @@
}
int wait() {
std::unique_lock<std::mutex> lk(mDoneMutex);
- mDoneCond.wait(lk, [this] { return mState == SESSION_DONE; });
- return 0;
+ bool success = mDoneCond.wait_for(
+ lk, std::chrono::seconds(30),
+ [this] { return mState == SESSION_DONE; });
+ if (!success) {
+ ALOGE("Timed out waiting for response");
+ }
+ return success ? 0 : -1;
}
virtual int getState() const override {
std::lock_guard<std::mutex> _l(mDoneMutex);
@@ -193,11 +241,17 @@
class AppMgmtSession : public Session {
enum {
TRANSFER = SESSION_USER,
+ QUERY_START,
+ START,
+ STOP_TRANSFER,
FINISH,
RUN,
+ STOP_RUN,
RUN_FAILED,
REBOOT,
+ ERASE_TRANSFER,
MGMT,
+ INFO,
};
uint32_t mCmd; // LOAD_APP, UNLOAD_APP, ENABLE_APP, DISABLE_APP
uint32_t mResult;
@@ -207,14 +261,21 @@
uint32_t mNextPos;
uint32_t mErrCnt;
hub_app_name_t mAppName;
+ uint32_t mFlashAddr;
+ std::vector<hub_app_name_t> mAppList;
- int setupMgmt(const hub_message_t *appMsg, uint32_t cmd);
- int handleTransfer(NanohubRsp &rsp);
- int handleFinish(NanohubRsp &rsp);
- int handleRun(NanohubRsp &rsp);
- int handleRunFailed(NanohubRsp &rsp);
- int handleReboot(NanohubRsp &rsp);
- int handleMgmt(NanohubRsp &rsp);
+ int setupMgmt(const hub_message_t *appMsg, uint32_t transactionId, uint32_t cmd, AppManager &appManager);
+ int handleTransfer(NanohubRsp &rsp, MessageBuf &, AppManager &appManager);
+ int handleStopTransfer(NanohubRsp &rsp, MessageBuf &buf, AppManager &);
+ int handleQueryStart(NanohubRsp &rsp, MessageBuf &buf, AppManager &appManager);
+ int handleStart(NanohubRsp &rsp, MessageBuf &buf, AppManager &);
+ int handleFinish(NanohubRsp &rsp, MessageBuf &buf, AppManager &appManager);
+ int handleRun(NanohubRsp &rsp, MessageBuf &buf, AppManager &appManager);
+ int handleStopRun(NanohubRsp &rsp, MessageBuf &buf, AppManager &);
+ int handleReboot(NanohubRsp &rsp, MessageBuf &buf, AppManager &);
+ int handleEraseTransfer(NanohubRsp &rsp, MessageBuf &buf, AppManager &appManager);
+ int handleMgmt(NanohubRsp &rsp, MessageBuf &buf, AppManager &appManager);
+ int handleInfo(NanohubRsp &rsp, MessageBuf &buf, AppManager &appManager);
public:
AppMgmtSession() {
mCmd = 0;
@@ -223,34 +284,133 @@
mLen = 0;
memset(&mAppName, 0, sizeof(mAppName));
}
- virtual int handleRx(MessageBuf &buf) override;
- virtual int setup(const hub_message_t *app_msg) override;
+ virtual int handleRx(MessageBuf &buf, uint32_t transactionId, AppManager &appManager, bool chre) override;
+ virtual int setup(const hub_message_t *app_msg, uint32_t transactionId, AppManager &appManager) override;
};
class MemInfoSession : public Session {
public:
- virtual int setup(const hub_message_t *app_msg) override;
- virtual int handleRx(MessageBuf &buf) override;
+ virtual int setup(const hub_message_t *app_msg, uint32_t transactionId, AppManager &) override;
+ virtual int handleRx(MessageBuf &buf, uint32_t transactionId, AppManager &, bool chre) override;
};
class KeyInfoSession : public Session {
std::vector<uint8_t> mRsaKeyData;
- int requestRsaKeys(void);
+ uint32_t mKeyNum;
+ uint32_t mKeyOffset;
+ int requestRsaKeys(uint32_t transactionId);
public:
- virtual int setup(const hub_message_t *) override;
- virtual int handleRx(MessageBuf &buf) override;
+ virtual int setup(const hub_message_t *, uint32_t, AppManager &) override;
+ virtual int handleRx(MessageBuf &buf, uint32_t transactionId, AppManager &, bool chre) override;
bool haveKeys() const {
std::lock_guard<std::mutex> _l(mLock);
return mRsaKeyData.size() > 0 && !isRunning();
}
};
- class AppInfoSession : public Session {
- std::vector<hub_app_info> mAppInfo;
- int requestNext();
+ class AppManager {
+ struct AppData {
+ uint32_t version, flashUse, ramUse;
+ uint32_t tid, crc, flashAddr;
+ uint8_t chre_major, chre_minor;
+ bool chre, running, loaded;
+
+ bool cached_start, cached_napp;
+ uint32_t cached_version, cached_crc;
+ };
+
+ typedef std::map<uint64_t, std::unique_ptr<AppData>> AppMap;
+
+ AppMap apps_;
+
public:
- virtual int setup(const hub_message_t *) override;
- virtual int handleRx(MessageBuf &buf) override;
+ AppManager() {
+ restoreApps();
+ }
+ void dumpAppInfo(std::string &result);
+ bool saveApps();
+ bool restoreApps();
+ bool eraseApps();
+ bool writeApp(hub_app_name_t &appName, const uint8_t *data, int32_t len);
+ int32_t readApp(hub_app_name_t &appName, void **data);
+ bool cmpApp(hub_app_name_t &appName, const uint8_t *data, uint32_t len);
+ uint32_t readNanohubAppInfo(MessageBuf &buf);
+ void sendAppInfoToApp(uint32_t transactionId);
+ int getAppsToStart(std::vector<hub_app_name_t> &apps);
+ bool setCachedCrc(hub_app_name_t &appName, uint32_t crc) {
+ if (!isAppPresent(appName))
+ return false;
+ else {
+ apps_[appName.id]->cached_napp = true;
+ apps_[appName.id]->cached_crc = crc;
+ apps_[appName.id]->cached_start = false;
+ saveApps();
+ return true;
+ }
+ }
+ bool clearCachedApp(hub_app_name_t &appName) {
+ if (!isAppPresent(appName))
+ return false;
+ else {
+ apps_[appName.id]->cached_napp = false;
+ apps_[appName.id]->cached_start = false;
+ saveApps();
+ return true;
+ }
+ }
+ bool clearRunning(hub_app_name_t &appName) {
+ if (!isAppLoaded(appName))
+ return false;
+ else {
+ apps_[appName.id]->running = false;
+ return true;
+ }
+ }
+
+ bool setCachedVersion(hub_app_name_t &appName, uint32_t version) {
+ if (!isAppPresent(appName))
+ return false;
+ else {
+ apps_[appName.id]->cached_version = version;
+ return true;
+ }
+ }
+ bool setCachedStart(hub_app_name_t &appName, bool start) {
+ if (!isAppPresent(appName))
+ return false;
+ else {
+ apps_[appName.id]->cached_start = start;
+ saveApps();
+ return true;
+ }
+ }
+ bool addNewApp(hub_app_name_t &appName, uint32_t version) {
+ if (isAppLoaded(appName))
+ return false;
+ else
+ apps_[appName.id] = std::unique_ptr<AppData>(new AppData);
+ apps_[appName.id]->loaded = false;
+ apps_[appName.id]->running = false;
+ apps_[appName.id]->chre = false;
+ apps_[appName.id]->cached_napp = false;
+ apps_[appName.id]->cached_version = version;
+ return true;
+ }
+ bool isAppPresent(hub_app_name_t &appName) {
+ return apps_.count(appName.id) != 0;
+ }
+ bool isAppLoaded(hub_app_name_t &appName) {
+ return apps_.count(appName.id) != 0 && apps_[appName.id]->loaded;
+ }
+ bool isAppRunning(hub_app_name_t &appName) {
+ return apps_.count(appName.id) != 0 && apps_[appName.id]->running;
+ }
+ uint32_t getFlashAddr(hub_app_name_t &appName) {
+ if (isAppPresent(appName))
+ return apps_[appName.id]->flashAddr;
+ else
+ return 0xFFFFFFFF;
+ }
};
class SessionManager {
@@ -269,8 +429,8 @@
}
public:
- int handleRx(MessageBuf &buf);
- int setup_and_add(int id, Session *session, const hub_message_t *appMsg);
+ int handleRx(MessageBuf &buf, uint32_t transactionId, AppManager &appManager, bool chre, bool &reboot, uint32_t &rebootStatus);
+ int setup_and_add(int id, Session *session, const hub_message_t *appMsg, uint32_t transactionId, AppManager &appManager);
} mSessions;
const hub_app_name_t mHostIfAppName = {
@@ -286,29 +446,44 @@
SystemComm () = default;
~SystemComm() = default;
- int doHandleTx(const hub_message_t *txMsg);
- int doHandleRx(const nano_message *rxMsg);
+ int doHandleTx(const hub_message_t *txMsg, uint32_t transactionId);
+ int doHandleRx(uint64_t appId, uint32_t transactionId, const char *data, int len, bool chre);
+ void doDumpAppInfo(std::string &result);
- static void sendToApp(uint32_t typ, const void *data, uint32_t len) {
- if (NanoHub::messageTracingEnabled()) {
- dumpBuffer("HAL -> APP", get_hub_info()->os_app_name, typ, data, len);
- }
- NanoHub::sendToApp(HubMessage(&get_hub_info()->os_app_name, typ, data, len));
+ int doHandleRx(const nano_message_raw *rxMsg) {
+ return doHandleRx(rxMsg->hdr.appId, 0, reinterpret_cast<const char*>(rxMsg->data), rxMsg->hdr.len, false);
}
- static int sendToSystem(const void *data, size_t len);
+
+ int doHandleRx(const nano_message_chre *rxMsg) {
+ return doHandleRx(rxMsg->hdr.appId, rxMsg->hdr.appEventId, reinterpret_cast<const char*>(rxMsg->data), rxMsg->hdr.len, true);
+ }
+
+ static void sendToApp(uint32_t typ, uint32_t transactionId, const void *data, uint32_t len) {
+ if (NanoHub::messageTracingEnabled()) {
+ dumpBuffer("HAL -> APP", get_hub_info()->os_app_name, transactionId, 0, data, len);
+ }
+ NanoHub::sendToApp(HubMessage(&get_hub_info()->os_app_name, typ, transactionId, ENDPOINT_BROADCAST, data, len));
+ }
+ static int sendToSystem(const void *data, size_t len, uint32_t transactionId);
KeyInfoSession mKeySession;
AppMgmtSession mAppMgmtSession;
- AppInfoSession mAppInfoSession;
MemInfoSession mMemInfoSession;
+ AppManager mAppManager;
public:
- static int handleTx(const hub_message_t *txMsg) {
- return getSystem()->doHandleTx(txMsg);
+ static int handleTx(const hub_message_t *txMsg, uint32_t transactionId) {
+ return getSystem()->doHandleTx(txMsg, transactionId);
}
- static int handleRx(const nano_message *rxMsg) {
+ static int handleRx(const nano_message_raw *rxMsg) {
return getSystem()->doHandleRx(rxMsg);
}
+ static int handleRx(const nano_message_chre *rxMsg) {
+ return getSystem()->doHandleRx(rxMsg);
+ }
+ static void dumpAppInfo(std::string &result) {
+ return getSystem()->doDumpAppInfo(result);
+ }
};
}; // namespace nanohub
diff --git a/firmware/Android.mk b/firmware/Android.mk
index db2fb86..3f4b909 100644
--- a/firmware/Android.mk
+++ b/firmware/Android.mk
@@ -88,6 +88,7 @@
libnanohub_os \
LOCAL_STATIC_LIBRARIES := \
+ libnanohub_common_os \
libnanomath_os \
libnanolibc_os \
diff --git a/firmware/app/chre/chre.mk b/firmware/app/chre/chre.mk
index aed3ce7..4d4de74 100644
--- a/firmware/app/chre/chre.mk
+++ b/firmware/app/chre/chre.mk
@@ -1,5 +1,5 @@
#
-# Copyright (C) 2016 The Android Open Source Project
+# Copyright (C) 2017 The Android Open Source Project
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
@@ -26,8 +26,9 @@
CFLAGS += $(COMMON_FLAGS)
-# CHRE API 1.1
-BIN_POSTPROCESS_ARGS := -c 0x0101
+# CHRE API 1.2
+BIN_POSTPROCESS_ARGS := -c 0x0102
CFLAGS += -I$(NANOHUB_DIR)/../../../../system/chre/chre_api/include/chre_api
+CFLAGS += -I$(NANOHUB_DIR)/../../../../system/chre/util/include
include $(NANOHUB_DIR)/app/app.mk
diff --git a/firmware/app/chre/chre11.mk b/firmware/app/chre/chre11.mk
new file mode 100644
index 0000000..1051e78
--- /dev/null
+++ b/firmware/app/chre/chre11.mk
@@ -0,0 +1,33 @@
+#
+# Copyright (C) 2017 The Android Open Source Project
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+################################################################################
+#
+# NanoApp C/C++ Makefile Utils
+#
+################################################################################
+
+SRCS += $(NANOHUB_DIR)/app/chre/common/chre_app.c
+SRCS += $(NANOHUB_DIR)/app/chre/common/chre11_app_syscalls.c
+
+include $(NANOHUB_DIR)/firmware_conf.mk
+
+CFLAGS += $(COMMON_FLAGS)
+
+# CHRE API 1.1
+BIN_POSTPROCESS_ARGS := -c 0x0101
+CFLAGS += -I$(NANOHUB_DIR)/../../../../system/chre/chre_api/legacy/v1_1
+
+include $(NANOHUB_DIR)/app/app.mk
diff --git a/firmware/app/chre/common/Android.mk b/firmware/app/chre/common/Android.mk
index 0c8302f..6a996db 100644
--- a/firmware/app/chre/common/Android.mk
+++ b/firmware/app/chre/common/Android.mk
@@ -1,5 +1,5 @@
#
-# Copyright (C) 2016 The Android Open Source Project
+# Copyright (C) 2017 The Android Open Source Project
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
@@ -16,6 +16,46 @@
LOCAL_PATH := $(call my-dir)
+########################################################
+# CHRE 1.0 Library
+########################################################
+
+include $(CLEAR_NANO_VARS)
+
+LOCAL_MODULE := libnanochre10
+LOCAL_MODULE_TAGS := optional
+
+LOCAL_SRC_FILES := \
+ chre10_app.c \
+ chre10_app_syscalls.c \
+
+LOCAL_STATIC_LIBRARIES += libnanobuiltins
+LOCAL_STATIC_LIBRARIES += libnanolibc
+
+include $(BUILD_NANOHUB_APP_STATIC_LIBRARY)
+
+########################################################
+# CHRE 1.1 Library
+########################################################
+
+include $(CLEAR_NANO_VARS)
+
+LOCAL_MODULE := libnanochre11
+LOCAL_MODULE_TAGS := optional
+
+LOCAL_SRC_FILES := \
+ chre_app.c \
+ chre11_app_syscalls.c \
+
+LOCAL_STATIC_LIBRARIES += libnanobuiltins
+LOCAL_STATIC_LIBRARIES += libnanolibc
+
+include $(BUILD_NANOHUB_APP_STATIC_LIBRARY)
+
+########################################################
+# CHRE 1.2 Library
+########################################################
+
include $(CLEAR_NANO_VARS)
LOCAL_MODULE := libnanochre
diff --git a/firmware/app/chre/common/chre10_app.c b/firmware/app/chre/common/chre10_app.c
index deda37b..6da4cc9 100644
--- a/firmware/app/chre/common/chre10_app.c
+++ b/firmware/app/chre/common/chre10_app.c
@@ -1,5 +1,5 @@
/*
- * Copyright (C) 2016 The Android Open Source Project
+ * Copyright (C) 2017 The Android Open Source Project
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
diff --git a/firmware/app/chre/common/chre10_app_syscalls.c b/firmware/app/chre/common/chre10_app_syscalls.c
index 666bdae..f72f4d4 100644
--- a/firmware/app/chre/common/chre10_app_syscalls.c
+++ b/firmware/app/chre/common/chre10_app_syscalls.c
@@ -1,5 +1,5 @@
/*
- * Copyright (C) 2016 The Android Open Source Project
+ * Copyright (C) 2017 The Android Open Source Project
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
@@ -28,6 +28,9 @@
#include <syscall.h>
#include <syscall_defs.h>
+/* not defined in chre 1.0 */
+#define CHRE_HOST_ENDPOINT_BROADCAST UINT16_C(0xFFFF)
+
#define SYSCALL_CHRE_API(name) \
SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_MAIN, SYSCALL_CHRE_MAIN_API, SYSCALL_CHRE_MAIN_API_ ## name)
@@ -113,23 +116,12 @@
interval_lo, interval_hi, latency_lo, latency_hi);
}
-bool chreSendEvent(uint16_t eventType, void *eventData,
- chreEventCompleteFunction *freeCallback,
- uint32_t targetInstanceId)
-{
- return syscallDo4P(SYSCALL_CHRE_API(SEND_EVENT), eventType, eventData, freeCallback, targetInstanceId);
-}
-
-bool chreSendMessageToHost(void *message, uint32_t messageSize,
- uint32_t reservedMessageType,
- chreMessageFreeFunction *freeCallback)
-{
- return syscallDo4P(SYSCALL_CHRE_API(SEND_MSG), message, messageSize, reservedMessageType, freeCallback);
-}
-
uint32_t chreGetApiVersion(void)
{
- return syscallDo0P(SYSCALL_CHRE_API(GET_OS_API_VERSION));
+ static uint32_t apiVersion = 0;
+ if (!apiVersion)
+ apiVersion = syscallDo0P(SYSCALL_CHRE_API(GET_OS_API_VERSION));
+ return apiVersion;
}
uint32_t chreGetVersion(void)
@@ -143,3 +135,23 @@
(void)syscallDo1P(SYSCALL_CHRE_API(GET_PLATFORM_ID), &plat);
return plat;
}
+
+bool chreSendEvent(uint16_t eventType, void *eventData,
+ chreEventCompleteFunction *freeCallback,
+ uint32_t targetInstanceId)
+{
+ if (chreGetApiVersion() == CHRE_API_VERSION_1_0)
+ return syscallDo4P(SYSCALL_CHRE_API(SEND_EVENT), eventType, eventData, freeCallback, targetInstanceId);
+ else
+ return syscallDo4P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_MAIN, SYSCALL_CHRE_MAIN_EVENT, SYSCALL_CHRE_MAIN_EVENT_SEND_EVENT), eventType, eventData, freeCallback, targetInstanceId);
+}
+
+bool chreSendMessageToHost(void *message, uint32_t messageSize,
+ uint32_t reservedMessageType,
+ chreMessageFreeFunction *freeCallback)
+{
+ if (chreGetApiVersion() == CHRE_API_VERSION_1_0)
+ return syscallDo4P(SYSCALL_CHRE_API(SEND_MSG), message, messageSize, reservedMessageType, freeCallback);
+ else
+ return syscallDo5P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_MAIN, SYSCALL_CHRE_MAIN_EVENT, SYSCALL_CHRE_MAIN_EVENT_SEND_MSG), message, messageSize, reservedMessageType, CHRE_HOST_ENDPOINT_BROADCAST, freeCallback);
+}
diff --git a/firmware/app/chre/common/chre11_app_syscalls.c b/firmware/app/chre/common/chre11_app_syscalls.c
new file mode 100644
index 0000000..4265bfa
--- /dev/null
+++ b/firmware/app/chre/common/chre11_app_syscalls.c
@@ -0,0 +1,237 @@
+/*
+ * Copyright (C) 2017 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <stdarg.h>
+
+#include <gpio.h>
+#include <osApi.h>
+#include <sensors.h>
+#include <seos.h>
+#include <util.h>
+
+/* CHRE syscalls */
+#include <chre.h>
+#include <chreApi.h>
+#include <syscall.h>
+#include <syscall_defs.h>
+
+#define SYSCALL_CHRE_API(name) \
+ SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_MAIN, SYSCALL_CHRE_MAIN_API, SYSCALL_CHRE_MAIN_API_ ## name)
+
+uint64_t chreGetAppId(void)
+{
+ uint64_t appId = 0;
+ (void)syscallDo1P(SYSCALL_CHRE_API(GET_APP_ID), &appId);
+ return appId;
+}
+
+uint32_t chreGetInstanceId(void)
+{
+ return syscallDo0P(SYSCALL_CHRE_API(GET_INST_ID));
+}
+
+uint64_t chreGetTime(void) {
+ uint64_t time_ns = 0;
+ (void)syscallDo1P(SYSCALL_CHRE_API(GET_TIME), &time_ns);
+ return time_ns;
+}
+
+int64_t chreGetEstimatedHostTimeOffset(void) {
+ int64_t time_ns = 0;
+ (void)syscallDo1P(SYSCALL_CHRE_API(GET_HOST_TIME_OFFSET), &time_ns);
+ return time_ns;
+}
+
+void chreLog(enum chreLogLevel level, const char *str, ...)
+{
+ va_list vl;
+
+ va_start(vl, str);
+ (void)syscallDo3P(SYSCALL_CHRE_API(LOG), level, str, VA_LIST_TO_INTEGER(vl));
+ va_end(vl);
+}
+
+uint32_t chreTimerSet(uint64_t duration, const void* cookie, bool oneShot)
+{
+ uint32_t dur_lo = duration;
+ uint32_t dur_hi = duration >> 32;
+ return syscallDo4P(SYSCALL_CHRE_API(TIMER_SET), dur_lo, dur_hi, cookie, oneShot);
+}
+
+bool chreTimerCancel(uint32_t timerId)
+{
+ return syscallDo1P(SYSCALL_CHRE_API(TIMER_CANCEL), timerId);
+}
+
+void chreAbort(uint32_t abortCode)
+{
+ (void)syscallDo1P(SYSCALL_CHRE_API(ABORT), abortCode);
+}
+
+void* chreHeapAlloc(uint32_t bytes)
+{
+ return (void *)syscallDo1P(SYSCALL_CHRE_API(HEAP_ALLOC), bytes);
+}
+
+void chreHeapFree(void* ptr)
+{
+ (void)syscallDo1P(SYSCALL_CHRE_API(HEAP_FREE), ptr);
+}
+
+bool chreSensorFindDefault(uint8_t sensorType, uint32_t *handle)
+{
+ return syscallDo2P(SYSCALL_CHRE_API(SENSOR_FIND_DEFAULT), sensorType, handle);
+}
+
+bool chreGetSensorInfo(uint32_t sensorHandle, struct chreSensorInfo *info)
+{
+ return syscallDo2P(SYSCALL_CHRE_API(SENSOR_GET_INFO), sensorHandle, info);
+}
+
+bool chreGetSensorSamplingStatus(uint32_t sensorHandle,
+ struct chreSensorSamplingStatus *status)
+{
+ return syscallDo2P(SYSCALL_CHRE_API(SENSOR_GET_STATUS), sensorHandle, status);
+}
+
+bool chreSensorConfigure(uint32_t sensorHandle,
+ enum chreSensorConfigureMode mode,
+ uint64_t interval, uint64_t latency)
+{
+ uint32_t interval_lo = interval;
+ uint32_t interval_hi = interval >> 32;
+ uint32_t latency_lo = latency;
+ uint32_t latency_hi = latency >> 32;
+ return syscallDo6P(SYSCALL_CHRE_API(SENSOR_CONFIG), sensorHandle, mode,
+ interval_lo, interval_hi, latency_lo, latency_hi);
+}
+
+uint32_t chreGetApiVersion(void)
+{
+ static uint32_t apiVersion = 0;
+ if (!apiVersion)
+ apiVersion = syscallDo0P(SYSCALL_CHRE_API(GET_OS_API_VERSION));
+ return apiVersion;
+}
+
+uint32_t chreGetVersion(void)
+{
+ return syscallDo0P(SYSCALL_CHRE_API(GET_OS_VERSION));
+}
+
+uint64_t chreGetPlatformId(void)
+{
+ uint64_t plat = 0;
+ (void)syscallDo1P(SYSCALL_CHRE_API(GET_PLATFORM_ID), &plat);
+ return plat;
+}
+
+bool chreSendEvent(uint16_t eventType, void *eventData,
+ chreEventCompleteFunction *freeCallback,
+ uint32_t targetInstanceId)
+{
+ if (chreGetApiVersion() == CHRE_API_VERSION_1_0)
+ return syscallDo4P(SYSCALL_CHRE_API(SEND_EVENT), eventType, eventData, freeCallback, targetInstanceId);
+ else
+ return syscallDo4P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_MAIN, SYSCALL_CHRE_MAIN_EVENT, SYSCALL_CHRE_MAIN_EVENT_SEND_EVENT), eventType, eventData, freeCallback, targetInstanceId);
+}
+
+bool chreSendMessageToHost(void *message, uint32_t messageSize,
+ uint32_t messageType,
+ chreMessageFreeFunction *freeCallback)
+{
+ if (chreGetApiVersion() == CHRE_API_VERSION_1_0)
+ return syscallDo4P(SYSCALL_CHRE_API(SEND_MSG), message, messageSize, messageType, freeCallback);
+ else
+ return syscallDo5P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_MAIN, SYSCALL_CHRE_MAIN_EVENT, SYSCALL_CHRE_MAIN_EVENT_SEND_MSG), message, messageSize, messageType, CHRE_HOST_ENDPOINT_BROADCAST, freeCallback);
+}
+
+bool chreSendMessageToHostEndpoint(void *message, size_t messageSize,
+ uint32_t messageType, uint16_t hostEndpoint,
+ chreMessageFreeFunction *freeCallback)
+{
+ if (chreGetApiVersion() == CHRE_API_VERSION_1_0)
+ return syscallDo4P(SYSCALL_CHRE_API(SEND_MSG), message, messageSize, messageType, freeCallback);
+ else
+ return syscallDo5P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_MAIN, SYSCALL_CHRE_MAIN_EVENT, SYSCALL_CHRE_MAIN_EVENT_SEND_MSG), message, messageSize, messageType, hostEndpoint, freeCallback);
+}
+
+bool chreGetNanoappInfoByAppId(uint64_t appId, struct chreNanoappInfo *info)
+{
+ uint32_t app_lo = appId;
+ uint32_t app_hi = appId >> 32;
+ return syscallDo3P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_MAIN, SYSCALL_CHRE_MAIN_EVENT, SYSCALL_CHRE_MAIN_EVENT_INFO_BY_APP_ID), app_lo, app_hi, info);
+}
+
+bool chreGetNanoappInfoByInstanceId(uint32_t instanceId, struct chreNanoappInfo *info)
+{
+ return syscallDo2P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_MAIN, SYSCALL_CHRE_MAIN_EVENT, SYSCALL_CHRE_MAIN_EVENT_INFO_BY_INST_ID), instanceId, info);
+}
+
+void chreConfigureNanoappInfoEvents(bool enable)
+{
+ syscallDo1P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_MAIN, SYSCALL_CHRE_MAIN_EVENT, SYSCALL_CHRE_MAIN_EVENT_CFG_INFO), enable);
+}
+
+uint32_t chreGnssGetCapabilities(void)
+{
+ return syscallDo0P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_DRIVERS, SYSCALL_CHRE_DRV_GNSS, SYSCALL_CHRE_DRV_GNSS_GET_CAP));
+}
+
+bool chreGnssLocationSessionStartAsync(uint32_t minIntervalMs, uint32_t minTimeToNextFixMs, const void *cookie)
+{
+ return syscallDo3P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_DRIVERS, SYSCALL_CHRE_DRV_GNSS, SYSCALL_CHRE_DRV_GNSS_LOC_START_ASYNC), minIntervalMs, minTimeToNextFixMs, cookie);
+}
+
+bool chreGnssLocationSessionStopAsync(const void *cookie)
+{
+ return syscallDo1P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_DRIVERS, SYSCALL_CHRE_DRV_GNSS, SYSCALL_CHRE_DRV_GNSS_LOC_STOP_ASYNC), cookie);
+}
+
+bool chreGnssMeasurementSessionStartAsync(uint32_t minIntervalMs, const void *cookie)
+{
+ return syscallDo2P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_DRIVERS, SYSCALL_CHRE_DRV_GNSS, SYSCALL_CHRE_DRV_GNSS_MEAS_START_ASYNC), minIntervalMs, cookie);
+}
+
+bool chreGnssMeasurementSessionStopAsync(const void *cookie)
+{
+ return syscallDo1P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_DRIVERS, SYSCALL_CHRE_DRV_GNSS, SYSCALL_CHRE_DRV_GNSS_MEAS_STOP_ASYNC), cookie);
+}
+
+uint32_t chreWifiGetCapabilities(void)
+{
+ return syscallDo0P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_DRIVERS, SYSCALL_CHRE_DRV_WIFI, SYSCALL_CHRE_DRV_WIFI_GET_CAP));
+}
+
+bool chreWifiConfigureScanMonitorAsync(bool enable, const void *cookie)
+{
+ return syscallDo2P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_DRIVERS, SYSCALL_CHRE_DRV_WIFI, SYSCALL_CHRE_DRV_WIFI_CONF_SCAN_MON_ASYNC), enable, cookie);
+}
+
+bool chreWifiRequestScanAsync(const struct chreWifiScanParams *params, const void *cookie)
+{
+ return syscallDo2P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_DRIVERS, SYSCALL_CHRE_DRV_WIFI, SYSCALL_CHRE_DRV_WIFI_REQ_SCAN_ASYNC), params, cookie);
+}
+
+uint32_t chreWwanGetCapabilities(void)
+{
+ return syscallDo0P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_DRIVERS, SYSCALL_CHRE_DRV_WWAN, SYSCALL_CHRE_DRV_WWAN_GET_CAP));
+}
+
+bool chreWwanGetCellInfoAsync(const void *cookie)
+{
+ return syscallDo1P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_DRIVERS, SYSCALL_CHRE_DRV_WWAN, SYSCALL_CHRE_DRV_WWAN_GET_CELL_INFO_ASYNC), cookie);
+}
diff --git a/firmware/app/chre/common/chre_app.c b/firmware/app/chre/common/chre_app.c
index 0eba239..04829b6 100644
--- a/firmware/app/chre/common/chre_app.c
+++ b/firmware/app/chre/common/chre_app.c
@@ -1,5 +1,5 @@
/*
- * Copyright (C) 2016 The Android Open Source Project
+ * Copyright (C) 2017 The Android Open Source Project
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
diff --git a/firmware/app/chre/common/chre_app_syscalls.c b/firmware/app/chre/common/chre_app_syscalls.c
index fc1bc8c..4335fee 100644
--- a/firmware/app/chre/common/chre_app_syscalls.c
+++ b/firmware/app/chre/common/chre_app_syscalls.c
@@ -1,5 +1,5 @@
/*
- * Copyright (C) 2016 The Android Open Source Project
+ * Copyright (C) 2017 The Android Open Source Project
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
@@ -163,7 +163,10 @@
uint32_t messageType, uint16_t hostEndpoint,
chreMessageFreeFunction *freeCallback)
{
- return syscallDo5P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_MAIN, SYSCALL_CHRE_MAIN_EVENT, SYSCALL_CHRE_MAIN_EVENT_SEND_MSG), message, messageSize, messageType, hostEndpoint, freeCallback);
+ if (chreGetApiVersion() == CHRE_API_VERSION_1_0)
+ return syscallDo4P(SYSCALL_CHRE_API(SEND_MSG), message, messageSize, messageType, freeCallback);
+ else
+ return syscallDo5P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_MAIN, SYSCALL_CHRE_MAIN_EVENT, SYSCALL_CHRE_MAIN_EVENT_SEND_MSG), message, messageSize, messageType, hostEndpoint, freeCallback);
}
bool chreGetNanoappInfoByAppId(uint64_t appId, struct chreNanoappInfo *info)
@@ -183,6 +186,16 @@
syscallDo1P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_MAIN, SYSCALL_CHRE_MAIN_EVENT, SYSCALL_CHRE_MAIN_EVENT_CFG_INFO), enable);
}
+void chreConfigureHostSleepStateEvents(bool enable)
+{
+ syscallDo1P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_MAIN, SYSCALL_CHRE_MAIN_EVENT, SYSCALL_CHRE_MAIN_EVENT_HOST_SLEEP), enable);
+}
+
+bool chreIsHostAwake(void)
+{
+ return syscallDo0P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_MAIN, SYSCALL_CHRE_MAIN_EVENT, SYSCALL_CHRE_MAIN_EVENT_IS_HOST_AWAKE));
+}
+
uint32_t chreGnssGetCapabilities(void)
{
return syscallDo0P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_DRIVERS, SYSCALL_CHRE_DRV_GNSS, SYSCALL_CHRE_DRV_GNSS_GET_CAP));
@@ -208,6 +221,11 @@
return syscallDo1P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_DRIVERS, SYSCALL_CHRE_DRV_GNSS, SYSCALL_CHRE_DRV_GNSS_MEAS_STOP_ASYNC), cookie);
}
+bool chreGnssConfigurePassiveLocationListener(bool enable)
+{
+ return syscallDo1P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_DRIVERS, SYSCALL_CHRE_DRV_GNSS, SYSCALL_CHRE_DRV_GNSS_CONF_PASV_LOC_LIS), enable);
+}
+
uint32_t chreWifiGetCapabilities(void)
{
return syscallDo0P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_DRIVERS, SYSCALL_CHRE_DRV_WIFI, SYSCALL_CHRE_DRV_WIFI_GET_CAP));
@@ -232,3 +250,25 @@
{
return syscallDo1P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_DRIVERS, SYSCALL_CHRE_DRV_WWAN, SYSCALL_CHRE_DRV_WWAN_GET_CELL_INFO_ASYNC), cookie);
}
+
+bool chreAudioGetSource(uint32_t handle, struct chreAudioSource *audioSource)
+{
+ return syscallDo2P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_DRIVERS, SYSCALL_CHRE_DRV_AUDIO, SYSCALL_CHRE_DRV_AUDIO_GET_SRC), handle, audioSource);
+}
+
+bool chreAudioConfigureSource(uint32_t handle, bool enable,
+ uint64_t bufferDuration,
+ uint64_t deliveryInterval)
+{
+ uint32_t duration_lo = bufferDuration;
+ uint32_t duration_hi = bufferDuration >> 32;
+ uint32_t interval_lo = deliveryInterval;
+ uint32_t interval_hi = deliveryInterval >> 32;
+
+ return syscallDo6P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_DRIVERS, SYSCALL_CHRE_DRV_AUDIO, SYSCALL_CHRE_DRV_AUDIO_CONF_SRC), handle, enable, duration_lo, duration_hi, interval_lo, interval_hi);
+}
+
+bool chreAudioGetStatus(uint32_t handle, struct chreAudioSourceStatus *status)
+{
+ return syscallDo2P(SYSCALL_NO(SYSCALL_DOMAIN_CHRE, SYSCALL_CHRE_DRIVERS, SYSCALL_CHRE_DRV_AUDIO, SYSCALL_CHRE_DRV_AUDIO_GET_STATUS), handle, status);
+}
diff --git a/firmware/build/app_chre10_executable.mk b/firmware/build/app_chre10_executable.mk
new file mode 100644
index 0000000..43f8776
--- /dev/null
+++ b/firmware/build/app_chre10_executable.mk
@@ -0,0 +1,43 @@
+#
+# Copyright (C) 2017 The Android Open Source Project
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+
+LOCAL_MODULE_SUFFIX := .napp
+include $(NANOHUB_APP_CONFIG)
+
+my_variants := $(LOCAL_NANO_VARIANT_LIST)
+
+ifeq ($(strip $(my_variants)),)
+# default is to use all variants supported by this OS
+my_variants := $(AUX_OS_VARIANT_LIST_$(NANO_OS))
+endif
+
+# mark the app as CHRE 1.0 nanoapp
+LOCAL_NANO_APP_POSTPROCESS_FLAGS += -c 0x0100
+
+# add app-side CHRE implementation
+LOCAL_WHOLE_STATIC_LIBRARIES += \
+ libnanochre10 \
+
+# add standard libaries
+LOCAL_STATIC_LIBRARIES += \
+ libnanobuiltins \
+ libnanolibc \
+ libnanolibm \
+
+LOCAL_C_INCLUDES += \
+ system/chre/chre_api/legacy/v1_0 \
+
+$(call for-each-variant,$(my_variants),APP,$(BUILD_NANOHUB_EXECUTABLE))
diff --git a/firmware/build/app_chre11_executable.mk b/firmware/build/app_chre11_executable.mk
new file mode 100644
index 0000000..2112418
--- /dev/null
+++ b/firmware/build/app_chre11_executable.mk
@@ -0,0 +1,43 @@
+#
+# Copyright (C) 2017 The Android Open Source Project
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+
+LOCAL_MODULE_SUFFIX := .napp
+include $(NANOHUB_APP_CONFIG)
+
+my_variants := $(LOCAL_NANO_VARIANT_LIST)
+
+ifeq ($(strip $(my_variants)),)
+# default is to use all variants supported by this OS
+my_variants := $(AUX_OS_VARIANT_LIST_$(NANO_OS))
+endif
+
+# mark the app as CHRE 1.1 nanoapp
+LOCAL_NANO_APP_POSTPROCESS_FLAGS += -c 0x0101
+
+# add app-side CHRE implementation
+LOCAL_WHOLE_STATIC_LIBRARIES += \
+ libnanochre11 \
+
+# add standard libaries
+LOCAL_STATIC_LIBRARIES += \
+ libnanobuiltins \
+ libnanolibc \
+ libnanolibm \
+
+LOCAL_C_INCLUDES += \
+ system/chre/chre_api/legacy/v1_1 \
+
+$(call for-each-variant,$(my_variants),APP,$(BUILD_NANOHUB_EXECUTABLE))
diff --git a/firmware/build/app_chre_executable.mk b/firmware/build/app_chre_executable.mk
index 009dc15..da77682 100644
--- a/firmware/build/app_chre_executable.mk
+++ b/firmware/build/app_chre_executable.mk
@@ -1,5 +1,5 @@
#
-# Copyright (C) 2016 The Android Open Source Project
+# Copyright (C) 2017 The Android Open Source Project
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
@@ -24,8 +24,8 @@
my_variants := $(AUX_OS_VARIANT_LIST_$(NANO_OS))
endif
-# mark the app as CHRE 1.1 nanoapp
-LOCAL_NANO_APP_POSTPROCESS_FLAGS += -c 0x0101
+# mark the app as CHRE 1.2 nanoapp
+LOCAL_NANO_APP_POSTPROCESS_FLAGS += -c 0x0102
# add app-side CHRE implementation
LOCAL_WHOLE_STATIC_LIBRARIES += \
@@ -37,4 +37,8 @@
libnanolibc \
libnanolibm \
+LOCAL_C_INCLUDES += \
+ system/chre/chre_api/include/chre_api \
+ system/chre/util/include \
+
$(call for-each-variant,$(my_variants),APP,$(BUILD_NANOHUB_EXECUTABLE))
diff --git a/firmware/build/config.mk b/firmware/build/config.mk
index 2776653..423d9d0 100644
--- a/firmware/build/config.mk
+++ b/firmware/build/config.mk
@@ -1,5 +1,5 @@
#
-# Copyright (C) 2016 The Android Open Source Project
+# Copyright (C) 2017 The Android Open Source Project
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
@@ -37,6 +37,8 @@
BUILD_NANOHUB_OS_EXECUTABLE := $(NANO_BUILD)/os_executable.mk
BUILD_NANOHUB_OS_IMAGE := $(NANO_BUILD)/os_image.mk
BUILD_NANOHUB_APP_EXECUTABLE := $(NANO_BUILD)/app_executable.mk
+BUILD_NANOHUB_APP_CHRE10_EXECUTABLE := $(NANO_BUILD)/app_chre10_executable.mk
+BUILD_NANOHUB_APP_CHRE11_EXECUTABLE := $(NANO_BUILD)/app_chre11_executable.mk
BUILD_NANOHUB_APP_CHRE_EXECUTABLE := $(NANO_BUILD)/app_chre_executable.mk
NANOAPP_POSTPROCESS := $(HOST_OUT_EXECUTABLES)/nanoapp_postprocess
diff --git a/firmware/firmware.mk b/firmware/firmware.mk
index 9e02634..221ee0c 100644
--- a/firmware/firmware.mk
+++ b/firmware/firmware.mk
@@ -85,9 +85,7 @@
#some help for bootloader
SRCS_bl += os/core/printf.c
-ifndef PLATFORM_HAS_HARDWARE_CRC
SRCS_os += ../lib/nanohub/softcrc.c
-endif
#extra deps
DEPS += $(wildcard inc/*.h)
diff --git a/firmware/os/algos/calibration/accelerometer/accel_cal.c b/firmware/os/algos/calibration/accelerometer/accel_cal.c
index c700253..99e96ef 100644
--- a/firmware/os/algos/calibration/accelerometer/accel_cal.c
+++ b/firmware/os/algos/calibration/accelerometer/accel_cal.c
@@ -15,13 +15,16 @@
*/
#include "calibration/accelerometer/accel_cal.h"
+
#include <errno.h>
+#include <inttypes.h>
#include <math.h>
#include <stdio.h>
#include <string.h>
-#include "calibration/magnetometer/mag_cal.h"
+
#include "calibration/util/cal_log.h"
+// clang-format off
#define KSCALE \
0.101936799f // Scaling from m/s^2 to g (0.101 = 1/(9.81 m/s^2)).
#define KSCALE2 9.81f // Scaling from g to m/s^2.
@@ -36,8 +39,9 @@
#define MIN_TEMP 20.0f // No Data is collected below 20 degree C.
#define MAX_TEMP 45.0f // No Data is collected above 45 degree C.
#define TEMP_CUT 30 // Separation point for temperature buckets 30 degree C.
-#define EIGEN_RATIO 0.35 // EIGEN_RATIO (must be greater than 0.35).
-#define EIGEN_MAG 0.97 // Eigen value magnitude (must be greater than 0.97).
+#define EIGEN_RATIO 0.35f // EIGEN_RATIO (must be greater than 0.35).
+#define EIGEN_MAG 0.97f // Eigen value magnitude (must be greater than 0.97).
+#define ACCEL_NEW_BIAS_THRESHOLD (0.0f) // Bias update detection threshold.
#ifdef ACCEL_CAL_DBG_ENABLED
#define TEMP_HIST_LOW \
16 // Putting all Temp counts in first bucket for temp < 16 degree C.
@@ -47,8 +51,9 @@
#endif
#ifdef IMU_TEMP_DBG_ENABLED
#define IMU_TEMP_DELTA_TIME_NANOS \
- 5000000000 // Printing every 5 seconds IMU temp.
+ 5000000000 // Printing every 5 seconds IMU temp.
#endif
+// clang-format on
/////////// Start Debug //////////////////////
@@ -159,19 +164,29 @@
uint32_t fxb, uint32_t fy, uint32_t fyb,
uint32_t fz, uint32_t fzb, uint32_t fle) {
accelGoodDataInit(&acc->agd, fx, fxb, fy, fyb, fz, fzb, fle);
- initKasa(&acc->akf);
+ kasaInit(&acc->akf);
+}
+
+// Returns true when a new accel calibration is available.
+bool accelCalNewBiasAvailable(struct AccelCal *acc) {
+ return fabsf(acc->x_bias - acc->x_bias_new) > ACCEL_NEW_BIAS_THRESHOLD ||
+ fabsf(acc->y_bias - acc->y_bias_new) > ACCEL_NEW_BIAS_THRESHOLD ||
+ fabsf(acc->z_bias - acc->z_bias_new) > ACCEL_NEW_BIAS_THRESHOLD;
}
// Accel cal init.
-void accelCalInit(struct AccelCal *acc, uint32_t t0, uint32_t n_s, float th,
- uint32_t fx, uint32_t fxb, uint32_t fy, uint32_t fyb,
- uint32_t fz, uint32_t fzb, uint32_t fle) {
+void accelCalInit(struct AccelCal *acc,
+ const struct AccelCalParameters *parameters) {
// Init core accel data.
- accelCalAlgoInit(&acc->ac1[0], fx, fxb, fy, fyb, fz, fzb, fle);
- accelCalAlgoInit(&acc->ac1[1], fx, fxb, fy, fyb, fz, fzb, fle);
+ accelCalAlgoInit(&acc->ac1[0], parameters->fx, parameters->fxb,
+ parameters->fy, parameters->fyb, parameters->fz,
+ parameters->fzb, parameters->fle);
+ accelCalAlgoInit(&acc->ac1[1], parameters->fx, parameters->fxb,
+ parameters->fy, parameters->fyb, parameters->fz,
+ parameters->fzb, parameters->fle);
// Stillness Reset.
- accelStillInit(&acc->asd, t0, n_s, th);
+ accelStillInit(&acc->asd, parameters->t0, parameters->n_s, parameters->th);
// Debug data init.
#ifdef ACCEL_CAL_DBG_ENABLED
@@ -267,31 +282,6 @@
return complete;
}
-// Accumulate data for KASA fit.
-static void accelCalUpdate(struct KasaFit *akf, struct AccelStillDet *asd) {
- // Run accumulators.
- float w = asd->mean_x * asd->mean_x + asd->mean_y * asd->mean_y +
- asd->mean_z * asd->mean_z;
-
- akf->acc_x += asd->mean_x;
- akf->acc_y += asd->mean_y;
- akf->acc_z += asd->mean_z;
- akf->acc_w += w;
-
- akf->acc_xx += asd->mean_x * asd->mean_x;
- akf->acc_xy += asd->mean_x * asd->mean_y;
- akf->acc_xz += asd->mean_x * asd->mean_z;
- akf->acc_xw += asd->mean_x * w;
-
- akf->acc_yy += asd->mean_y * asd->mean_y;
- akf->acc_yz += asd->mean_y * asd->mean_z;
- akf->acc_yw += asd->mean_y * w;
-
- akf->acc_zz += asd->mean_z * asd->mean_z;
- akf->acc_zw += asd->mean_z * w;
- akf->nsamples += 1;
-}
-
// Good data detection, sorting and accumulate the data for Kasa.
static int accelGoodData(struct AccelStillDet *asd, struct AccelCalAlgo *ac1,
float temp) {
@@ -304,42 +294,42 @@
ac1->agd.nx += 1;
ac1->agd.acc_t += temp;
ac1->agd.acc_tt += temp * temp;
- accelCalUpdate(&ac1->akf, asd);
+ kasaAccumulate(&ac1->akf, asd->mean_x, asd->mean_y, asd->mean_z);
}
// Negative x bucket nxb.
if (PHIb > asd->mean_x && ac1->agd.nxb < ac1->agd.nfxb) {
ac1->agd.nxb += 1;
ac1->agd.acc_t += temp;
ac1->agd.acc_tt += temp * temp;
- accelCalUpdate(&ac1->akf, asd);
+ kasaAccumulate(&ac1->akf, asd->mean_x, asd->mean_y, asd->mean_z);
}
// Y bucket ny.
if (PHI < asd->mean_y && ac1->agd.ny < ac1->agd.nfy) {
ac1->agd.ny += 1;
ac1->agd.acc_t += temp;
ac1->agd.acc_tt += temp * temp;
- accelCalUpdate(&ac1->akf, asd);
+ kasaAccumulate(&ac1->akf, asd->mean_x, asd->mean_y, asd->mean_z);
}
// Negative y bucket nyb.
if (PHIb > asd->mean_y && ac1->agd.nyb < ac1->agd.nfyb) {
ac1->agd.nyb += 1;
ac1->agd.acc_t += temp;
ac1->agd.acc_tt += temp * temp;
- accelCalUpdate(&ac1->akf, asd);
+ kasaAccumulate(&ac1->akf, asd->mean_x, asd->mean_y, asd->mean_z);
}
// Z bucket nz.
if (PHIZ < asd->mean_z && ac1->agd.nz < ac1->agd.nfz) {
ac1->agd.nz += 1;
ac1->agd.acc_t += temp;
ac1->agd.acc_tt += temp * temp;
- accelCalUpdate(&ac1->akf, asd);
+ kasaAccumulate(&ac1->akf, asd->mean_x, asd->mean_y, asd->mean_z);
}
// Negative z bucket nzb.
if (PHIZb > asd->mean_z && ac1->agd.nzb < ac1->agd.nfzb) {
ac1->agd.nzb += 1;
ac1->agd.acc_t += temp;
ac1->agd.acc_tt += temp * temp;
- accelCalUpdate(&ac1->akf, asd);
+ kasaAccumulate(&ac1->akf, asd->mean_x, asd->mean_y, asd->mean_z);
}
// The leftover bucket nle.
if (PHI > asd->mean_x && PHIb < asd->mean_x && PHI > asd->mean_y &&
@@ -348,7 +338,7 @@
ac1->agd.nle += 1;
ac1->agd.acc_t += temp;
ac1->agd.acc_tt += temp * temp;
- accelCalUpdate(&ac1->akf, asd);
+ kasaAccumulate(&ac1->akf, asd->mean_x, asd->mean_y, asd->mean_z);
}
// Checking if all buckets are full.
if (ac1->agd.nx == ac1->agd.nfx && ac1->agd.nxb == ac1->agd.nfxb &&
@@ -357,32 +347,16 @@
// Check if akf->nsamples is zero.
if (ac1->akf.nsamples == 0) {
agdReset(&ac1->agd);
- magKasaReset(&ac1->akf);
+ kasaReset(&ac1->akf);
complete = 0;
return complete;
- } else {
- // Normalize the data to the sample numbers.
- inv = 1.0f / ac1->akf.nsamples;
}
- ac1->akf.acc_x *= inv;
- ac1->akf.acc_y *= inv;
- ac1->akf.acc_z *= inv;
- ac1->akf.acc_w *= inv;
+ // Normalize the data to the sample numbers.
+ kasaNormalize(&ac1->akf);
- ac1->akf.acc_xx *= inv;
- ac1->akf.acc_xy *= inv;
- ac1->akf.acc_xz *= inv;
- ac1->akf.acc_xw *= inv;
-
- ac1->akf.acc_yy *= inv;
- ac1->akf.acc_yz *= inv;
- ac1->akf.acc_yw *= inv;
-
- ac1->akf.acc_zz *= inv;
- ac1->akf.acc_zw *= inv;
-
- // Calculate the temp VAR and MEA.N
+ // Calculate the temp VAR and MEAN.
+ inv = 1.0f / ac1->akf.nsamples;
ac1->agd.var_t =
(ac1->agd.acc_tt - (ac1->agd.acc_t * ac1->agd.acc_t) * inv) * inv;
ac1->agd.mean_t = ac1->agd.acc_t * inv;
@@ -395,7 +369,7 @@
ac1->agd.ny > ac1->agd.nfy || ac1->agd.nyb > ac1->agd.nfyb ||
ac1->agd.nz > ac1->agd.nfz || ac1->agd.nzb > ac1->agd.nfzb) {
agdReset(&ac1->agd);
- magKasaReset(&ac1->akf);
+ kasaReset(&ac1->akf);
complete = 0;
return complete;
}
@@ -485,14 +459,15 @@
#ifdef IMU_TEMP_DBG_ENABLED
if ((sample_time_nanos - acc->temp_time_nanos) > IMU_TEMP_DELTA_TIME_NANOS) {
CAL_DEBUG_LOG("IMU Temp Data: ",
- ", %s%d.%02d, %llu, %s%d.%05d, %s%d.%05d, %s%d.%05d \n",
- CAL_ENCODE_FLOAT(temp, 2),
- (unsigned long long int)sample_time_nanos,
- CAL_ENCODE_FLOAT(acc->x_bias_new,5),
- CAL_ENCODE_FLOAT(acc->y_bias_new,5),
- CAL_ENCODE_FLOAT(acc->z_bias_new,5));
+ ", " CAL_FORMAT_3DIGITS ", %" PRIu64
+ ", " CAL_FORMAT_6DIGITS_TRIPLET " \n",
+ CAL_ENCODE_FLOAT(temp, 3),
+ sample_time_nanos,
+ CAL_ENCODE_FLOAT(acc->x_bias_new, 6),
+ CAL_ENCODE_FLOAT(acc->y_bias_new, 6),
+ CAL_ENCODE_FLOAT(acc->z_bias_new, 6));
acc->temp_time_nanos = sample_time_nanos;
- }
+ }
#endif
int temp_gate = 0;
@@ -523,7 +498,8 @@
float radius;
// Grabbing the fit from the MAG cal.
- magKasaFit(&acc->ac1[temp_gate].akf, &bias, &radius);
+ kasaFit(&acc->ac1[temp_gate].akf, &bias, &radius, G_NORM_MAX,
+ G_NORM_MIN);
// If offset is too large don't take.
if (fabsf(bias.x) < MAX_OFF && fabsf(bias.y) < MAX_OFF &&
@@ -567,238 +543,223 @@
// Resetting the structs for a new accel cal run.
agdReset(&acc->ac1[temp_gate].agd);
- magKasaReset(&acc->ac1[temp_gate].akf);
+ kasaReset(&acc->ac1[temp_gate].akf);
}
}
}
}
#ifdef ACCEL_CAL_DBG_ENABLED
+
+// Local helper macro for printing log messages.
+#ifdef CAL_NO_FLOAT_FORMAT_STRINGS
+#define CAL_FORMAT_ACCEL_HISTORY \
+ "%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d," \
+ "%s%d.%06d,%s%d.%06d,%s%d.%06d"
+#else
+#define CAL_FORMAT_ACCEL_HISTORY \
+ "%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.6f"
+#endif // CAL_NO_FLOAT_FORMAT_STRINGS
+
// Debug Print Output
void accelCalDebPrint(struct AccelCal *acc, float temp) {
static int32_t kk = 0;
if (++kk == 1000) {
// X offset history last 10 values.
- CAL_DEBUG_LOG(
- "[BMI160]",
- "{MK_ACCEL,11,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%"
- "06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,}(x_off history)\n",
- CAL_ENCODE_FLOAT(acc->adf.x_o[0], 6),
- CAL_ENCODE_FLOAT(acc->adf.x_o[1], 6),
- CAL_ENCODE_FLOAT(acc->adf.x_o[2], 6),
- CAL_ENCODE_FLOAT(acc->adf.x_o[3], 6),
- CAL_ENCODE_FLOAT(acc->adf.x_o[4], 6),
- CAL_ENCODE_FLOAT(acc->adf.x_o[5], 6),
- CAL_ENCODE_FLOAT(acc->adf.x_o[6], 6),
- CAL_ENCODE_FLOAT(acc->adf.x_o[7], 6),
- CAL_ENCODE_FLOAT(acc->adf.x_o[8], 6),
- CAL_ENCODE_FLOAT(acc->adf.x_o[9], 6));
+ CAL_DEBUG_LOG("[ACCEL_CAL]",
+ "{11," CAL_FORMAT_ACCEL_HISTORY "}(x_off history)\n",
+ CAL_ENCODE_FLOAT(acc->adf.x_o[0], 6),
+ CAL_ENCODE_FLOAT(acc->adf.x_o[1], 6),
+ CAL_ENCODE_FLOAT(acc->adf.x_o[2], 6),
+ CAL_ENCODE_FLOAT(acc->adf.x_o[3], 6),
+ CAL_ENCODE_FLOAT(acc->adf.x_o[4], 6),
+ CAL_ENCODE_FLOAT(acc->adf.x_o[5], 6),
+ CAL_ENCODE_FLOAT(acc->adf.x_o[6], 6),
+ CAL_ENCODE_FLOAT(acc->adf.x_o[7], 6),
+ CAL_ENCODE_FLOAT(acc->adf.x_o[8], 6),
+ CAL_ENCODE_FLOAT(acc->adf.x_o[9], 6));
// Y offset history last 10 values.
- CAL_DEBUG_LOG(
- "[BMI160]",
- "{MK_ACCEL,12,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%"
- "06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,}(y_off history)\n",
- CAL_ENCODE_FLOAT(acc->adf.y_o[0], 6),
- CAL_ENCODE_FLOAT(acc->adf.y_o[1], 6),
- CAL_ENCODE_FLOAT(acc->adf.y_o[2], 6),
- CAL_ENCODE_FLOAT(acc->adf.y_o[3], 6),
- CAL_ENCODE_FLOAT(acc->adf.y_o[4], 6),
- CAL_ENCODE_FLOAT(acc->adf.y_o[5], 6),
- CAL_ENCODE_FLOAT(acc->adf.y_o[6], 6),
- CAL_ENCODE_FLOAT(acc->adf.y_o[7], 6),
- CAL_ENCODE_FLOAT(acc->adf.y_o[8], 6),
- CAL_ENCODE_FLOAT(acc->adf.y_o[9], 6));
+ CAL_DEBUG_LOG("[ACCEL_CAL]",
+ "{12," CAL_FORMAT_ACCEL_HISTORY "}(y_off history)\n",
+ CAL_ENCODE_FLOAT(acc->adf.y_o[0], 6),
+ CAL_ENCODE_FLOAT(acc->adf.y_o[1], 6),
+ CAL_ENCODE_FLOAT(acc->adf.y_o[2], 6),
+ CAL_ENCODE_FLOAT(acc->adf.y_o[3], 6),
+ CAL_ENCODE_FLOAT(acc->adf.y_o[4], 6),
+ CAL_ENCODE_FLOAT(acc->adf.y_o[5], 6),
+ CAL_ENCODE_FLOAT(acc->adf.y_o[6], 6),
+ CAL_ENCODE_FLOAT(acc->adf.y_o[7], 6),
+ CAL_ENCODE_FLOAT(acc->adf.y_o[8], 6),
+ CAL_ENCODE_FLOAT(acc->adf.y_o[9], 6));
// Z offset history last 10 values.
- CAL_DEBUG_LOG(
- "[BMI160]",
- "{MK_ACCEL,13,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%"
- "06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,}(z_off history)\n",
- CAL_ENCODE_FLOAT(acc->adf.z_o[0], 6),
- CAL_ENCODE_FLOAT(acc->adf.z_o[1], 6),
- CAL_ENCODE_FLOAT(acc->adf.z_o[2], 6),
- CAL_ENCODE_FLOAT(acc->adf.z_o[3], 6),
- CAL_ENCODE_FLOAT(acc->adf.z_o[4], 6),
- CAL_ENCODE_FLOAT(acc->adf.z_o[5], 6),
- CAL_ENCODE_FLOAT(acc->adf.z_o[6], 6),
- CAL_ENCODE_FLOAT(acc->adf.z_o[7], 6),
- CAL_ENCODE_FLOAT(acc->adf.z_o[8], 6),
- CAL_ENCODE_FLOAT(acc->adf.z_o[9], 6));
+ CAL_DEBUG_LOG("[ACCEL_CAL]",
+ "{13," CAL_FORMAT_ACCEL_HISTORY "}(z_off history)\n",
+ CAL_ENCODE_FLOAT(acc->adf.z_o[0], 6),
+ CAL_ENCODE_FLOAT(acc->adf.z_o[1], 6),
+ CAL_ENCODE_FLOAT(acc->adf.z_o[2], 6),
+ CAL_ENCODE_FLOAT(acc->adf.z_o[3], 6),
+ CAL_ENCODE_FLOAT(acc->adf.z_o[4], 6),
+ CAL_ENCODE_FLOAT(acc->adf.z_o[5], 6),
+ CAL_ENCODE_FLOAT(acc->adf.z_o[6], 6),
+ CAL_ENCODE_FLOAT(acc->adf.z_o[7], 6),
+ CAL_ENCODE_FLOAT(acc->adf.z_o[8], 6),
+ CAL_ENCODE_FLOAT(acc->adf.z_o[9], 6));
// Temp history variation VAR of offset.
- CAL_DEBUG_LOG(
- "[BMI160]",
- "{MK_ACCEL,14,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%"
- "06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,}(VAR temp history)\n",
- CAL_ENCODE_FLOAT(acc->adf.var_t[0], 6),
- CAL_ENCODE_FLOAT(acc->adf.var_t[1], 6),
- CAL_ENCODE_FLOAT(acc->adf.var_t[2], 6),
- CAL_ENCODE_FLOAT(acc->adf.var_t[3], 6),
- CAL_ENCODE_FLOAT(acc->adf.var_t[4], 6),
- CAL_ENCODE_FLOAT(acc->adf.var_t[5], 6),
- CAL_ENCODE_FLOAT(acc->adf.var_t[6], 6),
- CAL_ENCODE_FLOAT(acc->adf.var_t[7], 6),
- CAL_ENCODE_FLOAT(acc->adf.var_t[8], 6),
- CAL_ENCODE_FLOAT(acc->adf.var_t[9], 6));
+ CAL_DEBUG_LOG("[ACCEL_CAL]",
+ "{14," CAL_FORMAT_ACCEL_HISTORY "}(VAR temp history)\n",
+ CAL_ENCODE_FLOAT(acc->adf.var_t[0], 6),
+ CAL_ENCODE_FLOAT(acc->adf.var_t[1], 6),
+ CAL_ENCODE_FLOAT(acc->adf.var_t[2], 6),
+ CAL_ENCODE_FLOAT(acc->adf.var_t[3], 6),
+ CAL_ENCODE_FLOAT(acc->adf.var_t[4], 6),
+ CAL_ENCODE_FLOAT(acc->adf.var_t[5], 6),
+ CAL_ENCODE_FLOAT(acc->adf.var_t[6], 6),
+ CAL_ENCODE_FLOAT(acc->adf.var_t[7], 6),
+ CAL_ENCODE_FLOAT(acc->adf.var_t[8], 6),
+ CAL_ENCODE_FLOAT(acc->adf.var_t[9], 6));
// Temp mean history of offset.
- CAL_DEBUG_LOG(
- "[BMI160]",
- "{MK_ACCEL,15,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%"
- "06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,}(MEAN Temp history)\n",
- CAL_ENCODE_FLOAT(acc->adf.mean_t[0], 6),
- CAL_ENCODE_FLOAT(acc->adf.mean_t[1], 6),
- CAL_ENCODE_FLOAT(acc->adf.mean_t[2], 6),
- CAL_ENCODE_FLOAT(acc->adf.mean_t[3], 6),
- CAL_ENCODE_FLOAT(acc->adf.mean_t[4], 6),
- CAL_ENCODE_FLOAT(acc->adf.mean_t[5], 6),
- CAL_ENCODE_FLOAT(acc->adf.mean_t[6], 6),
- CAL_ENCODE_FLOAT(acc->adf.mean_t[7], 6),
- CAL_ENCODE_FLOAT(acc->adf.mean_t[8], 6),
- CAL_ENCODE_FLOAT(acc->adf.mean_t[9], 6));
+ CAL_DEBUG_LOG("[ACCEL_CAL]",
+ "{15," CAL_FORMAT_ACCEL_HISTORY "}(MEAN Temp history)\n",
+ CAL_ENCODE_FLOAT(acc->adf.mean_t[0], 6),
+ CAL_ENCODE_FLOAT(acc->adf.mean_t[1], 6),
+ CAL_ENCODE_FLOAT(acc->adf.mean_t[2], 6),
+ CAL_ENCODE_FLOAT(acc->adf.mean_t[3], 6),
+ CAL_ENCODE_FLOAT(acc->adf.mean_t[4], 6),
+ CAL_ENCODE_FLOAT(acc->adf.mean_t[5], 6),
+ CAL_ENCODE_FLOAT(acc->adf.mean_t[6], 6),
+ CAL_ENCODE_FLOAT(acc->adf.mean_t[7], 6),
+ CAL_ENCODE_FLOAT(acc->adf.mean_t[8], 6),
+ CAL_ENCODE_FLOAT(acc->adf.mean_t[9], 6));
// KASA radius history.
- CAL_DEBUG_LOG(
- "[BMI160]",
- "{MK_ACCEL,16,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%"
- "06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,}(radius)\n",
- CAL_ENCODE_FLOAT(acc->adf.rad[0], 6),
- CAL_ENCODE_FLOAT(acc->adf.rad[1], 6),
- CAL_ENCODE_FLOAT(acc->adf.rad[2], 6),
- CAL_ENCODE_FLOAT(acc->adf.rad[3], 6),
- CAL_ENCODE_FLOAT(acc->adf.rad[4], 6),
- CAL_ENCODE_FLOAT(acc->adf.rad[5], 6),
- CAL_ENCODE_FLOAT(acc->adf.rad[6], 6),
- CAL_ENCODE_FLOAT(acc->adf.rad[7], 6),
- CAL_ENCODE_FLOAT(acc->adf.rad[8], 6),
- CAL_ENCODE_FLOAT(acc->adf.rad[9], 6));
+ CAL_DEBUG_LOG("[ACCEL_CAL]", "{16," CAL_FORMAT_ACCEL_HISTORY "}(radius)\n",
+ CAL_ENCODE_FLOAT(acc->adf.rad[0], 6),
+ CAL_ENCODE_FLOAT(acc->adf.rad[1], 6),
+ CAL_ENCODE_FLOAT(acc->adf.rad[2], 6),
+ CAL_ENCODE_FLOAT(acc->adf.rad[3], 6),
+ CAL_ENCODE_FLOAT(acc->adf.rad[4], 6),
+ CAL_ENCODE_FLOAT(acc->adf.rad[5], 6),
+ CAL_ENCODE_FLOAT(acc->adf.rad[6], 6),
+ CAL_ENCODE_FLOAT(acc->adf.rad[7], 6),
+ CAL_ENCODE_FLOAT(acc->adf.rad[8], 6),
+ CAL_ENCODE_FLOAT(acc->adf.rad[9], 6));
kk = 0;
}
if (kk == 750) {
// Eigen Vector X.
- CAL_DEBUG_LOG(
- "[BMI160]",
- "{MK_ACCEL, "
- "7,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%"
- "06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,}(eigen x)\n",
- CAL_ENCODE_FLOAT(acc->adf.e_x[0], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_x[1], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_x[2], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_x[3], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_x[4], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_x[5], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_x[6], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_x[7], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_x[8], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_x[9], 6));
+ CAL_DEBUG_LOG("[ACCEL_CAL]", "{ 7," CAL_FORMAT_ACCEL_HISTORY "}(eigen x)\n",
+ CAL_ENCODE_FLOAT(acc->adf.e_x[0], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_x[1], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_x[2], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_x[3], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_x[4], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_x[5], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_x[6], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_x[7], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_x[8], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_x[9], 6));
// Y.
- CAL_DEBUG_LOG(
- "[BMI160]",
- "{MK_ACCEL, "
- "8,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%"
- "06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,}(eigen y)\n",
- CAL_ENCODE_FLOAT(acc->adf.e_y[0], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_y[1], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_y[2], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_y[3], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_y[4], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_y[5], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_y[6], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_y[7], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_y[8], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_y[9], 6));
+ CAL_DEBUG_LOG("[ACCEL_CAL]", "{ 8," CAL_FORMAT_ACCEL_HISTORY "}(eigen y)\n",
+ CAL_ENCODE_FLOAT(acc->adf.e_y[0], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_y[1], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_y[2], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_y[3], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_y[4], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_y[5], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_y[6], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_y[7], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_y[8], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_y[9], 6));
// Z.
- CAL_DEBUG_LOG(
- "[BMI160]",
- "{MK_ACCEL, "
- "9,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,%s%d.%"
- "06d,%s%d.%06d,%s%d.%06d,%s%d.%06d,}(eigen z)\n",
- CAL_ENCODE_FLOAT(acc->adf.e_z[0], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_z[1], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_z[2], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_z[3], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_z[4], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_z[5], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_z[6], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_z[7], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_z[8], 6),
- CAL_ENCODE_FLOAT(acc->adf.e_z[9], 6));
+ CAL_DEBUG_LOG("[ACCEL_CAL]", "{ 9," CAL_FORMAT_ACCEL_HISTORY "}(eigen z)\n",
+ CAL_ENCODE_FLOAT(acc->adf.e_z[0], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_z[1], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_z[2], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_z[3], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_z[4], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_z[5], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_z[6], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_z[7], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_z[8], 6),
+ CAL_ENCODE_FLOAT(acc->adf.e_z[9], 6));
// Accel Time in ns.
- CAL_DEBUG_LOG(
- "[BMI160]",
- "{MK_ACCEL,10,%llu,%llu,%llu,%llu,%llu,%llu,%llu,%llu,%llu,%llu,}("
- "timestamp ns)\n",
- acc->adf.cal_time[0], acc->adf.cal_time[1], acc->adf.cal_time[2],
- acc->adf.cal_time[3], acc->adf.cal_time[4], acc->adf.cal_time[5],
- acc->adf.cal_time[6], acc->adf.cal_time[7], acc->adf.cal_time[8],
- acc->adf.cal_time[9]);
+ CAL_DEBUG_LOG("[ACCEL_CAL]",
+ "{10,%" PRIu64 ",%" PRIu64 ",%" PRIu64 ",%" PRIu64 ",%" PRIu64
+ ",%" PRIu64 ",%" PRIu64 ",%" PRIu64 ",%" PRIu64 ",%" PRIu64
+ "}(timestamp ns)\n",
+ acc->adf.cal_time[0], acc->adf.cal_time[1],
+ acc->adf.cal_time[2], acc->adf.cal_time[3],
+ acc->adf.cal_time[4], acc->adf.cal_time[5],
+ acc->adf.cal_time[6], acc->adf.cal_time[7],
+ acc->adf.cal_time[8], acc->adf.cal_time[9]);
}
if (kk == 500) {
// Total bucket count.
- CAL_DEBUG_LOG(
- "[BMI160]",
- "{MK_ACCEL, 0,%2d, %2d, %2d, %2d, %2d, %2d, %2d,}(Total Bucket #)\n",
- (unsigned)acc->adf.ntx, (unsigned)acc->adf.ntxb, (unsigned)acc->adf.nty,
- (unsigned)acc->adf.ntyb, (unsigned)acc->adf.ntz,
- (unsigned)acc->adf.ntzb, (unsigned)acc->adf.ntle);
+ CAL_DEBUG_LOG("[ACCEL_CAL]",
+ "{ 0,%2d, %2d, %2d, %2d, %2d, %2d, %2d}(Total Bucket #)\n",
+ (unsigned)acc->adf.ntx, (unsigned)acc->adf.ntxb,
+ (unsigned)acc->adf.nty, (unsigned)acc->adf.ntyb,
+ (unsigned)acc->adf.ntz, (unsigned)acc->adf.ntzb,
+ (unsigned)acc->adf.ntle);
// Live bucket count lower.
- CAL_DEBUG_LOG(
- "[BMI160]",
- "{MK_ACCEL, 1,%2d, %2d, %2d, %2d, %2d, %2d, %2d, %3d,}(Bucket # "
- "lower)\n",
- (unsigned)acc->ac1[0].agd.nx, (unsigned)acc->ac1[0].agd.nxb,
- (unsigned)acc->ac1[0].agd.ny, (unsigned)acc->ac1[0].agd.nyb,
- (unsigned)acc->ac1[0].agd.nz, (unsigned)acc->ac1[0].agd.nzb,
- (unsigned)acc->ac1[0].agd.nle, (unsigned)acc->ac1[0].akf.nsamples);
+ CAL_DEBUG_LOG("[ACCEL_CAL]",
+ "{ 1,%2d, %2d, %2d, %2d, %2d, %2d, %2d, %3d}(Bucket # "
+ "lower)\n",
+ (unsigned)acc->ac1[0].agd.nx, (unsigned)acc->ac1[0].agd.nxb,
+ (unsigned)acc->ac1[0].agd.ny, (unsigned)acc->ac1[0].agd.nyb,
+ (unsigned)acc->ac1[0].agd.nz, (unsigned)acc->ac1[0].agd.nzb,
+ (unsigned)acc->ac1[0].agd.nle,
+ (unsigned)acc->ac1[0].akf.nsamples);
// Live bucket count hogher.
- CAL_DEBUG_LOG(
- "[BMI160]",
- "{MK_ACCEL, 2,%2d, %2d, %2d, %2d, %2d, %2d, %2d, %3d,}(Bucket # "
- "higher)\n",
- (unsigned)acc->ac1[1].agd.nx, (unsigned)acc->ac1[1].agd.nxb,
- (unsigned)acc->ac1[1].agd.ny, (unsigned)acc->ac1[1].agd.nyb,
- (unsigned)acc->ac1[1].agd.nz, (unsigned)acc->ac1[1].agd.nzb,
- (unsigned)acc->ac1[1].agd.nle, (unsigned)acc->ac1[1].akf.nsamples);
+ CAL_DEBUG_LOG("[ACCEL_CAL]",
+ "{ 2,%2d, %2d, %2d, %2d, %2d, %2d, %2d, %3d}(Bucket # "
+ "higher)\n",
+ (unsigned)acc->ac1[1].agd.nx, (unsigned)acc->ac1[1].agd.nxb,
+ (unsigned)acc->ac1[1].agd.ny, (unsigned)acc->ac1[1].agd.nyb,
+ (unsigned)acc->ac1[1].agd.nz, (unsigned)acc->ac1[1].agd.nzb,
+ (unsigned)acc->ac1[1].agd.nle,
+ (unsigned)acc->ac1[1].akf.nsamples);
// Offset used.
- CAL_DEBUG_LOG(
- "[BMI160]",
- "{MK_ACCEL, 3,%s%d.%06d, %s%d.%06d, %s%d.%06d, %2d,}(updated offset "
- "x,y,z, total # of offsets)\n",
- CAL_ENCODE_FLOAT(acc->x_bias, 6), CAL_ENCODE_FLOAT(acc->y_bias, 6),
- CAL_ENCODE_FLOAT(acc->z_bias, 6), (unsigned)acc->adf.noff);
+ CAL_DEBUG_LOG("[ACCEL_CAL]",
+ "{ 3,"CAL_FORMAT_6DIGITS_TRIPLET", %2d}(updated offset "
+ "x,y,z, total # of offsets)\n",
+ CAL_ENCODE_FLOAT(acc->x_bias, 6),
+ CAL_ENCODE_FLOAT(acc->y_bias, 6),
+ CAL_ENCODE_FLOAT(acc->z_bias, 6), (unsigned)acc->adf.noff);
// Offset New.
- CAL_DEBUG_LOG(
- "[BMI160]",
- "{MK_ACCEL, 4,%s%d.%06d, %s%d.%06d, %s%d.%06d, %s%d.%06d,}(New offset "
- "x,y,z, live temp)\n",
- CAL_ENCODE_FLOAT(acc->x_bias_new, 6),
- CAL_ENCODE_FLOAT(acc->y_bias_new, 6),
- CAL_ENCODE_FLOAT(acc->z_bias_new, 6), CAL_ENCODE_FLOAT(temp, 6));
+ CAL_DEBUG_LOG("[ACCEL_CAL]",
+ "{ 4," CAL_FORMAT_6DIGITS_TRIPLET ", " CAL_FORMAT_6DIGITS
+ "}(New offset x,y,z, live temp)\n",
+ CAL_ENCODE_FLOAT(acc->x_bias_new, 6),
+ CAL_ENCODE_FLOAT(acc->y_bias_new, 6),
+ CAL_ENCODE_FLOAT(acc->z_bias_new, 6),
+ CAL_ENCODE_FLOAT(temp, 6));
// Temp Histogram.
- CAL_DEBUG_LOG(
- "[BMI160]",
- "{MK_ACCEL, 5,%7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, "
- "%7d, %7d,}(temp histo)\n",
- (unsigned)acc->adf.t_hist[0], (unsigned)acc->adf.t_hist[1],
- (unsigned)acc->adf.t_hist[2], (unsigned)acc->adf.t_hist[3],
- (unsigned)acc->adf.t_hist[4], (unsigned)acc->adf.t_hist[5],
- (unsigned)acc->adf.t_hist[6], (unsigned)acc->adf.t_hist[7],
- (unsigned)acc->adf.t_hist[8], (unsigned)acc->adf.t_hist[9],
- (unsigned)acc->adf.t_hist[10], (unsigned)acc->adf.t_hist[11],
- (unsigned)acc->adf.t_hist[12]);
- CAL_DEBUG_LOG(
- "[BMI160]",
- "M{K_ACCEL, 6,%7d, %7d, %7d,%7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, "
- "%7d,}(temp histo)\n",
- (unsigned)acc->adf.t_hist[13], (unsigned)acc->adf.t_hist[14],
- (unsigned)acc->adf.t_hist[15], (unsigned)acc->adf.t_hist[16],
- (unsigned)acc->adf.t_hist[17], (unsigned)acc->adf.t_hist[18],
- (unsigned)acc->adf.t_hist[19], (unsigned)acc->adf.t_hist[20],
- (unsigned)acc->adf.t_hist[21], (unsigned)acc->adf.t_hist[22],
- (unsigned)acc->adf.t_hist[23], (unsigned)acc->adf.t_hist[24]);
+ CAL_DEBUG_LOG("[ACCEL_CAL]",
+ "{ 5,%7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, "
+ "%7d, %7d}(temp histo)\n",
+ (unsigned)acc->adf.t_hist[0], (unsigned)acc->adf.t_hist[1],
+ (unsigned)acc->adf.t_hist[2], (unsigned)acc->adf.t_hist[3],
+ (unsigned)acc->adf.t_hist[4], (unsigned)acc->adf.t_hist[5],
+ (unsigned)acc->adf.t_hist[6], (unsigned)acc->adf.t_hist[7],
+ (unsigned)acc->adf.t_hist[8], (unsigned)acc->adf.t_hist[9],
+ (unsigned)acc->adf.t_hist[10], (unsigned)acc->adf.t_hist[11],
+ (unsigned)acc->adf.t_hist[12]);
+ CAL_DEBUG_LOG("[ACCEL_CAL]",
+ "{ 6,%7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, %7d, "
+ "%7d}(temp histo)\n",
+ (unsigned)acc->adf.t_hist[13], (unsigned)acc->adf.t_hist[14],
+ (unsigned)acc->adf.t_hist[15], (unsigned)acc->adf.t_hist[16],
+ (unsigned)acc->adf.t_hist[17], (unsigned)acc->adf.t_hist[18],
+ (unsigned)acc->adf.t_hist[19], (unsigned)acc->adf.t_hist[20],
+ (unsigned)acc->adf.t_hist[21], (unsigned)acc->adf.t_hist[22],
+ (unsigned)acc->adf.t_hist[23], (unsigned)acc->adf.t_hist[24]);
}
}
#endif
diff --git a/firmware/os/algos/calibration/accelerometer/accel_cal.h b/firmware/os/algos/calibration/accelerometer/accel_cal.h
index 1cfef61..3324875 100644
--- a/firmware/os/algos/calibration/accelerometer/accel_cal.h
+++ b/firmware/os/algos/calibration/accelerometer/accel_cal.h
@@ -27,7 +27,7 @@
#include <stdint.h>
#include <sys/types.h>
-#include "calibration/magnetometer/mag_cal.h"
+#include "common/math/kasa.h"
#include "common/math/mat.h"
#ifdef __cplusplus
@@ -127,6 +127,25 @@
struct KasaFit akf;
};
+// AccelCal algorithm parameters (see the AccelCal for details).
+struct AccelCalParameters {
+ // t0 -> Sets the time how long the accel has to be still in ns.
+ // n_s -> Defines the minimum number of samples for the stillness.
+ // th -> Sets the threshold for the stillness VAR in (g rms)^2.
+ // fx,fxb,fy,fyb,fz,fzb,fle -> Defines how many counts of data in the
+ // sphere cap (Bucket) is needed to reach full.
+ uint32_t t0;
+ uint32_t n_s;
+ uint32_t fx;
+ uint32_t fxb;
+ uint32_t fy;
+ uint32_t fyb;
+ uint32_t fz;
+ uint32_t fzb;
+ uint32_t fle;
+ float th;
+};
+
// Complete accel calibration struct.
struct AccelCal {
struct AccelCalAlgo ac1[ACCEL_CAL_NUM_TEMP_WINDOWS];
@@ -163,15 +182,15 @@
float y, float z, float temp);
/* This function initializes the accCalRun data struct.
- * t0 -> Sets the time how long the accel has to be still in ns.
- * n_s -> Defines the minimum number of samples for the stillness.
- * th -> Sets the threshold for the stillness VAR in (g rms)^2.
- * fx,fxb,fy,fyb,fz,fzb,fle -> Defines how many counts of data in the
- * sphere cap (Bucket) is needed to reach full.
+ * [parameters]:
+ * t0 -> Sets the time how long the accel has to be still in ns.
+ * n_s -> Defines the minimum number of samples for the stillness.
+ * th -> Sets the threshold for the stillness VAR in (g rms)^2.
+ * fx,fxb,fy,fyb,fz,fzb,fle -> Defines how many counts of data in the
+ * sphere cap (Bucket) is needed to reach full.
*/
-void accelCalInit(struct AccelCal *acc, uint32_t t0, uint32_t n_s, float th,
- uint32_t fx, uint32_t fxb, uint32_t fy, uint32_t fyb,
- uint32_t fz, uint32_t fzb, uint32_t fle);
+void accelCalInit(struct AccelCal *acc,
+ const struct AccelCalParameters *parameters);
void accelCalDestroy(struct AccelCal *acc);
@@ -182,6 +201,9 @@
void accelCalBiasRemove(struct AccelCal *acc, float *x, float *y, float *z);
+// Returns true when a new accel calibration is available.
+bool accelCalNewBiasAvailable(struct AccelCal *acc);
+
#ifdef ACCEL_CAL_DBG_ENABLED
void accelCalDebPrint(struct AccelCal *acc, float temp);
#endif
diff --git a/firmware/os/algos/calibration/common/calibration_data.c b/firmware/os/algos/calibration/common/calibration_data.c
deleted file mode 100644
index 9ae72d2..0000000
--- a/firmware/os/algos/calibration/common/calibration_data.c
+++ /dev/null
@@ -1,36 +0,0 @@
-#include "calibration/common/calibration_data.h"
-
-#include <string.h>
-
-#include "common/math/vec.h"
-
-// FUNCTION IMPLEMENTATIONS
-//////////////////////////////////////////////////////////////////////////////
-
-// Set calibration data to identity scale factors, zero skew and
-// zero bias.
-void calDataReset(struct ThreeAxisCalData *calstruct) {
- memset(calstruct, 0, sizeof(struct ThreeAxisCalData));
- calstruct->scale_factor_x = 1.0f;
- calstruct->scale_factor_y = 1.0f;
- calstruct->scale_factor_z = 1.0f;
-}
-
-void calDataCorrectData(const struct ThreeAxisCalData* calstruct,
- const float x_impaired[THREE_AXIS_DIM],
- float* x_corrected) {
- // x_temp = (x_impaired - bias).
- float x_temp[THREE_AXIS_DIM];
- vecSub(x_temp, x_impaired, calstruct->bias, THREE_AXIS_DIM);
-
- // x_corrected = scale_skew_mat * x_temp, where:
- // scale_skew_mat = [scale_factor_x 0 0
- // skew_yx scale_factor_y 0
- // skew_zx skew_zy scale_factor_z].
- x_corrected[0] = calstruct->scale_factor_x * x_temp[0];
- x_corrected[1] = calstruct->skew_yx * x_temp[0] +
- calstruct->scale_factor_y * x_temp[1];
- x_corrected[2] = calstruct->skew_zx * x_temp[0] +
- calstruct->skew_zy * x_temp[1] +
- calstruct->scale_factor_z * x_temp[2];
-}
diff --git a/firmware/os/algos/calibration/common/diversity_checker.c b/firmware/os/algos/calibration/common/diversity_checker.c
deleted file mode 100644
index d71ad9a..0000000
--- a/firmware/os/algos/calibration/common/diversity_checker.c
+++ /dev/null
@@ -1,216 +0,0 @@
-/*
- * Copyright (C) 2016 The Android Open Source Project
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#include "calibration/common/diversity_checker.h"
-
-#include <errno.h>
-#include <stdarg.h>
-#include <stdio.h>
-#include <string.h>
-#include "common/math/vec.h"
-
-// Struct initialization.
-void diversityCheckerInit(
- struct DiversityChecker* diverse_data,
- size_t min_num_diverse_vectors,
- size_t max_num_max_distance,
- float var_threshold,
- float max_min_threshold,
- float local_field,
- float threshold_tuning_param,
- float max_distance_tuning_param) {
- ASSERT_NOT_NULL(diverse_data);
-
- // Initialize parameters.
- diverse_data->threshold_tuning_param_sq =
- (threshold_tuning_param * threshold_tuning_param);
- diverse_data->max_distance_tuning_param_sq =
- (max_distance_tuning_param * max_distance_tuning_param);
-
- // Updating the threshold and max_distance using assumed local field.
- // Testing for zero and negative local_field.
- if (local_field <= 0) {
- local_field = 1;
- }
- diversityCheckerLocalFieldUpdate(diverse_data, local_field);
- diverse_data->min_num_diverse_vectors = min_num_diverse_vectors;
-
- // Checking for min_num_diverse_vectors = 0.
- if (min_num_diverse_vectors < 1) {
- diverse_data->min_num_diverse_vectors = 1;
- }
- diverse_data->max_num_max_distance = max_num_max_distance;
- diverse_data->var_threshold = var_threshold;
- diverse_data->max_min_threshold = max_min_threshold;
-
- // Setting the rest to zero.
- diversityCheckerReset(diverse_data);
-
- // Debug Messages
-#ifdef DIVERSE_DEBUG_ENABLE
- memset(&diverse_data->diversity_dbg, 0, sizeof(diverse_data->diversity_dbg));
-#endif
-}
-
-// Reset
-void diversityCheckerReset(struct DiversityChecker* diverse_data) {
- ASSERT_NOT_NULL(diverse_data);
- // Clear data memory.
- memset(&diverse_data->diverse_data, 0,
- sizeof(diverse_data->diverse_data));
-
- // Resetting counters and data full bit.
- diverse_data->num_points = 0;
- diverse_data->num_max_dist_violations = 0;
- diverse_data->data_full = false;
-}
-
-void diversityCheckerUpdate(
- struct DiversityChecker* diverse_data, float x, float y, float z) {
- ASSERT_NOT_NULL(diverse_data);
-
- // Converting three single inputs to a vector.
- const float vec[3] = {x, y, z};
-
- // Result vector for vector difference.
- float vec_diff[3];
-
- // normSquared result (k)
- float norm_squared_result;
-
- // If memory is full, no need to run through the data.
- if (!diverse_data->data_full) {
- size_t i;
- // Running over all existing data points
- for (i = 0; i < diverse_data->num_points; ++i) {
- // v = v1 - v2;
- vecSub(vec_diff,
- &diverse_data->diverse_data[i * THREE_AXIS_DATA_DIM],
- vec,
- THREE_AXIS_DATA_DIM);
-
- // k = |v|^2
- norm_squared_result = vecNormSquared(vec_diff, THREE_AXIS_DATA_DIM);
-
- // if k < Threshold then leave the function.
- if (norm_squared_result < diverse_data->threshold) {
- return;
- }
-
- // if k > max_distance, count and leave the function.
- if (norm_squared_result > diverse_data->max_distance) {
- diverse_data->num_max_dist_violations++;
- return;
- }
- }
-
- // If none of the above caused to leave the function, data is diverse.
- // Notice that the first data vector will be stored no matter what.
- memcpy(&diverse_data->
- diverse_data[diverse_data->num_points * THREE_AXIS_DATA_DIM],
- vec,
- sizeof(float) * THREE_AXIS_DATA_DIM);
- // Count new data point.
- diverse_data->num_points++;
-
- // Setting data_full to 1, if memory is full.
- if (diverse_data->num_points == NUM_DIVERSE_VECTORS) {
- diverse_data->data_full = true;
- }
- }
-}
-
-bool diversityCheckerNormQuality(struct DiversityChecker* diverse_data,
- float x_bias,
- float y_bias,
- float z_bias) {
- ASSERT_NOT_NULL(diverse_data);
- // If not enough diverse data points or max distance violations return false.
- if (diverse_data->num_points <= diverse_data->min_num_diverse_vectors ||
- diverse_data->num_max_dist_violations >=
- diverse_data->max_num_max_distance) {
- return false;
- }
- float vec_bias[3] = {x_bias, y_bias, z_bias};
- float vec_bias_removed[3];
- float norm_results;
- float acc_norm = 0.0f;
- float acc_norm_square = 0.0f;
- float max = 0.0f;
- float min = 0.0f;
- size_t i;
- for (i = 0; i < diverse_data->num_points; ++i) {
- // v = v1 - v_bias;
- vecSub(vec_bias_removed,
- &diverse_data->diverse_data[i * THREE_AXIS_DATA_DIM],
- vec_bias,
- THREE_AXIS_DATA_DIM);
-
- // norm = ||v||
- norm_results = vecNorm(vec_bias_removed, THREE_AXIS_DATA_DIM);
-
- // Accumulate for mean and VAR.
- acc_norm += norm_results;
- acc_norm_square += norm_results * norm_results ;
-
- if (i == 0) {
- min = norm_results;
- max = norm_results;
- }
- // Finding min
- if (norm_results < min) {
- min = norm_results;
- }
-
- // Finding max.
- if (norm_results > max) {
- max = norm_results;
- }
- // can leave the function if max-min is violated
- // no need to continue.
- if ((max - min) > diverse_data->max_min_threshold) {
- return false;
- }
- }
- float inv = 1.0f / diverse_data->num_points;
- float var = (acc_norm_square - (acc_norm * acc_norm) * inv) * inv;
-
- // Debug Message.
-#ifdef DIVERSE_DEBUG_ENABLE
- diverse_data->diversity_dbg.diversity_count++;
- diverse_data->diversity_dbg.var_log = var;
- diverse_data->diversity_dbg.mean_log = acc_norm * inv;
- diverse_data->diversity_dbg.max_log = max;
- diverse_data->diversity_dbg.min_log = min;
- memcpy(&diverse_data->diversity_dbg.diverse_data_log,
- &diverse_data->diverse_data,
- sizeof(diverse_data->diversity_dbg.diverse_data_log));
-#endif
- return (var < diverse_data->var_threshold);
-}
-
-void diversityCheckerLocalFieldUpdate(struct DiversityChecker* diverse_data,
- float local_field) {
- if (local_field > 0) {
- // Updating threshold based on the local field information.
- diverse_data->threshold = diverse_data->threshold_tuning_param_sq *
- (local_field * local_field);
-
- // Updating max distance based on the local field information.
- diverse_data->max_distance = diverse_data->max_distance_tuning_param_sq *
- (local_field * local_field);
- }
-}
diff --git a/firmware/os/algos/calibration/diversity_checker/diversity_checker.c b/firmware/os/algos/calibration/diversity_checker/diversity_checker.c
new file mode 100644
index 0000000..3fab81f
--- /dev/null
+++ b/firmware/os/algos/calibration/diversity_checker/diversity_checker.c
@@ -0,0 +1,216 @@
+/*
+ * Copyright (C) 2016 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "calibration/diversity_checker/diversity_checker.h"
+
+#include <errno.h>
+#include <stdarg.h>
+#include <stdio.h>
+#include <string.h>
+
+#include "common/math/vec.h"
+
+// Struct initialization.
+void diversityCheckerInit(struct DiversityChecker* diverse_data,
+ const struct DiversityCheckerParameters* parameters) {
+ ASSERT_NOT_NULL(diverse_data);
+
+ // Initialize parameters.
+ diverse_data->threshold_tuning_param_sq =
+ (parameters->threshold_tuning_param * parameters->threshold_tuning_param);
+ diverse_data->max_distance_tuning_param_sq =
+ (parameters->max_distance_tuning_param *
+ parameters->max_distance_tuning_param);
+
+ // Updating the threshold and max_distance using assumed local field.
+ // Testing for zero and negative local_field.
+ const float local_field =
+ (parameters->local_field <= 0.0f) ? 1.0f : parameters->local_field;
+ diversityCheckerLocalFieldUpdate(diverse_data, local_field);
+ diverse_data->min_num_diverse_vectors = parameters->min_num_diverse_vectors;
+
+ // Checking for min_num_diverse_vectors = 0.
+ if (parameters->min_num_diverse_vectors < 1) {
+ diverse_data->min_num_diverse_vectors = 1;
+ }
+ diverse_data->max_num_max_distance = parameters->max_num_max_distance;
+ diverse_data->var_threshold = parameters->var_threshold;
+ diverse_data->max_min_threshold = parameters->max_min_threshold;
+
+ // Setting the rest to zero.
+ diversityCheckerReset(diverse_data);
+
+ // Debug Messages
+#ifdef DIVERSE_DEBUG_ENABLE
+ memset(&diverse_data->diversity_dbg, 0, sizeof(diverse_data->diversity_dbg));
+#endif
+}
+
+// Reset
+void diversityCheckerReset(struct DiversityChecker* diverse_data) {
+ ASSERT_NOT_NULL(diverse_data);
+ // Clear data memory.
+ memset(&diverse_data->diverse_data, 0, sizeof(diverse_data->diverse_data));
+
+ // Resetting counters and data full bit.
+ diverse_data->num_points = 0;
+ diverse_data->num_max_dist_violations = 0;
+ diverse_data->data_full = false;
+}
+
+bool diversityCheckerFindNearestPoint(struct DiversityChecker* diverse_data,
+ float x, float y, float z) {
+ // Converting three single inputs to a vector.
+ const float vec[THREE_AXIS_DATA_DIM] = {x, y, z};
+
+ // Result vector for vector difference.
+ float vec_diff[THREE_AXIS_DATA_DIM];
+
+ // normSquared result (k)
+ float norm_squared_result;
+
+ size_t i;
+
+ // Running over all existing data points
+ for (i = 0; i < diverse_data->num_points; ++i) {
+ // v = v1 - v2;
+ vecSub(vec_diff, &diverse_data->diverse_data[i * THREE_AXIS_DATA_DIM], vec,
+ THREE_AXIS_DATA_DIM);
+
+ // k = |v|^2
+ norm_squared_result = vecNormSquared(vec_diff, THREE_AXIS_DATA_DIM);
+
+ // if k < Threshold then leave the function.
+ if (norm_squared_result < diverse_data->threshold) {
+ return false;
+ }
+
+ // if k > max_distance, count and leave the function.
+ if (norm_squared_result > diverse_data->max_distance) {
+ diverse_data->num_max_dist_violations++;
+ return false;
+ }
+ }
+ return true;
+}
+
+void diversityCheckerUpdate(struct DiversityChecker* diverse_data, float x,
+ float y, float z) {
+ ASSERT_NOT_NULL(diverse_data);
+
+ // If memory is full, no need to run through the data.
+ if (!diverse_data->data_full) {
+ // diversityCheckerDataSet() returns true, if input data is diverse against
+ // the already stored.
+ if (diversityCheckerFindNearestPoint(diverse_data, x, y, z)) {
+ // Converting three single inputs to a vector.
+ const float vec[THREE_AXIS_DATA_DIM] = {x, y, z};
+
+ // Notice that the first data vector will be stored no matter what.
+ memcpy(
+ &diverse_data
+ ->diverse_data[diverse_data->num_points * THREE_AXIS_DATA_DIM],
+ vec, sizeof(float) * THREE_AXIS_DATA_DIM);
+
+ // Count new data point.
+ diverse_data->num_points++;
+
+ // Setting data_full to true, if memory is full.
+ if (diverse_data->num_points == NUM_DIVERSE_VECTORS) {
+ diverse_data->data_full = true;
+ }
+ }
+ }
+}
+
+bool diversityCheckerNormQuality(struct DiversityChecker* diverse_data,
+ float x_bias, float y_bias, float z_bias) {
+ ASSERT_NOT_NULL(diverse_data);
+ // If not enough diverse data points or max distance violations return false.
+ if (diverse_data->num_points <= diverse_data->min_num_diverse_vectors ||
+ diverse_data->num_max_dist_violations >=
+ diverse_data->max_num_max_distance) {
+ return false;
+ }
+ float vec_bias[THREE_AXIS_DATA_DIM] = {x_bias, y_bias, z_bias};
+ float vec_bias_removed[THREE_AXIS_DATA_DIM];
+ float norm_results;
+ float acc_norm = 0.0f;
+ float acc_norm_square = 0.0f;
+ float max = 0.0f;
+ float min = 0.0f;
+ size_t i;
+ for (i = 0; i < diverse_data->num_points; ++i) {
+ // v = v1 - v_bias;
+ vecSub(vec_bias_removed,
+ &diverse_data->diverse_data[i * THREE_AXIS_DATA_DIM], vec_bias,
+ THREE_AXIS_DATA_DIM);
+
+ // norm = ||v||
+ norm_results = vecNorm(vec_bias_removed, THREE_AXIS_DATA_DIM);
+
+ // Accumulate for mean and VAR.
+ acc_norm += norm_results;
+ acc_norm_square += norm_results * norm_results;
+
+ if (i == 0) {
+ min = norm_results;
+ max = norm_results;
+ }
+ // Finding min
+ if (norm_results < min) {
+ min = norm_results;
+ }
+
+ // Finding max.
+ if (norm_results > max) {
+ max = norm_results;
+ }
+ // can leave the function if max-min is violated
+ // no need to continue.
+ if ((max - min) > diverse_data->max_min_threshold) {
+ return false;
+ }
+ }
+ float inv = 1.0f / diverse_data->num_points;
+ float var = (acc_norm_square - (acc_norm * acc_norm) * inv) * inv;
+
+ // Debug Message.
+#ifdef DIVERSE_DEBUG_ENABLE
+ diverse_data->diversity_dbg.diversity_count++;
+ diverse_data->diversity_dbg.var_log = var;
+ diverse_data->diversity_dbg.mean_log = acc_norm * inv;
+ diverse_data->diversity_dbg.max_log = max;
+ diverse_data->diversity_dbg.min_log = min;
+ memcpy(&diverse_data->diversity_dbg.diverse_data_log,
+ &diverse_data->diverse_data,
+ sizeof(diverse_data->diversity_dbg.diverse_data_log));
+#endif
+ return (var < diverse_data->var_threshold);
+}
+
+void diversityCheckerLocalFieldUpdate(struct DiversityChecker* diverse_data,
+ float local_field) {
+ if (local_field > 0) {
+ // Updating threshold based on the local field information.
+ diverse_data->threshold =
+ diverse_data->threshold_tuning_param_sq * (local_field * local_field);
+
+ // Updating max distance based on the local field information.
+ diverse_data->max_distance = diverse_data->max_distance_tuning_param_sq *
+ (local_field * local_field);
+ }
+}
diff --git a/firmware/os/algos/calibration/common/diversity_checker.h b/firmware/os/algos/calibration/diversity_checker/diversity_checker.h
similarity index 79%
rename from firmware/os/algos/calibration/common/diversity_checker.h
rename to firmware/os/algos/calibration/diversity_checker/diversity_checker.h
index 5a24529..c38549b 100644
--- a/firmware/os/algos/calibration/common/diversity_checker.h
+++ b/firmware/os/algos/calibration/diversity_checker/diversity_checker.h
@@ -41,13 +41,18 @@
* full. This has been done in order to save processing power.
*/
-#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_COMMON_DIVERSITY_CHECKER_H_
-#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_COMMON_DIVERSITY_CHECKER_H_
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_DIVERSITY_CHECKER_DIVERSITY_CHECKER_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_DIVERSITY_CHECKER_DIVERSITY_CHECKER_H_
#include <stdbool.h>
#include <stddef.h>
#include <stdint.h>
+#if defined(MAG_CAL_DEBUG_ENABLE) && !defined(DIVERSE_DEBUG_ENABLE)
+// Ensures that diversity messaging is set when mag_cal debugging is enabled.
+#define DIVERSE_DEBUG_ENABLE
+#endif // MAG_CAL_DEBUG_ENABLE && !DIVERSE_DEBUG_ENABLE
+
#ifdef __cplusplus
extern "C" {
#endif
@@ -68,6 +73,17 @@
};
#endif
+// DiversityChecker parameters container.
+struct DiversityCheckerParameters {
+ float var_threshold;
+ float max_min_threshold;
+ float local_field;
+ float threshold_tuning_param;
+ float max_distance_tuning_param;
+ size_t min_num_diverse_vectors;
+ size_t max_num_max_distance;
+};
+
// Main data struct.
struct DiversityChecker {
// Data memory.
@@ -82,7 +98,7 @@
// Threshold value that is used to check k against.
float threshold;
- // Threshold tuning paramter used to calculate threshold (k_algo):
+ // Threshold tuning parameter used to calculate threshold (k_algo):
// threshold = threshold_tuning_param_sq * (local_field)^2.
float threshold_tuning_param_sq;
@@ -97,7 +113,6 @@
bool data_full;
// Setup variables for NormQuality check.
-
size_t min_num_diverse_vectors;
size_t max_num_max_distance;
float var_threshold;
@@ -109,7 +124,7 @@
#endif
};
-// Initialization of the function/struct, input:
+// Initialization of the function/struct, input parameters struct consists of:
// min_num_diverse_vectors -> sets the gate for a minimum number of data points
// in the memory
// max_num_max_distance -> sets the value for a max distance violation number
@@ -123,16 +138,18 @@
// max_distance_tuning_param -> Max distance tuning parameter used to calculate
// max_distance.
void diversityCheckerInit(struct DiversityChecker* diverse_data,
- size_t min_num_diverse_vectors,
- size_t max_num_max_distance, float var_threshold,
- float max_min_threshold, float local_field,
- float threshold_tuning_param,
- float max_distance_tuning_param);
+ const struct DiversityCheckerParameters* parameters);
// Resetting the memory and the counters, leaves threshold and max_distance
// as well as the setup variables for NormQuality check untouched.
void diversityCheckerReset(struct DiversityChecker* diverse_data);
+// Checks if data point (x, y, z) is diverse against the diverse_data set.
+// Returns true when the input point is diverse.
+// Returns false when a maximum distance check is violated.
+bool diversityCheckerFindNearestPoint(struct DiversityChecker* diverse_data,
+ float x, float y, float z);
+
// Main function. Tests the data (x,y,z) against the memory if diverse and
// stores it, if so.
void diversityCheckerUpdate(struct DiversityChecker* diverse_data, float x,
@@ -149,9 +166,7 @@
// -> norm must be within a MAX/MIN window.
// Returned value will only be true if all 4 gates are passed.
bool diversityCheckerNormQuality(struct DiversityChecker* diverse_data,
- float x_bias,
- float y_bias,
- float z_bias);
+ float x_bias, float y_bias, float z_bias);
// This function updates the threshold value and max distance value based on the
// local field. This ensures a local field independent operation of the
@@ -165,4 +180,4 @@
}
#endif
-#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_COMMON_DIVERSITY_CHECKER_H_
+#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_DIVERSITY_CHECKER_DIVERSITY_CHECKER_H_
diff --git a/firmware/os/algos/calibration/gyroscope/gyro_cal.c b/firmware/os/algos/calibration/gyroscope/gyro_cal.c
index 3179b0e..90b2544 100644
--- a/firmware/os/algos/calibration/gyroscope/gyro_cal.c
+++ b/firmware/os/algos/calibration/gyroscope/gyro_cal.c
@@ -17,6 +17,7 @@
#include "calibration/gyroscope/gyro_cal.h"
#include <float.h>
+#include <inttypes.h>
#include <math.h>
#include <string.h>
@@ -40,6 +41,10 @@
// A debug version label to help with tracking results.
#define GYROCAL_DEBUG_VERSION_STRING "[July 05, 2017]"
+// Parameters used for sample rate estimation.
+#define GYROCAL_DEBUG_SAMPLE_RATE_NUM_INTERVALS (100)
+#define GYROCAL_DEBUG_SAMPLE_RATE_GAP_SEC (1.0f)
+
// Debug log tag string used to identify debug report output data.
#define GYROCAL_REPORT_TAG "[GYRO_CAL:REPORT]"
#endif // GYRO_CAL_DBG_ENABLED
@@ -103,26 +108,6 @@
MAG_STATS_TUNING
};
-/*
- * Updates the running calculation of the gyro's mean sampling rate.
- *
- * Behavior:
- * 1) If 'debug_mean_sampling_rate_hz' pointer is not NULL then the local
- * calculation of the sampling rate is copied, and the function returns.
- * 2) Else, if 'reset_stats' is 'true' then the local estimate is reset and
- * the function returns.
- * 3) Otherwise, the local estimate of the mean sampling rates is updated.
- *
- * INPUTS:
- * sample_rate_estimator: Pointer to the estimator data structure.
- * debug_mean_sampling_rate_hz: Pointer to the mean sampling rate to update.
- * timestamp_nanos: Time stamp (nanoseconds).
- * reset_stats: Flag that signals a reset of the sampling rate estimate.
- */
-static void gyroSamplingRateUpdate(struct SampleRateData* sample_rate_estimator,
- float* debug_mean_sampling_rate_hz,
- uint64_t timestamp_nanos, bool reset_stats);
-
// Updates the information used for debug printouts.
static void gyroCalUpdateDebug(struct GyroCal* gyro_cal);
@@ -130,35 +115,13 @@
static void gyroCalDebugPrintData(const struct GyroCal* gyro_cal,
char* debug_tag,
enum DebugPrintData print_data);
-
-// This conversion function is necessary for Nanohub firmware compilation (i.e.,
-// can't cast a uint64_t to a float directly). This conversion function was
-// copied from: /third_party/contexthub/firmware/src/floatRt.c
-static float floatFromUint64(uint64_t v)
-{
- uint32_t hi = v >> 32, lo = v;
-
- if (!hi) //this is very fast for cases where we fit into a uint32_t
- return (float)lo;
- else {
- return ((float)hi) * 4294967296.0f + (float)lo;
- }
-}
#endif // GYRO_CAL_DBG_ENABLED
/////// FUNCTION DEFINITIONS /////////////////////////////////////////
// Initialize the gyro calibration data structure.
-void gyroCalInit(struct GyroCal* gyro_cal, uint64_t min_still_duration_nanos,
- uint64_t max_still_duration_nanos, float bias_x, float bias_y,
- float bias_z, uint64_t calibration_time_nanos,
- uint64_t window_time_duration_nanos, float gyro_var_threshold,
- float gyro_confidence_delta, float accel_var_threshold,
- float accel_confidence_delta, float mag_var_threshold,
- float mag_confidence_delta, float stillness_threshold,
- float stillness_mean_delta_limit,
- float temperature_delta_limit_celsius,
- bool gyro_calibration_enable) {
+void gyroCalInit(struct GyroCal* gyro_cal,
+ const struct GyroCalParameters* parameters) {
// Clear gyro_cal structure memory.
memset(gyro_cal, 0, sizeof(struct GyroCal));
@@ -166,35 +129,38 @@
// Gyro parameter input units are [rad/sec].
// Accel parameter input units are [m/sec^2].
// Magnetometer parameter input units are [uT].
- gyroStillDetInit(&gyro_cal->gyro_stillness_detect, gyro_var_threshold,
- gyro_confidence_delta);
- gyroStillDetInit(&gyro_cal->accel_stillness_detect, accel_var_threshold,
- accel_confidence_delta);
- gyroStillDetInit(&gyro_cal->mag_stillness_detect, mag_var_threshold,
- mag_confidence_delta);
+ gyroStillDetInit(&gyro_cal->gyro_stillness_detect,
+ parameters->gyro_var_threshold,
+ parameters->gyro_confidence_delta);
+ gyroStillDetInit(&gyro_cal->accel_stillness_detect,
+ parameters->accel_var_threshold,
+ parameters->accel_confidence_delta);
+ gyroStillDetInit(&gyro_cal->mag_stillness_detect,
+ parameters->mag_var_threshold,
+ parameters->mag_confidence_delta);
// Reset stillness flag and start timestamp.
gyro_cal->prev_still = false;
gyro_cal->start_still_time_nanos = 0;
// Set the min and max window stillness duration.
- gyro_cal->min_still_duration_nanos = min_still_duration_nanos;
- gyro_cal->max_still_duration_nanos = max_still_duration_nanos;
+ gyro_cal->min_still_duration_nanos = parameters->min_still_duration_nanos;
+ gyro_cal->max_still_duration_nanos = parameters->max_still_duration_nanos;
// Sets the duration of the stillness processing windows.
- gyro_cal->window_time_duration_nanos = window_time_duration_nanos;
+ gyro_cal->window_time_duration_nanos = parameters->window_time_duration_nanos;
// Set the watchdog timeout duration.
gyro_cal->gyro_watchdog_timeout_duration_nanos = GYRO_WATCHDOG_TIMEOUT_NANOS;
// Load the last valid cal from system memory.
- gyro_cal->bias_x = bias_x; // [rad/sec]
- gyro_cal->bias_y = bias_y; // [rad/sec]
- gyro_cal->bias_z = bias_z; // [rad/sec]
- gyro_cal->calibration_time_nanos = calibration_time_nanos;
+ gyro_cal->bias_x = parameters->bias_x; // [rad/sec]
+ gyro_cal->bias_y = parameters->bias_y; // [rad/sec]
+ gyro_cal->bias_z = parameters->bias_z; // [rad/sec]
+ gyro_cal->calibration_time_nanos = parameters->calibration_time_nanos;
// Set the stillness threshold required for gyro bias calibration.
- gyro_cal->stillness_threshold = stillness_threshold;
+ gyro_cal->stillness_threshold = parameters->stillness_threshold;
// Current window end-time used to assist in keeping sensor data collection in
// sync. Setting this to zero signals that sensor data will be dropped until a
@@ -202,13 +168,14 @@
gyro_cal->stillness_win_endtime_nanos = 0;
// Gyro calibrations will be applied (see, gyroCalRemoveBias()).
- gyro_cal->gyro_calibration_enable = (gyro_calibration_enable > 0);
+ gyro_cal->gyro_calibration_enable = (parameters->gyro_calibration_enable > 0);
// Sets the stability limit for the stillness window mean acceptable delta.
- gyro_cal->stillness_mean_delta_limit = stillness_mean_delta_limit;
+ gyro_cal->stillness_mean_delta_limit = parameters->stillness_mean_delta_limit;
// Sets the min/max temperature delta limit for the stillness period.
- gyro_cal->temperature_delta_limit_celsius = temperature_delta_limit_celsius;
+ gyro_cal->temperature_delta_limit_celsius =
+ parameters->temperature_delta_limit_celsius;
// Ensures that the data tracking functionality is reset.
gyroStillMeanTracker(gyro_cal, DO_RESET);
@@ -221,41 +188,47 @@
CAL_DEBUG_LOG("[GYRO_CAL:INIT]", "Online gyroscope calibration DISABLED.");
}
- // Ensures that the gyro sampling rate estimate is reset.
- gyroSamplingRateUpdate(&gyro_cal->sample_rate_estimator, NULL, 0,
- /*reset_stats=*/true);
+ // Initializes the gyro sampling rate estimator.
+ sampleRateEstimatorInit(&gyro_cal->debug_gyro_cal.sample_rate_estimator,
+ GYROCAL_DEBUG_SAMPLE_RATE_NUM_INTERVALS,
+ GYROCAL_DEBUG_SAMPLE_RATE_GAP_SEC);
#endif // GYRO_CAL_DBG_ENABLED
}
// Void pointer in the gyro calibration data structure (doesn't do anything
// except prevent compiler warnings).
-void gyroCalDestroy(struct GyroCal* gyro_cal) {
- (void)gyro_cal;
-}
+void gyroCalDestroy(struct GyroCal* gyro_cal) { (void)gyro_cal; }
// Get the most recent bias calibration value.
void gyroCalGetBias(struct GyroCal* gyro_cal, float* bias_x, float* bias_y,
- float* bias_z, float* temperature_celsius) {
+ float* bias_z, float* temperature_celsius,
+ uint64_t* calibration_time_nanos) {
*bias_x = gyro_cal->bias_x;
*bias_y = gyro_cal->bias_y;
*bias_z = gyro_cal->bias_z;
+ *calibration_time_nanos = gyro_cal->calibration_time_nanos;
*temperature_celsius = gyro_cal->bias_temperature_celsius;
}
// Set an initial bias calibration value.
void gyroCalSetBias(struct GyroCal* gyro_cal, float bias_x, float bias_y,
- float bias_z, uint64_t calibration_time_nanos) {
+ float bias_z, float temperature_celsius,
+ uint64_t calibration_time_nanos) {
gyro_cal->bias_x = bias_x;
gyro_cal->bias_y = bias_y;
gyro_cal->bias_z = bias_z;
gyro_cal->calibration_time_nanos = calibration_time_nanos;
+ gyro_cal->bias_temperature_celsius = temperature_celsius;
#ifdef GYRO_CAL_DBG_ENABLED
CAL_DEBUG_LOG("[GYRO_CAL:SET BIAS]",
- "Gyro Bias Calibration [mDPS]: " CAL_FORMAT_3DIGITS_TRIPLET,
- CAL_ENCODE_FLOAT(gyro_cal->bias_x * RAD_TO_MDEG, 3),
- CAL_ENCODE_FLOAT(gyro_cal->bias_y * RAD_TO_MDEG, 3),
- CAL_ENCODE_FLOAT(gyro_cal->bias_z * RAD_TO_MDEG, 3));
+ "Offset|Temp|Time [mDPS|C|nsec]: " CAL_FORMAT_3DIGITS_TRIPLET
+ ", " CAL_FORMAT_3DIGITS ", %" PRIu64,
+ CAL_ENCODE_FLOAT(bias_x * RAD_TO_MDEG, 3),
+ CAL_ENCODE_FLOAT(bias_y * RAD_TO_MDEG, 3),
+ CAL_ENCODE_FLOAT(bias_z * RAD_TO_MDEG, 3),
+ CAL_ENCODE_FLOAT(temperature_celsius, 3),
+ calibration_time_nanos);
#endif // GYRO_CAL_DBG_ENABLED
}
@@ -298,8 +271,8 @@
#ifdef GYRO_CAL_DBG_ENABLED
// Update the gyro sampling rate estimate.
- gyroSamplingRateUpdate(&gyro_cal->sample_rate_estimator, NULL,
- sample_time_nanos, /*reset_stats=*/false);
+ sampleRateEstimatorUpdate(&gyro_cal->debug_gyro_cal.sample_rate_estimator,
+ sample_time_nanos);
#endif // GYRO_CAL_DBG_ENABLED
// Pass gyro data to stillness detector
@@ -396,7 +369,7 @@
// Determines if the device is currently still.
device_is_still = (conf_still > gyro_cal->stillness_threshold) &&
- !mean_not_stable && !min_max_temp_exceeded;
+ !mean_not_stable && !min_max_temp_exceeded;
if (device_is_still) {
// Device is "still" logic:
@@ -440,12 +413,6 @@
computeGyroCal(gyro_cal,
gyro_cal->gyro_stillness_detect.last_sample_time);
-#ifdef GYRO_CAL_DBG_ENABLED
- // Resets the sampling rate estimate.
- gyroSamplingRateUpdate(&gyro_cal->sample_rate_estimator, NULL,
- sample_time_nanos, /*reset_stats=*/true);
-#endif // GYRO_CAL_DBG_ENABLED
-
// Update stillness flag. Force the start of a new stillness period.
gyro_cal->prev_still = false;
} else {
@@ -484,12 +451,6 @@
gyroTemperatureStatsTracker(gyro_cal, 0.0f, DO_RESET);
gyroStillMeanTracker(gyro_cal, DO_RESET);
-#ifdef GYRO_CAL_DBG_ENABLED
- // Resets the sampling rate estimate.
- gyroSamplingRateUpdate(&gyro_cal->sample_rate_estimator, NULL,
- sample_time_nanos, /*reset_stats=*/true);
-#endif // GYRO_CAL_DBG_ENABLED
-
// Update stillness flag.
gyro_cal->prev_still = false;
}
@@ -501,17 +462,17 @@
// Calculates a new gyro bias offset calibration value.
void computeGyroCal(struct GyroCal* gyro_cal, uint64_t calibration_time_nanos) {
// Check to see if new calibration values is within acceptable range.
- if (!(gyro_cal->gyro_stillness_detect.prev_mean_x < MAX_GYRO_BIAS &&
+ if (!(gyro_cal->gyro_stillness_detect.prev_mean_x < MAX_GYRO_BIAS &&
gyro_cal->gyro_stillness_detect.prev_mean_x > -MAX_GYRO_BIAS &&
- gyro_cal->gyro_stillness_detect.prev_mean_y < MAX_GYRO_BIAS &&
+ gyro_cal->gyro_stillness_detect.prev_mean_y < MAX_GYRO_BIAS &&
gyro_cal->gyro_stillness_detect.prev_mean_y > -MAX_GYRO_BIAS &&
- gyro_cal->gyro_stillness_detect.prev_mean_z < MAX_GYRO_BIAS &&
+ gyro_cal->gyro_stillness_detect.prev_mean_z < MAX_GYRO_BIAS &&
gyro_cal->gyro_stillness_detect.prev_mean_z > -MAX_GYRO_BIAS)) {
#ifdef GYRO_CAL_DBG_ENABLED
CAL_DEBUG_LOG(
"[GYRO_CAL:REJECT]",
"Offset|Temp|Time [mDPS|C|nsec]: " CAL_FORMAT_3DIGITS_TRIPLET
- ", " CAL_FORMAT_3DIGITS ", %llu",
+ ", " CAL_FORMAT_3DIGITS ", %" PRIu64,
CAL_ENCODE_FLOAT(
gyro_cal->gyro_stillness_detect.prev_mean_x * RAD_TO_MDEG, 3),
CAL_ENCODE_FLOAT(
@@ -519,7 +480,7 @@
CAL_ENCODE_FLOAT(
gyro_cal->gyro_stillness_detect.prev_mean_z * RAD_TO_MDEG, 3),
CAL_ENCODE_FLOAT(gyro_cal->temperature_mean_celsius, 3),
- (unsigned long long int)calibration_time_nanos);
+ calibration_time_nanos);
#endif // GYRO_CAL_DBG_ENABLED
// Outside of range. Ignore, reset, and continue.
@@ -580,21 +541,17 @@
#ifdef GYRO_CAL_DBG_ENABLED
gyro_cal->debug_watchdog_count++;
if (sample_time_nanos < gyro_cal->gyro_watchdog_start_nanos) {
- CAL_DEBUG_LOG(
- "[GYRO_CAL:WATCHDOG]",
- "Total#, Timestamp | Delta [nsec]: %lu, %llu, -%llu",
- (unsigned long int)gyro_cal->debug_watchdog_count,
- (unsigned long long int)sample_time_nanos,
- (unsigned long long int)(gyro_cal->gyro_watchdog_start_nanos -
- sample_time_nanos));
+ CAL_DEBUG_LOG("[GYRO_CAL:WATCHDOG]",
+ "Total#, Timestamp | Delta [nsec]: %zu, %" PRIu64
+ ", -%" PRIu64,
+ gyro_cal->debug_watchdog_count, sample_time_nanos,
+ gyro_cal->gyro_watchdog_start_nanos - sample_time_nanos);
} else {
- CAL_DEBUG_LOG(
- "[GYRO_CAL:WATCHDOG]",
- "Total#, Timestamp | Delta [nsec]: %lu, %llu, %llu",
- (unsigned long int)gyro_cal->debug_watchdog_count,
- (unsigned long long int)sample_time_nanos,
- (unsigned long long int)(sample_time_nanos -
- gyro_cal->gyro_watchdog_start_nanos));
+ CAL_DEBUG_LOG("[GYRO_CAL:WATCHDOG]",
+ "Total#, Timestamp | Delta [nsec]: %zu, %" PRIu64
+ ", %" PRIu64,
+ gyro_cal->debug_watchdog_count, sample_time_nanos,
+ sample_time_nanos - gyro_cal->gyro_watchdog_start_nanos);
}
#endif // GYRO_CAL_DBG_ENABLED
@@ -607,12 +564,6 @@
gyroTemperatureStatsTracker(gyro_cal, 0.0f, DO_RESET);
gyroStillMeanTracker(gyro_cal, DO_RESET);
-#ifdef GYRO_CAL_DBG_ENABLED
- // Resets the sampling rate estimate.
- gyroSamplingRateUpdate(&gyro_cal->sample_rate_estimator, NULL,
- sample_time_nanos, /*reset_stats=*/true);
-#endif // GYRO_CAL_DBG_ENABLED
-
// Resets the stillness window end-time.
gyro_cal->stillness_win_endtime_nanos = 0;
@@ -631,7 +582,6 @@
}
// Assert watchdog timeout flags.
- gyro_cal->gyro_watchdog_timeout |= watchdog_timeout;
gyro_cal->gyro_watchdog_start_nanos = 0;
}
}
@@ -833,53 +783,6 @@
}
#ifdef GYRO_CAL_DBG_ENABLED
-void gyroSamplingRateUpdate(struct SampleRateData* sample_rate_estimator,
- float* debug_mean_sampling_rate_hz,
- uint64_t timestamp_nanos, bool reset_stats) {
- // If 'debug_mean_sampling_rate_hz' is not NULL then this function just reads
- // out the estimate of the sampling rate.
- if (debug_mean_sampling_rate_hz) {
- if (sample_rate_estimator->num_samples > 1 &&
- sample_rate_estimator->time_delta_accumulator > 0) {
- // Computes the final mean calculation.
- *debug_mean_sampling_rate_hz =
- sample_rate_estimator->num_samples /
- (floatFromUint64(sample_rate_estimator->time_delta_accumulator) *
- NANOS_TO_SEC);
- } else {
- // Not enough samples to compute a valid sample rate estimate. Indicate
- // this with a -1 value.
- *debug_mean_sampling_rate_hz = -1.0f;
- }
- reset_stats = true;
- }
-
- // Resets the sampling rate mean estimator data.
- if (reset_stats) {
- sample_rate_estimator->last_timestamp_nanos = 0;
- sample_rate_estimator->time_delta_accumulator = 0;
- sample_rate_estimator->num_samples = 0;
- return;
- }
-
- // Skip adding this data to the accumulator if:
- // 1. A bad timestamp was received (i.e., time not monotonic).
- // 2. 'last_timestamp_nanos' is zero.
- if (timestamp_nanos <= sample_rate_estimator->last_timestamp_nanos ||
- sample_rate_estimator->last_timestamp_nanos == 0) {
- sample_rate_estimator->last_timestamp_nanos = timestamp_nanos;
- return;
- }
-
- // Increments the number of samples.
- sample_rate_estimator->num_samples++;
-
- // Accumulate the time steps.
- sample_rate_estimator->time_delta_accumulator +=
- timestamp_nanos - sample_rate_estimator->last_timestamp_nanos;
- sample_rate_estimator->last_timestamp_nanos = timestamp_nanos;
-}
-
void gyroCalUpdateDebug(struct GyroCal* gyro_cal) {
// Only update this data if debug printing is not currently in progress
// (i.e., don't want to risk overwriting debug information that is actively
@@ -912,11 +815,6 @@
gyro_cal->debug_gyro_cal.calibration[1] = gyro_cal->bias_y;
gyro_cal->debug_gyro_cal.calibration[2] = gyro_cal->bias_z;
- // Records the mean gyroscope sampling rate.
- gyroSamplingRateUpdate(&gyro_cal->sample_rate_estimator,
- &gyro_cal->debug_gyro_cal.mean_sampling_rate_hz, 0,
- /*reset_stats=*/true);
-
// Records the min/max gyroscope window stillness mean values.
memcpy(gyro_cal->debug_gyro_cal.gyro_winmean_min, gyro_cal->gyro_winmean_min,
sizeof(gyro_cal->gyro_winmean_min));
@@ -983,8 +881,9 @@
CAL_DEBUG_LOG(
debug_tag,
"Cal#|Offset|Temp|Time [mDPS|C|nsec]: "
- "%lu, " CAL_FORMAT_3DIGITS_TRIPLET ", " CAL_FORMAT_3DIGITS ", %llu",
- (unsigned long int)gyro_cal->debug_calibration_count,
+ "%zu, " CAL_FORMAT_3DIGITS_TRIPLET ", " CAL_FORMAT_3DIGITS
+ ", %" PRIu64,
+ gyro_cal->debug_calibration_count,
CAL_ENCODE_FLOAT(
gyro_cal->debug_gyro_cal.calibration[0] * RAD_TO_MDEG, 3),
CAL_ENCODE_FLOAT(
@@ -993,8 +892,7 @@
gyro_cal->debug_gyro_cal.calibration[2] * RAD_TO_MDEG, 3),
CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.temperature_mean_celsius,
3),
- (unsigned long long int)
- gyro_cal->debug_gyro_cal.end_still_time_nanos);
+ gyro_cal->debug_gyro_cal.end_still_time_nanos);
break;
case STILLNESS_DATA:
@@ -1003,13 +901,11 @@
: -1.0f; // Signals that magnetometer was not used.
CAL_DEBUG_LOG(
debug_tag,
- "Cal#|Stillness|Confidence [nsec]: %lu, "
- "%llu, " CAL_FORMAT_3DIGITS_TRIPLET,
- (unsigned long int)gyro_cal->debug_calibration_count,
- (unsigned long long int)(gyro_cal->debug_gyro_cal
- .end_still_time_nanos -
- gyro_cal->debug_gyro_cal
- .start_still_time_nanos),
+ "Cal#|Stillness|Confidence [nsec]: %zu, "
+ "%" PRIu64 ", " CAL_FORMAT_3DIGITS_TRIPLET,
+ gyro_cal->debug_calibration_count,
+ gyro_cal->debug_gyro_cal.end_still_time_nanos -
+ gyro_cal->debug_gyro_cal.start_still_time_nanos,
CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_stillness_conf, 3),
CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_stillness_conf, 3),
CAL_ENCODE_FLOAT(mag_data, 3));
@@ -1019,9 +915,9 @@
CAL_DEBUG_LOG(
debug_tag,
"Cal#|Mean|Min|Max|Delta|Sample Rate [C|Hz]: "
- "%lu, " CAL_FORMAT_3DIGITS_TRIPLET ", " CAL_FORMAT_3DIGITS
+ "%zu, " CAL_FORMAT_3DIGITS_TRIPLET ", " CAL_FORMAT_3DIGITS
", " CAL_FORMAT_3DIGITS,
- (unsigned long int)gyro_cal->debug_calibration_count,
+ gyro_cal->debug_calibration_count,
CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.temperature_mean_celsius,
3),
CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.temperature_min_celsius, 3),
@@ -1029,15 +925,17 @@
CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.temperature_max_celsius -
gyro_cal->debug_gyro_cal.temperature_min_celsius,
3),
- CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mean_sampling_rate_hz, 3));
+ CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.sample_rate_estimator
+ .mean_sampling_rate_estimate_hz,
+ 3));
break;
case GYRO_MINMAX_STILLNESS_MEAN:
CAL_DEBUG_LOG(
debug_tag,
"Cal#|Gyro Peak Stillness Variation [mDPS]: "
- "%lu, " CAL_FORMAT_3DIGITS_TRIPLET,
- (unsigned long int)gyro_cal->debug_calibration_count,
+ "%zu, " CAL_FORMAT_3DIGITS_TRIPLET,
+ gyro_cal->debug_calibration_count,
CAL_ENCODE_FLOAT((gyro_cal->debug_gyro_cal.gyro_winmean_max[0] -
gyro_cal->debug_gyro_cal.gyro_winmean_min[0]) *
RAD_TO_MDEG,
@@ -1055,9 +953,9 @@
case ACCEL_STATS:
CAL_DEBUG_LOG(debug_tag,
"Cal#|Accel Mean|Var [m/sec^2|(m/sec^2)^2]: "
- "%lu, " CAL_FORMAT_3DIGITS_TRIPLET
+ "%zu, " CAL_FORMAT_3DIGITS_TRIPLET
", " CAL_FORMAT_6DIGITS_TRIPLET,
- (unsigned long int)gyro_cal->debug_calibration_count,
+ gyro_cal->debug_calibration_count,
CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_mean[0], 3),
CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_mean[1], 3),
CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.accel_mean[2], 3),
@@ -1069,9 +967,9 @@
case GYRO_STATS:
CAL_DEBUG_LOG(
debug_tag,
- "Cal#|Gyro Mean|Var [mDPS|mDPS^2]: %lu, " CAL_FORMAT_3DIGITS_TRIPLET
+ "Cal#|Gyro Mean|Var [mDPS|mDPS^2]: %zu, " CAL_FORMAT_3DIGITS_TRIPLET
", " CAL_FORMAT_3DIGITS_TRIPLET,
- (unsigned long int)gyro_cal->debug_calibration_count,
+ gyro_cal->debug_calibration_count,
CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_mean[0] * RAD_TO_MDEG,
3),
CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.gyro_mean[1] * RAD_TO_MDEG,
@@ -1093,9 +991,9 @@
if (gyro_cal->debug_gyro_cal.using_mag_sensor) {
CAL_DEBUG_LOG(
debug_tag,
- "Cal#|Mag Mean|Var [uT|uT^2]: %lu, " CAL_FORMAT_3DIGITS_TRIPLET
+ "Cal#|Mag Mean|Var [uT|uT^2]: %zu, " CAL_FORMAT_3DIGITS_TRIPLET
", " CAL_FORMAT_6DIGITS_TRIPLET,
- (unsigned long int)gyro_cal->debug_calibration_count,
+ gyro_cal->debug_calibration_count,
CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[0], 3),
CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[1], 3),
CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_mean[2], 3),
@@ -1104,9 +1002,9 @@
CAL_ENCODE_FLOAT(gyro_cal->debug_gyro_cal.mag_var[2], 6));
} else {
CAL_DEBUG_LOG(debug_tag,
- "Cal#|Mag Mean|Var [uT|uT^2]: %lu, 0, 0, 0, -1.0, -1.0, "
+ "Cal#|Mag Mean|Var [uT|uT^2]: %zu, 0, 0, 0, -1.0, -1.0, "
"-1.0",
- (unsigned long int)gyro_cal->debug_calibration_count);
+ gyro_cal->debug_calibration_count);
}
break;
diff --git a/firmware/os/algos/calibration/gyroscope/gyro_cal.h b/firmware/os/algos/calibration/gyroscope/gyro_cal.h
index 5e7d5ee..1f17254 100644
--- a/firmware/os/algos/calibration/gyroscope/gyro_cal.h
+++ b/firmware/os/algos/calibration/gyroscope/gyro_cal.h
@@ -35,7 +35,6 @@
* - Temperature [Celsius]
*
* #define GYRO_CAL_DBG_ENABLED to enable debug printout statements.
- * data to assist in tuning the GyroCal parameters.
*/
#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_GYROSCOPE_GYRO_CAL_H_
@@ -43,6 +42,10 @@
#include "calibration/gyroscope/gyro_stillness_detect.h"
+#ifdef GYRO_CAL_DBG_ENABLED
+#include "calibration/sample_rate_estimator/sample_rate_estimator.h"
+#endif // GYRO_CAL_DBG_ENABLED
+
#ifdef __cplusplus
extern "C" {
#endif
@@ -63,10 +66,10 @@
// Gyro Cal debug information/data tracking structure.
struct DebugGyroCal {
+ struct SampleRateEstimator sample_rate_estimator;
uint64_t start_still_time_nanos;
uint64_t end_still_time_nanos;
uint64_t stillness_duration_nanos;
- float mean_sampling_rate_hz;
float accel_stillness_conf;
float gyro_stillness_conf;
float mag_stillness_conf;
@@ -84,15 +87,29 @@
float temperature_mean_celsius;
bool using_mag_sensor;
};
-
-// Data structure for sample rate estimation.
-struct SampleRateData {
- uint64_t last_timestamp_nanos;
- uint64_t time_delta_accumulator;
- size_t num_samples;
-};
#endif // GYRO_CAL_DBG_ENABLED
+// GyroCal algorithm parameters (see GyroCal and GyroStillDet for details).
+struct GyroCalParameters {
+ uint64_t min_still_duration_nanos;
+ uint64_t max_still_duration_nanos;
+ uint64_t calibration_time_nanos;
+ uint64_t window_time_duration_nanos;
+ float bias_x; // units: radians per second
+ float bias_y;
+ float bias_z;
+ float stillness_threshold; // units: (radians per second)^2
+ float stillness_mean_delta_limit; // units: radians per second
+ float gyro_var_threshold; // units: (radians per second)^2
+ float gyro_confidence_delta; // units: (radians per second)^2
+ float accel_var_threshold; // units: (meters per second)^2
+ float accel_confidence_delta; // units: (meters per second)^2
+ float mag_var_threshold; // units: micro-tesla^2
+ float mag_confidence_delta; // units: micro-tesla^2
+ float temperature_delta_limit_celsius;
+ bool gyro_calibration_enable;
+};
+
// Data structure for tracking min/max window mean during device stillness.
struct MinMaxWindowMeanData {
float gyro_winmean_min[3];
@@ -149,7 +166,6 @@
// Watchdog timer to reset to a known good state when data capture stalls.
uint64_t gyro_watchdog_start_nanos;
uint64_t gyro_watchdog_timeout_duration_nanos;
- bool gyro_watchdog_timeout;
// Flag is "true" when the magnetometer is used.
bool using_mag_sensor;
@@ -177,7 +193,7 @@
float temperature_mean_celsius;
float temperature_delta_limit_celsius;
-//----------------------------------------------------------------
+ //----------------------------------------------------------------
#ifdef GYRO_CAL_DBG_ENABLED
// Debug info.
@@ -185,9 +201,6 @@
enum GyroCalDebugState debug_state; // Debug printout state machine.
enum GyroCalDebugState next_state; // Debug state machine next state.
uint64_t wait_timer_nanos; // Debug message throttle timer.
-
- struct SampleRateData sample_rate_estimator; // Debug sample rate estimator.
-
size_t debug_calibration_count; // Total number of cals performed.
size_t debug_watchdog_count; // Total number of watchdog timeouts.
bool debug_print_trigger; // Flag used to trigger data printout.
@@ -197,27 +210,21 @@
/////// FUNCTION PROTOTYPES //////////////////////////////////////////
// Initialize the gyro calibration data structure.
-void gyroCalInit(struct GyroCal* gyro_cal, uint64_t min_still_duration,
- uint64_t max_still_duration_nanos, float bias_x, float bias_y,
- float bias_z, uint64_t calibration_time_nanos,
- uint64_t window_time_duration_nanos, float gyro_var_threshold,
- float gyro_confidence_delta, float accel_var_threshold,
- float accel_confidence_delta, float mag_var_threshold,
- float mag_confidence_delta, float stillness_threshold,
- float stillness_mean_delta_limit,
- float temperature_delta_limit_celsius,
- bool gyro_calibration_enable);
+void gyroCalInit(struct GyroCal* gyro_cal,
+ const struct GyroCalParameters* parameters);
// Void all pointers in the gyro calibration data structure.
void gyroCalDestroy(struct GyroCal* gyro_cal);
// Get the most recent bias calibration value.
void gyroCalGetBias(struct GyroCal* gyro_cal, float* bias_x, float* bias_y,
- float* bias_z, float* temperature_celsius);
+ float* bias_z, float* temperature_celsius,
+ uint64_t* calibration_time_nanos);
// Set an initial bias calibration value.
void gyroCalSetBias(struct GyroCal* gyro_cal, float bias_x, float bias_y,
- float bias_z, uint64_t calibration_time_nanos);
+ float bias_z, float temperature_celsius,
+ uint64_t calibration_time_nanos);
// Remove gyro bias from the calibration [rad/sec].
void gyroCalRemoveBias(struct GyroCal* gyro_cal, float xi, float yi, float zi,
diff --git a/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.c b/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.c
index 80f2fa2..bb2063b 100644
--- a/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.c
+++ b/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.c
@@ -15,6 +15,7 @@
*/
#include "calibration/gyroscope/gyro_stillness_detect.h"
+
#include <string.h>
/////// FORWARD DECLARATIONS /////////////////////////////////////////
@@ -25,8 +26,8 @@
/////// FUNCTION DEFINITIONS /////////////////////////////////////////
// Initialize the GyroStillDet structure.
-void gyroStillDetInit(struct GyroStillDet* gyro_still_det,
- float var_threshold, float confidence_delta) {
+void gyroStillDetInit(struct GyroStillDet* gyro_still_det, float var_threshold,
+ float confidence_delta) {
// Clear all data structure variables to 0.
memset(gyro_still_det, 0, sizeof(struct GyroStillDet));
@@ -192,12 +193,12 @@
// Each axis score is limited [0,1].
tmp_denom = 1.f / (upper_var_thresh - lower_var_thresh);
gyro_still_det->stillness_confidence =
- gyroStillDetLimit(
- 0.5f - (gyro_still_det->win_var_x - var_thresh) * tmp_denom) *
- gyroStillDetLimit(
- 0.5f - (gyro_still_det->win_var_y - var_thresh) * tmp_denom) *
- gyroStillDetLimit(
- 0.5f - (gyro_still_det->win_var_z - var_thresh) * tmp_denom);
+ gyroStillDetLimit(0.5f - (gyro_still_det->win_var_x - var_thresh) *
+ tmp_denom) *
+ gyroStillDetLimit(0.5f - (gyro_still_det->win_var_y - var_thresh) *
+ tmp_denom) *
+ gyroStillDetLimit(0.5f - (gyro_still_det->win_var_z - var_thresh) *
+ tmp_denom);
}
}
@@ -207,8 +208,7 @@
// Resets the stillness detector and initiates a new detection window.
// 'reset_stats' determines whether the stillness statistics are reset.
-void gyroStillDetReset(struct GyroStillDet* gyro_still_det,
- bool reset_stats) {
+void gyroStillDetReset(struct GyroStillDet* gyro_still_det, bool reset_stats) {
float tmp_denom = 1.f;
// Reset the stillness data ready flag.
diff --git a/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.h b/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.h
index 9a1d876..51c4bee 100644
--- a/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.h
+++ b/firmware/os/algos/calibration/gyroscope/gyro_stillness_detect.h
@@ -60,7 +60,7 @@
// is used to keep track of the window start time.
bool start_new_window;
- // Starting time stamp for the the current window.
+ // Starting time stamp for the current window.
uint64_t window_start_time;
// Accumulator variables for tracking the sample mean during
@@ -93,8 +93,8 @@
/////// FUNCTION PROTOTYPES //////////////////////////////////////////
// Initialize the gyro_still_det_t structure.
-void gyroStillDetInit(struct GyroStillDet* gyro_still_det,
- float var_threshold, float confidence_delta);
+void gyroStillDetInit(struct GyroStillDet* gyro_still_det, float var_threshold,
+ float confidence_delta);
// Update the stillness detector with a new sample.
void gyroStillDetUpdate(struct GyroStillDet* gyro_still_det,
diff --git a/firmware/os/algos/calibration/magnetometer/mag_cal.c b/firmware/os/algos/calibration/magnetometer/mag_cal.c
deleted file mode 100644
index 7f8e563..0000000
--- a/firmware/os/algos/calibration/magnetometer/mag_cal.c
+++ /dev/null
@@ -1,520 +0,0 @@
-/*
- * Copyright (C) 2016 The Android Open Source Project
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#include "calibration/magnetometer/mag_cal.h"
-
-#include <errno.h>
-#include <string.h>
-
-#include "calibration/util/cal_log.h"
-
-#ifdef MAG_CAL_ORIGINAL_TUNING
-#define MAX_EIGEN_RATIO 25.0f
-#define MAX_EIGEN_MAG 80.0f // uT
-#define MIN_EIGEN_MAG 10.0f // uT
-#define MAX_FIT_MAG 80.0f
-#define MIN_FIT_MAG 10.0f
-#define MAX_BATCH_WINDOW 15000000UL // 15 sec
-#define MIN_BATCH_SIZE 25 // samples
-#else
-#define MAX_EIGEN_RATIO 15.0f
-#define MAX_EIGEN_MAG 70.0f // uT
-#define MIN_EIGEN_MAG 20.0f // uT
-#define MAX_FIT_MAG 70.0f
-#define MIN_FIT_MAG 20.0f
-#define MAX_BATCH_WINDOW 15000000UL // 15 sec
-#define MIN_BATCH_SIZE 25 // samples
-#endif
-
-#ifdef DIVERSITY_CHECK_ENABLED
-#define MAX_DISTANCE_VIOLATIONS 2
-#ifdef SPHERE_FIT_ENABLED
-# define MAX_ITERATIONS 30
-# define INITIAL_U_SCALE 1.0e-4f
-# define GRADIENT_THRESHOLD 1.0e-16f
-# define RELATIVE_STEP_THRESHOLD 1.0e-7f
-# define FROM_MICRO_SEC_TO_SEC 1.0e-6f
-#endif
-#endif
-
-// eigen value magnitude and ratio test
-static int moc_eigen_test(struct KasaFit *kasa) {
- // covariance matrix
- struct Mat33 S;
- S.elem[0][0] = kasa->acc_xx - kasa->acc_x * kasa->acc_x;
- S.elem[0][1] = S.elem[1][0] = kasa->acc_xy - kasa->acc_x * kasa->acc_y;
- S.elem[0][2] = S.elem[2][0] = kasa->acc_xz - kasa->acc_x * kasa->acc_z;
- S.elem[1][1] = kasa->acc_yy - kasa->acc_y * kasa->acc_y;
- S.elem[1][2] = S.elem[2][1] = kasa->acc_yz - kasa->acc_y * kasa->acc_z;
- S.elem[2][2] = kasa->acc_zz - kasa->acc_z * kasa->acc_z;
-
- struct Vec3 eigenvals;
- struct Mat33 eigenvecs;
- mat33GetEigenbasis(&S, &eigenvals, &eigenvecs);
-
- float evmax = (eigenvals.x > eigenvals.y) ? eigenvals.x : eigenvals.y;
- evmax = (eigenvals.z > evmax) ? eigenvals.z : evmax;
-
- float evmin = (eigenvals.x < eigenvals.y) ? eigenvals.x : eigenvals.y;
- evmin = (eigenvals.z < evmin) ? eigenvals.z : evmin;
-
- float eigenvals_sum = eigenvals.x + eigenvals.y + eigenvals.z;
-
- // Testing for negative number.
- float evmag = (eigenvals_sum > 0) ? sqrtf(eigenvals_sum) : 0;
-
- int eigen_pass = (evmin * MAX_EIGEN_RATIO > evmax) &&
- (evmag > MIN_EIGEN_MAG) && (evmag < MAX_EIGEN_MAG);
-
- return eigen_pass;
-}
-
-// Kasa sphere fitting with normal equation
-int magKasaFit(struct KasaFit *kasa, struct Vec3 *bias, float *radius) {
- // A * out = b
- // (4 x 4) (4 x 1) (4 x 1)
- struct Mat44 A;
- A.elem[0][0] = kasa->acc_xx;
- A.elem[0][1] = kasa->acc_xy;
- A.elem[0][2] = kasa->acc_xz;
- A.elem[0][3] = kasa->acc_x;
- A.elem[1][0] = kasa->acc_xy;
- A.elem[1][1] = kasa->acc_yy;
- A.elem[1][2] = kasa->acc_yz;
- A.elem[1][3] = kasa->acc_y;
- A.elem[2][0] = kasa->acc_xz;
- A.elem[2][1] = kasa->acc_yz;
- A.elem[2][2] = kasa->acc_zz;
- A.elem[2][3] = kasa->acc_z;
- A.elem[3][0] = kasa->acc_x;
- A.elem[3][1] = kasa->acc_y;
- A.elem[3][2] = kasa->acc_z;
- A.elem[3][3] = 1.0f;
-
- struct Vec4 b;
- initVec4(&b, -kasa->acc_xw, -kasa->acc_yw, -kasa->acc_zw, -kasa->acc_w);
-
- struct Size4 pivot;
- mat44DecomposeLup(&A, &pivot);
-
- struct Vec4 out;
- mat44Solve(&A, &out, &b, &pivot);
-
- // sphere: (x - xc)^2 + (y - yc)^2 + (z - zc)^2 = r^2
- //
- // xc = -out[0] / 2, yc = -out[1] / 2, zc = -out[2] / 2
- // r = sqrt(xc^2 + yc^2 + zc^2 - out[3])
-
- struct Vec3 v;
- initVec3(&v, out.x, out.y, out.z);
- vec3ScalarMul(&v, -0.5f);
-
- float r_square = vec3Dot(&v, &v) - out.w;
- float r = (r_square > 0) ? sqrtf(r_square) : 0;
-
- initVec3(bias, v.x, v.y, v.z);
- *radius = r;
-
- int success = 0;
- if (r > MIN_FIT_MAG && r < MAX_FIT_MAG) {
- success = 1;
- }
-
- return success;
-}
-
-void magKasaReset(struct KasaFit *kasa) {
- kasa->acc_x = kasa->acc_y = kasa->acc_z = kasa->acc_w = 0.0f;
- kasa->acc_xx = kasa->acc_xy = kasa->acc_xz = kasa->acc_xw = 0.0f;
- kasa->acc_yy = kasa->acc_yz = kasa->acc_yw = 0.0f;
- kasa->acc_zz = kasa->acc_zw = 0.0f;
-
- kasa->nsamples = 0;
-}
-
-void magCalReset(struct MagCal *moc) {
- magKasaReset(&moc->kasa);
-#ifdef DIVERSITY_CHECK_ENABLED
- diversityCheckerReset(&moc->diversity_checker);
-#endif
- moc->start_time = 0;
- moc->kasa_batching = false;
-}
-
-static int moc_batch_complete(struct MagCal *moc, uint64_t sample_time_us) {
- int complete = 0;
-
- if ((sample_time_us - moc->start_time > moc->min_batch_window_in_micros) &&
- (moc->kasa.nsamples > MIN_BATCH_SIZE)) {
- complete = 1;
-
- } else if (sample_time_us - moc->start_time > MAX_BATCH_WINDOW) {
- // not enough samples collected in MAX_BATCH_WINDOW or too many
- // maximum distance violations detected.
- magCalReset(moc);
- }
-
- return complete;
-}
-
-void initKasa(struct KasaFit *kasa) {
- magKasaReset(kasa);
-}
-
-void initMagCal(struct MagCal *moc, float x_bias, float y_bias, float z_bias,
- float c00, float c01, float c02, float c10, float c11,
- float c12, float c20, float c21, float c22,
- uint32_t min_batch_window_in_micros
-#ifdef DIVERSITY_CHECK_ENABLED
- ,size_t min_num_diverse_vectors
- ,size_t max_num_max_distance
- ,float var_threshold
- ,float max_min_threshold
- ,float local_field
- ,float threshold_tuning_param
- ,float max_distance_tuning_param
-#endif
- ) {
- magCalReset(moc);
- moc->update_time = 0;
- moc->min_batch_window_in_micros = min_batch_window_in_micros;
- moc->radius = 0.0f;
-
- moc->x_bias = x_bias;
- moc->y_bias = y_bias;
- moc->z_bias = z_bias;
-
- moc->c00 = c00;
- moc->c01 = c01;
- moc->c02 = c02;
- moc->c10 = c10;
- moc->c11 = c11;
- moc->c12 = c12;
- moc->c20 = c20;
- moc->c21 = c21;
- moc->c22 = c22;
-
-#ifdef MAG_CAL_DEBUG_ENABLE
- moc->mag_dbg.mag_trigger_count = 0;
- moc->mag_dbg.kasa_count = 0;
-#endif
-
-#ifdef DIVERSITY_CHECK_ENABLED
- // Diversity Checker
- diversityCheckerInit(&moc->diversity_checker,
- min_num_diverse_vectors,
- max_num_max_distance,
- var_threshold,
- max_min_threshold,
- local_field,
- threshold_tuning_param,
- max_distance_tuning_param);
-#endif
-}
-
-void magCalDestroy(struct MagCal *moc) { (void)moc; }
-
-enum MagUpdate magCalUpdate(struct MagCal *moc, uint64_t sample_time_us,
- float x, float y, float z) {
- enum MagUpdate new_bias = NO_UPDATE;
-
-#ifdef DIVERSITY_CHECK_ENABLED
- // Diversity Checker Update.
- diversityCheckerUpdate(&moc->diversity_checker, x, y, z);
-#endif
-
- // 1. run accumulators
- float w = x * x + y * y + z * z;
-
- moc->kasa.acc_x += x;
- moc->kasa.acc_y += y;
- moc->kasa.acc_z += z;
- moc->kasa.acc_w += w;
-
- moc->kasa.acc_xx += x * x;
- moc->kasa.acc_xy += x * y;
- moc->kasa.acc_xz += x * z;
- moc->kasa.acc_xw += x * w;
-
- moc->kasa.acc_yy += y * y;
- moc->kasa.acc_yz += y * z;
- moc->kasa.acc_yw += y * w;
-
- moc->kasa.acc_zz += z * z;
- moc->kasa.acc_zw += z * w;
-
- if (++moc->kasa.nsamples == 1) {
- moc->start_time = sample_time_us;
- moc->kasa_batching = true;
- }
-
- // 2. batch has enough samples?
- if (moc_batch_complete(moc, sample_time_us)) {
- float inv = 1.0f / moc->kasa.nsamples;
-
- moc->kasa.acc_x *= inv;
- moc->kasa.acc_y *= inv;
- moc->kasa.acc_z *= inv;
- moc->kasa.acc_w *= inv;
-
- moc->kasa.acc_xx *= inv;
- moc->kasa.acc_xy *= inv;
- moc->kasa.acc_xz *= inv;
- moc->kasa.acc_xw *= inv;
-
- moc->kasa.acc_yy *= inv;
- moc->kasa.acc_yz *= inv;
- moc->kasa.acc_yw *= inv;
-
- moc->kasa.acc_zz *= inv;
- moc->kasa.acc_zw *= inv;
-
- // 3. eigen test
- if (moc_eigen_test(&moc->kasa)) {
- struct Vec3 bias;
- float radius;
- // 4. Kasa sphere fitting
- if (magKasaFit(&moc->kasa, &bias, &radius)) {
-
-#ifdef MAG_CAL_DEBUG_ENABLE
- moc->mag_dbg.kasa_count++;
- CAL_DEBUG_LOG("[MAG_CAL:KASA UPDATE] :,",
- "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %lu, %lu",
- CAL_ENCODE_FLOAT(bias.x, 3),
- CAL_ENCODE_FLOAT(bias.y, 3),
- CAL_ENCODE_FLOAT(bias.z, 3),
- CAL_ENCODE_FLOAT(radius, 3),
- (unsigned long int)moc->mag_dbg.kasa_count,
- (unsigned long int)moc->mag_dbg.mag_trigger_count);
-#endif
-
-#ifdef DIVERSITY_CHECK_ENABLED
- // Update the local field.
- diversityCheckerLocalFieldUpdate(&moc->diversity_checker,
- radius);
-
- // checking if data is diverse.
- if (diversityCheckerNormQuality(&moc->diversity_checker,
- bias.x,
- bias.y,
- bias.z) &&
- moc->diversity_checker.num_max_dist_violations
- <= MAX_DISTANCE_VIOLATIONS) {
-
- // DEBUG PRINT OUT.
-#ifdef MAG_CAL_DEBUG_ENABLE
- moc->mag_dbg.mag_trigger_count++;
-#ifdef DIVERSE_DEBUG_ENABLE
- moc->diversity_checker.diversity_dbg.new_trigger = 1;
- CAL_DEBUG_LOG("[MAG_CAL:BIAS UPDATE] :, ",
- "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%06d,"
- "%s%d.%03d, %s%d.%03d, %s%d.%03d, %zu, %s%d.%03d, "
- "%s%d.%03d, %lu, %lu, %llu, %s%d.%03d, %s%d.%03d, "
- "%s%d.%03d, %llu",
- CAL_ENCODE_FLOAT(bias.x, 3),
- CAL_ENCODE_FLOAT(bias.y, 3),
- CAL_ENCODE_FLOAT(bias.z, 3),
- CAL_ENCODE_FLOAT(radius, 3),
- CAL_ENCODE_FLOAT(
- moc->diversity_checker.diversity_dbg.var_log, 6),
- CAL_ENCODE_FLOAT(
- moc->diversity_checker.diversity_dbg.mean_log, 3),
- CAL_ENCODE_FLOAT(
- moc->diversity_checker.diversity_dbg.max_log, 3),
- CAL_ENCODE_FLOAT(
- moc->diversity_checker.diversity_dbg.min_log, 3),
- moc->diversity_checker.num_points,
- CAL_ENCODE_FLOAT(
- moc->diversity_checker.threshold, 3),
- CAL_ENCODE_FLOAT(
- moc->diversity_checker.max_distance, 3),
- (unsigned long int)moc->mag_dbg.mag_trigger_count,
- (unsigned long int)moc->mag_dbg.kasa_count,
- (unsigned long long int)sample_time_us,
- CAL_ENCODE_FLOAT(moc->x_bias, 3),
- CAL_ENCODE_FLOAT(moc->y_bias, 3),
- CAL_ENCODE_FLOAT(moc->z_bias, 3),
- (unsigned long long int)moc->update_time);
-#endif
-#endif
-#endif
- moc->x_bias = bias.x;
- moc->y_bias = bias.y;
- moc->z_bias = bias.z;
-
- moc->radius = radius;
- moc->update_time = sample_time_us;
-
- new_bias = UPDATE_BIAS;
-
-#ifdef DIVERSITY_CHECK_ENABLED
- }
-#endif
- }
- }
-
- // 5. reset for next batch
- magCalReset(moc);
- }
-
- return new_bias;
-}
-
-void magCalGetBias(struct MagCal *moc, float *x, float *y, float *z) {
- *x = moc->x_bias;
- *y = moc->y_bias;
- *z = moc->z_bias;
-}
-
-void magCalAddBias(struct MagCal *moc, float x, float y, float z) {
- moc->x_bias += x;
- moc->y_bias += y;
- moc->z_bias += z;
-}
-
-void magCalRemoveBias(struct MagCal *moc, float xi, float yi, float zi,
- float *xo, float *yo, float *zo) {
- *xo = xi - moc->x_bias;
- *yo = yi - moc->y_bias;
- *zo = zi - moc->z_bias;
-}
-
-void magCalSetSoftiron(struct MagCal *moc, float c00, float c01, float c02,
- float c10, float c11, float c12, float c20, float c21,
- float c22) {
- moc->c00 = c00;
- moc->c01 = c01;
- moc->c02 = c02;
- moc->c10 = c10;
- moc->c11 = c11;
- moc->c12 = c12;
- moc->c20 = c20;
- moc->c21 = c21;
- moc->c22 = c22;
-}
-
-void magCalRemoveSoftiron(struct MagCal *moc, float xi, float yi, float zi,
- float *xo, float *yo, float *zo) {
- *xo = moc->c00 * xi + moc->c01 * yi + moc->c02 * zi;
- *yo = moc->c10 * xi + moc->c11 * yi + moc->c12 * zi;
- *zo = moc->c20 * xi + moc->c21 * yi + moc->c22 * zi;
-}
-
-#if defined MAG_CAL_DEBUG_ENABLE && defined DIVERSE_DEBUG_ENABLE
-// This function prints every second sample parts of the dbg diverse_data_log,
-// which ensures that all the messages get printed into the log file.
-void magLogPrint(struct DiversityChecker* diverse_data, float temp) {
- // Sample counter.
- static size_t sample_counter = 0;
- const float* data_log_ptr =
- &diverse_data->diversity_dbg.diverse_data_log[0];
- if (diverse_data->diversity_dbg.new_trigger == 1) {
- sample_counter++;
- if (sample_counter == 2) {
- CAL_DEBUG_LOG("[MAG_CAL:MEMORY X] :,",
- "%lu, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d"
- ", %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, "
- "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, "
- "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, "
- "%s%d.%03d",
- (unsigned long int)diverse_data->
- diversity_dbg.diversity_count,
- CAL_ENCODE_FLOAT(data_log_ptr[0*3], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[1*3], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[2*3], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[3*3], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[4*3], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[5*3], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[6*3], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[7*3], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[8*3], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[9*3], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[10*3], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[11*3], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[12*3], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[13*3], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[14*3], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[15*3], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[16*3], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[17*3], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[18*3], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[19*3], 3),
- CAL_ENCODE_FLOAT(temp, 3));
- }
-
- if (sample_counter == 4) {
- CAL_DEBUG_LOG("[MAG_CAL:MEMORY Y] :,",
- "%lu, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d"
- ", %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, "
- "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, "
- "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, ",
- (unsigned long int)diverse_data->
- diversity_dbg.diversity_count,
- CAL_ENCODE_FLOAT(data_log_ptr[0*3+1], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[1*3+1], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[2*3+1], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[3*3+1], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[4*3+1], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[5*3+1], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[6*3+1], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[7*3+1], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[8*3+1], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[9*3+1], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[10*3+1], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[11*3+1], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[12*3+1], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[13*3+1], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[14*3+1], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[15*3+1], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[16*3+1], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[17*3+1], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[18*3+1], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[19*3+1], 3));
- }
- if (sample_counter == 6) {
- CAL_DEBUG_LOG("[MAG_CAL:MEMORY Z] :,",
- "%lu, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d"
- ", %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, "
- "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, "
- "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, ",
- (unsigned long int)diverse_data->
- diversity_dbg.diversity_count,
- CAL_ENCODE_FLOAT(data_log_ptr[0*3+2], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[1*3+2], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[2*3+2], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[3*3+2], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[4*3+2], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[5*3+2], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[6*3+2], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[7*3+2], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[8*3+2], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[9*3+2], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[10*3+2], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[11*3+2], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[12*3+2], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[13*3+2], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[14*3+2], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[15*3+2], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[16*3+2], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[17*3+2], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[18*3+2], 3),
- CAL_ENCODE_FLOAT(data_log_ptr[19*3+2], 3));
- sample_counter = 0;
- diverse_data->diversity_dbg.new_trigger = 0;
- }
- }
-}
-#endif
diff --git a/firmware/os/algos/calibration/magnetometer/mag_cal.h b/firmware/os/algos/calibration/magnetometer/mag_cal.h
deleted file mode 100644
index 8c9da6f..0000000
--- a/firmware/os/algos/calibration/magnetometer/mag_cal.h
+++ /dev/null
@@ -1,127 +0,0 @@
-/*
- * Copyright (C) 2016 The Android Open Source Project
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_CAL_H_
-#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_CAL_H_
-
-#ifdef SPHERE_FIT_ENABLED
-#ifndef DIVERSITY_CHECK_ENABLED
-#define DIVERSITY_CHECK_ENABLED
-#endif
-#endif
-
-#include <stdbool.h>
-#include <stdint.h>
-#include <sys/types.h>
-#ifdef DIVERSITY_CHECK_ENABLED
-#include "calibration/common/diversity_checker.h"
-#endif
-#include "common/math/mat.h"
-#include "common/math/vec.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-struct KasaFit {
- float acc_x, acc_y, acc_z, acc_w;
- float acc_xx, acc_xy, acc_xz, acc_xw;
- float acc_yy, acc_yz, acc_yw, acc_zz, acc_zw;
- size_t nsamples;
-};
-
-enum MagUpdate {
- NO_UPDATE = 0x00,
- UPDATE_BIAS = 0x01,
- UPDATE_SPHERE_FIT = 0x02,
-};
-
-#ifdef MAG_CAL_DEBUG_ENABLE
-struct MagDbg {
- uint32_t mag_trigger_count;
- uint32_t kasa_count;
-};
-#endif
-
-struct MagCal {
-#ifdef DIVERSITY_CHECK_ENABLED
- struct DiversityChecker diversity_checker;
-#endif
- struct KasaFit kasa;
-
- uint64_t start_time;
- uint64_t update_time;
- uint32_t min_batch_window_in_micros;
- float x_bias, y_bias, z_bias;
- float radius;
- bool kasa_batching;
- float c00, c01, c02, c10, c11, c12, c20, c21, c22;
-
-#ifdef MAG_CAL_DEBUG_ENABLE
- struct MagDbg mag_dbg;
-#endif
-};
-
-void initKasa(struct KasaFit *kasa);
-
-#ifdef DIVERSITY_CHECK_ENABLED
-void initMagCal(struct MagCal *moc, float x_bias, float y_bias, float z_bias,
- float c00, float c01, float c02, float c10, float c11,
- float c12, float c20, float c21, float c22,
- uint32_t min_batch_window_in_micros,
- size_t min_num_diverse_vectors, size_t max_num_max_distance,
- float var_threshold, float max_min_threshold, float local_field,
- float threshold_tuning_param, float max_distance_tuning_param);
-#else
-void initMagCal(struct MagCal *moc, float x_bias, float y_bias, float z_bias,
- float c00, float c01, float c02, float c10, float c11,
- float c12, float c20, float c21, float c22,
- uint32_t min_batch_window_in_micros);
-#endif
-
-void magCalDestroy(struct MagCal *moc);
-
-enum MagUpdate magCalUpdate(struct MagCal *moc, uint64_t sample_time_us,
- float x, float y, float z);
-
-void magCalGetBias(struct MagCal *moc, float *x, float *y, float *z);
-
-void magCalAddBias(struct MagCal *moc, float x, float y, float z);
-
-void magCalRemoveBias(struct MagCal *moc, float xi, float yi, float zi,
- float *xo, float *yo, float *zo);
-
-void magCalSetSoftiron(struct MagCal *moc, float c00, float c01, float c02,
- float c10, float c11, float c12, float c20, float c21,
- float c22);
-
-void magCalRemoveSoftiron(struct MagCal *moc, float xi, float yi, float zi,
- float *xo, float *yo, float *zo);
-
-void magKasaReset(struct KasaFit *kasa);
-
-void magCalReset(struct MagCal *moc);
-
-int magKasaFit(struct KasaFit *kasa, struct Vec3 *bias, float *radius);
-
-#if defined MAG_CAL_DEBUG_ENABLE && defined DIVERSITY_CHECK_ENABLED
-void magLogPrint(struct DiversityChecker *moc, float temp);
-#endif
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_CAL_H_
diff --git a/firmware/os/algos/calibration/magnetometer/mag_cal/mag_cal.c b/firmware/os/algos/calibration/magnetometer/mag_cal/mag_cal.c
new file mode 100644
index 0000000..c3f12ae
--- /dev/null
+++ b/firmware/os/algos/calibration/magnetometer/mag_cal/mag_cal.c
@@ -0,0 +1,358 @@
+/*
+ * Copyright (C) 2016 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "calibration/magnetometer/mag_cal/mag_cal.h"
+
+#include <errno.h>
+#include <inttypes.h>
+#include <string.h>
+
+#include "calibration/util/cal_log.h"
+
+// Local helper macro for printing log messages.
+#ifdef MAG_CAL_DEBUG_ENABLE
+#ifdef CAL_NO_FLOAT_FORMAT_STRINGS
+#define CAL_FORMAT_MAG_MEMORY \
+ "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, " \
+ "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, " \
+ "%s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, %s%d.%03d, " \
+ "%s%d.%03d, %s%d.%03d"
+#else
+#define CAL_FORMAT_MAG_MEMORY \
+ "%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, " \
+ "%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f"
+#endif // CAL_NO_FLOAT_FORMAT_STRINGS
+#endif // MAG_CAL_DEBUG_ENABLE
+
+// clang-format off
+#define MAX_EIGEN_RATIO 15.0f
+#define MAX_EIGEN_MAG 70.0f // uT
+#define MIN_EIGEN_MAG 20.0f // uT
+#define MAX_FIT_MAG 70.0f
+#define MIN_FIT_MAG 20.0f
+#define MAX_BATCH_WINDOW 15000000UL // 15 sec
+#define MIN_BATCH_SIZE 25 // samples
+#define MAX_DISTANCE_VIOLATIONS 2
+// clang-format
+
+// eigen value magnitude and ratio test.
+static int moc_eigen_test(struct KasaFit *kasa) {
+ // covariance matrix.
+ struct Mat33 S;
+ S.elem[0][0] = kasa->acc_xx - kasa->acc_x * kasa->acc_x;
+ S.elem[0][1] = S.elem[1][0] = kasa->acc_xy - kasa->acc_x * kasa->acc_y;
+ S.elem[0][2] = S.elem[2][0] = kasa->acc_xz - kasa->acc_x * kasa->acc_z;
+ S.elem[1][1] = kasa->acc_yy - kasa->acc_y * kasa->acc_y;
+ S.elem[1][2] = S.elem[2][1] = kasa->acc_yz - kasa->acc_y * kasa->acc_z;
+ S.elem[2][2] = kasa->acc_zz - kasa->acc_z * kasa->acc_z;
+
+ struct Vec3 eigenvals;
+ struct Mat33 eigenvecs;
+ mat33GetEigenbasis(&S, &eigenvals, &eigenvecs);
+
+ float evmax = (eigenvals.x > eigenvals.y) ? eigenvals.x : eigenvals.y;
+ evmax = (eigenvals.z > evmax) ? eigenvals.z : evmax;
+
+ float evmin = (eigenvals.x < eigenvals.y) ? eigenvals.x : eigenvals.y;
+ evmin = (eigenvals.z < evmin) ? eigenvals.z : evmin;
+
+ float eigenvals_sum = eigenvals.x + eigenvals.y + eigenvals.z;
+
+ // Testing for negative number.
+ float evmag = (eigenvals_sum > 0) ? sqrtf(eigenvals_sum) : 0;
+
+ int eigen_pass = (evmin * MAX_EIGEN_RATIO > evmax) &&
+ (evmag > MIN_EIGEN_MAG) && (evmag < MAX_EIGEN_MAG);
+
+ return eigen_pass;
+}
+
+void magCalReset(struct MagCal *moc) {
+ kasaReset(&moc->kasa);
+ diversityCheckerReset(&moc->diversity_checker);
+ moc->start_time = 0;
+ moc->kasa_batching = false;
+}
+
+static bool moc_batch_complete(struct MagCal *moc, uint64_t sample_time_us) {
+ bool complete = false;
+
+ if ((sample_time_us - moc->start_time > moc->min_batch_window_in_micros) &&
+ (moc->kasa.nsamples > MIN_BATCH_SIZE)) {
+ complete = true;
+
+ } else if (sample_time_us - moc->start_time > MAX_BATCH_WINDOW) {
+ // not enough samples collected in MAX_BATCH_WINDOW or too many
+ // maximum distance violations detected.
+ magCalReset(moc);
+ }
+
+ return complete;
+}
+
+void initMagCal(struct MagCal *moc,
+ const struct MagCalParameters *mag_cal_parameters,
+ const struct DiversityCheckerParameters *diverse_parameters) {
+ magCalReset(moc);
+ moc->update_time = 0;
+ moc->min_batch_window_in_micros =
+ mag_cal_parameters->min_batch_window_in_micros;
+ moc->radius = 0.0f;
+
+ moc->x_bias = mag_cal_parameters->x_bias;
+ moc->y_bias = mag_cal_parameters->y_bias;
+ moc->z_bias = mag_cal_parameters->z_bias;
+
+ moc->c00 = mag_cal_parameters->c00;
+ moc->c01 = mag_cal_parameters->c01;
+ moc->c02 = mag_cal_parameters->c02;
+ moc->c10 = mag_cal_parameters->c10;
+ moc->c11 = mag_cal_parameters->c11;
+ moc->c12 = mag_cal_parameters->c12;
+ moc->c20 = mag_cal_parameters->c20;
+ moc->c21 = mag_cal_parameters->c21;
+ moc->c22 = mag_cal_parameters->c22;
+
+#ifdef MAG_CAL_DEBUG_ENABLE
+ moc->mag_dbg.mag_trigger_count = 0;
+ moc->mag_dbg.kasa_count = 0;
+#endif // MAG_CAL_DEBUG_ENABLE
+
+ // Diversity Checker
+ diversityCheckerInit(&moc->diversity_checker, diverse_parameters);
+}
+
+void magCalDestroy(struct MagCal *moc) { (void)moc; }
+
+enum MagUpdate magCalUpdate(struct MagCal *moc, uint64_t sample_time_us,
+ float x, float y, float z) {
+ enum MagUpdate new_bias = NO_UPDATE;
+
+ // Diversity Checker Update.
+ diversityCheckerUpdate(&moc->diversity_checker, x, y, z);
+
+ // 1. run accumulators
+ kasaAccumulate(&moc->kasa, x, y, z);
+
+ if (moc->kasa.nsamples == 1) {
+ moc->start_time = sample_time_us;
+ moc->kasa_batching = true;
+ }
+
+ // 2. batch has enough samples?
+ if (moc_batch_complete(moc, sample_time_us)) {
+ kasaNormalize(&moc->kasa);
+
+ // 3. eigen test
+ if (moc_eigen_test(&moc->kasa)) {
+ struct Vec3 bias;
+ float radius;
+ // 4. Kasa sphere fitting
+ if (kasaFit(&moc->kasa, &bias, &radius, MAX_FIT_MAG, MIN_FIT_MAG)) {
+#ifdef MAG_CAL_DEBUG_ENABLE
+ moc->mag_dbg.kasa_count++;
+ CAL_DEBUG_LOG("[MAG_CAL:KASA UPDATE] :", CAL_FORMAT_3DIGITS_TRIPLET
+ ", " CAL_FORMAT_3DIGITS ", %" PRIu32 ", %" PRIu32,
+ CAL_ENCODE_FLOAT(bias.x, 3), CAL_ENCODE_FLOAT(bias.y, 3),
+ CAL_ENCODE_FLOAT(bias.z, 3), CAL_ENCODE_FLOAT(radius, 3),
+ moc->mag_dbg.kasa_count, moc->mag_dbg.mag_trigger_count);
+#endif // MAG_CAL_DEBUG_ENABLE
+
+ // Update the local field.
+ diversityCheckerLocalFieldUpdate(&moc->diversity_checker, radius);
+
+ // checking if data is diverse.
+ if (diversityCheckerNormQuality(&moc->diversity_checker, bias.x, bias.y,
+ bias.z) &&
+ moc->diversity_checker.num_max_dist_violations <=
+ MAX_DISTANCE_VIOLATIONS) {
+ // DEBUG PRINT OUT.
+#ifdef MAG_CAL_DEBUG_ENABLE
+ moc->mag_dbg.mag_trigger_count++;
+ moc->diversity_checker.diversity_dbg.new_trigger = 1;
+ CAL_DEBUG_LOG(
+ "[MAG_CAL:BIAS UPDATE] :", CAL_FORMAT_3DIGITS_TRIPLET ", "
+ CAL_FORMAT_3DIGITS ", " CAL_FORMAT_6DIGITS ", "
+ CAL_FORMAT_3DIGITS_TRIPLET ", %zu, " CAL_FORMAT_3DIGITS ", "
+ CAL_FORMAT_3DIGITS ", %" PRIu32 ", %" PRIu32 ", %" PRIu64 ", "
+ CAL_FORMAT_3DIGITS_TRIPLET ", %" PRIu64 "",
+ CAL_ENCODE_FLOAT(bias.x, 3), CAL_ENCODE_FLOAT(bias.y, 3),
+ CAL_ENCODE_FLOAT(bias.z, 3), CAL_ENCODE_FLOAT(radius, 3),
+ CAL_ENCODE_FLOAT(moc->diversity_checker.diversity_dbg.var_log, 6),
+ CAL_ENCODE_FLOAT(moc->diversity_checker.diversity_dbg.mean_log,
+ 3),
+ CAL_ENCODE_FLOAT(moc->diversity_checker.diversity_dbg.max_log, 3),
+ CAL_ENCODE_FLOAT(moc->diversity_checker.diversity_dbg.min_log, 3),
+ moc->diversity_checker.num_points,
+ CAL_ENCODE_FLOAT(moc->diversity_checker.threshold, 3),
+ CAL_ENCODE_FLOAT(moc->diversity_checker.max_distance, 3),
+ moc->mag_dbg.mag_trigger_count,
+ moc->mag_dbg.kasa_count,
+ sample_time_us,
+ CAL_ENCODE_FLOAT(moc->x_bias, 3),
+ CAL_ENCODE_FLOAT(moc->y_bias, 3),
+ CAL_ENCODE_FLOAT(moc->z_bias, 3),
+ moc->update_time);
+#endif // MAG_CAL_DEBUG_ENABLE
+ moc->x_bias = bias.x;
+ moc->y_bias = bias.y;
+ moc->z_bias = bias.z;
+
+ moc->radius = radius;
+ moc->update_time = sample_time_us;
+
+ new_bias = UPDATE_BIAS;
+ }
+ }
+ }
+ // 5. reset for next batch
+ magCalReset(moc);
+ }
+
+ return new_bias;
+}
+
+void magCalGetBias(const struct MagCal *moc, float *x, float *y, float *z) {
+ *x = moc->x_bias;
+ *y = moc->y_bias;
+ *z = moc->z_bias;
+}
+
+void magCalAddBias(struct MagCal *moc, float x, float y, float z) {
+ moc->x_bias += x;
+ moc->y_bias += y;
+ moc->z_bias += z;
+}
+
+void magCalRemoveBias(struct MagCal *moc, float xi, float yi, float zi,
+ float *xo, float *yo, float *zo) {
+ *xo = xi - moc->x_bias;
+ *yo = yi - moc->y_bias;
+ *zo = zi - moc->z_bias;
+}
+
+void magCalSetSoftiron(struct MagCal *moc, float c00, float c01, float c02,
+ float c10, float c11, float c12, float c20, float c21,
+ float c22) {
+ moc->c00 = c00;
+ moc->c01 = c01;
+ moc->c02 = c02;
+ moc->c10 = c10;
+ moc->c11 = c11;
+ moc->c12 = c12;
+ moc->c20 = c20;
+ moc->c21 = c21;
+ moc->c22 = c22;
+}
+
+void magCalRemoveSoftiron(struct MagCal *moc, float xi, float yi, float zi,
+ float *xo, float *yo, float *zo) {
+ *xo = moc->c00 * xi + moc->c01 * yi + moc->c02 * zi;
+ *yo = moc->c10 * xi + moc->c11 * yi + moc->c12 * zi;
+ *zo = moc->c20 * xi + moc->c21 * yi + moc->c22 * zi;
+}
+
+#if defined MAG_CAL_DEBUG_ENABLE
+// This function prints every second sample parts of the dbg diverse_data_log,
+// which ensures that all the messages get printed into the log file.
+void magLogPrint(struct DiversityChecker *diverse_data, float temp) {
+ // Sample counter.
+ static size_t sample_counter = 0;
+ const float *data_log_ptr = &diverse_data->diversity_dbg.diverse_data_log[0];
+ if (diverse_data->diversity_dbg.new_trigger == 1) {
+ sample_counter++;
+ if (sample_counter == 2) {
+ CAL_DEBUG_LOG(
+ "[MAG_CAL:MEMORY X] :", "%" PRIu32 ", " CAL_FORMAT_MAG_MEMORY ", "
+ CAL_FORMAT_3DIGITS,
+ diverse_data->diversity_dbg.diversity_count,
+ CAL_ENCODE_FLOAT(data_log_ptr[0 * 3], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[1 * 3], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[2 * 3], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[3 * 3], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[4 * 3], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[5 * 3], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[6 * 3], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[7 * 3], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[8 * 3], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[9 * 3], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[10 * 3], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[11 * 3], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[12 * 3], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[13 * 3], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[14 * 3], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[15 * 3], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[16 * 3], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[17 * 3], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[18 * 3], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[19 * 3], 3), CAL_ENCODE_FLOAT(temp, 3));
+ }
+
+ if (sample_counter == 4) {
+ CAL_DEBUG_LOG(
+ "[MAG_CAL:MEMORY Y] :", "%" PRIu32 ", " CAL_FORMAT_MAG_MEMORY,
+ diverse_data->diversity_dbg.diversity_count,
+ CAL_ENCODE_FLOAT(data_log_ptr[0 * 3 + 1], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[1 * 3 + 1], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[2 * 3 + 1], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[3 * 3 + 1], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[4 * 3 + 1], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[5 * 3 + 1], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[6 * 3 + 1], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[7 * 3 + 1], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[8 * 3 + 1], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[9 * 3 + 1], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[10 * 3 + 1], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[11 * 3 + 1], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[12 * 3 + 1], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[13 * 3 + 1], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[14 * 3 + 1], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[15 * 3 + 1], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[16 * 3 + 1], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[17 * 3 + 1], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[18 * 3 + 1], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[19 * 3 + 1], 3));
+ }
+ if (sample_counter == 6) {
+ CAL_DEBUG_LOG(
+ "[MAG_CAL:MEMORY Z] :", "%" PRIu32 ", " CAL_FORMAT_MAG_MEMORY,
+ diverse_data->diversity_dbg.diversity_count,
+ CAL_ENCODE_FLOAT(data_log_ptr[0 * 3 + 2], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[1 * 3 + 2], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[2 * 3 + 2], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[3 * 3 + 2], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[4 * 3 + 2], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[5 * 3 + 2], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[6 * 3 + 2], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[7 * 3 + 2], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[8 * 3 + 2], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[9 * 3 + 2], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[10 * 3 + 2], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[11 * 3 + 2], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[12 * 3 + 2], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[13 * 3 + 2], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[14 * 3 + 2], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[15 * 3 + 2], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[16 * 3 + 2], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[17 * 3 + 2], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[18 * 3 + 2], 3),
+ CAL_ENCODE_FLOAT(data_log_ptr[19 * 3 + 2], 3));
+ sample_counter = 0;
+ diverse_data->diversity_dbg.new_trigger = 0;
+ }
+ }
+}
+#endif // MAG_CAL_DEBUG_ENABLE
diff --git a/firmware/os/algos/calibration/magnetometer/mag_cal/mag_cal.h b/firmware/os/algos/calibration/magnetometer/mag_cal/mag_cal.h
new file mode 100644
index 0000000..566afa8
--- /dev/null
+++ b/firmware/os/algos/calibration/magnetometer/mag_cal/mag_cal.h
@@ -0,0 +1,116 @@
+/*
+ * Copyright (C) 2016 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_CAL_MAG_CAL_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_CAL_MAG_CAL_H_
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <sys/types.h>
+
+#include "calibration/diversity_checker/diversity_checker.h"
+#include "common/math/kasa.h"
+#include "common/math/mat.h"
+#include "common/math/vec.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+enum MagUpdate {
+ NO_UPDATE = 0x00,
+ UPDATE_BIAS = 0x01,
+ UPDATE_SPHERE_FIT = 0x02,
+ UPDATE_BIAS_MAGGYRO_MEDIUM = 0x04,
+ UPDATE_BIAS_MAGGYRO_HIGH = 0x08,
+ MAGGYRO_TIMEOUT = 0x10,
+};
+
+#ifdef MAG_CAL_DEBUG_ENABLE
+struct MagDbg {
+ uint32_t mag_trigger_count;
+ uint32_t kasa_count;
+};
+#endif
+
+// MagCal algorithm parameters (see MagCal for details).
+struct MagCalParameters {
+ uint32_t min_batch_window_in_micros;
+ float x_bias; // [micro-Tesla]
+ float y_bias; // [micro-Tesla]
+ float z_bias; // [micro-Tesla]
+ float c00;
+ float c01;
+ float c02;
+ float c10;
+ float c11;
+ float c12;
+ float c20;
+ float c21;
+ float c22;
+};
+
+struct MagCal {
+ struct DiversityChecker diversity_checker;
+ struct KasaFit kasa;
+
+ uint64_t start_time; // [micro-seconds]
+ uint64_t update_time; // [micro-seconds]
+ uint32_t min_batch_window_in_micros;
+ float x_bias, y_bias, z_bias;
+ float radius; // [micro-Tesla]
+ bool kasa_batching;
+ float c00, c01, c02, c10, c11, c12, c20, c21, c22;
+
+#ifdef MAG_CAL_DEBUG_ENABLE
+ struct MagDbg mag_dbg;
+#endif
+};
+
+void initMagCal(struct MagCal *moc,
+ const struct MagCalParameters *mag_cal_parameters,
+ const struct DiversityCheckerParameters *diverse_parameters);
+
+void magCalDestroy(struct MagCal *moc);
+
+enum MagUpdate magCalUpdate(struct MagCal *moc, uint64_t sample_time_us,
+ float x, float y, float z);
+
+void magCalGetBias(const struct MagCal *moc, float *x, float *y, float *z);
+
+void magCalAddBias(struct MagCal *moc, float x, float y, float z);
+
+void magCalRemoveBias(struct MagCal *moc, float xi, float yi, float zi,
+ float *xo, float *yo, float *zo);
+
+void magCalSetSoftiron(struct MagCal *moc, float c00, float c01, float c02,
+ float c10, float c11, float c12, float c20, float c21,
+ float c22);
+
+void magCalRemoveSoftiron(struct MagCal *moc, float xi, float yi, float zi,
+ float *xo, float *yo, float *zo);
+
+void magCalReset(struct MagCal *moc);
+
+#if defined MAG_CAL_DEBUG_ENABLE
+void magLogPrint(struct DiversityChecker *moc, float temp);
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_CAL_MAG_CAL_H_
diff --git a/firmware/os/algos/calibration/magnetometer/mag_sphere_fit.c b/firmware/os/algos/calibration/magnetometer/mag_sphere_fit_cal/mag_sphere_fit.c
similarity index 80%
rename from firmware/os/algos/calibration/magnetometer/mag_sphere_fit.c
rename to firmware/os/algos/calibration/magnetometer/mag_sphere_fit_cal/mag_sphere_fit.c
index eb23b87..93c2ac6 100644
--- a/firmware/os/algos/calibration/magnetometer/mag_sphere_fit.c
+++ b/firmware/os/algos/calibration/magnetometer/mag_sphere_fit_cal/mag_sphere_fit.c
@@ -14,13 +14,11 @@
* limitations under the License.
*/
-#include "calibration/magnetometer/mag_sphere_fit.h"
+#include "calibration/magnetometer/mag_sphere_fit_cal/mag_sphere_fit.h"
#include <errno.h>
#include <string.h>
-#include "calibration/util/cal_log.h"
-
#define MAX_ITERATIONS 30
#define INITIAL_U_SCALE 1.0e-4f
#define GRADIENT_THRESHOLD 1.0e-16f
@@ -33,22 +31,16 @@
memset(&mocs->sphere_data, 0, sizeof(mocs->sphere_data));
}
-void initMagCalSphere(struct MagCalSphere *mocs, float x_bias, float y_bias,
- float z_bias, float c00, float c01, float c02, float c10,
- float c11, float c12, float c20, float c21, float c22,
- uint32_t min_batch_window_in_micros,
- size_t min_num_diverse_vectors,
- size_t max_num_max_distance, float var_threshold,
- float max_min_threshold, float local_field,
- float threshold_tuning_param,
- float max_distance_tuning_param) {
- initMagCal(&mocs->moc, x_bias, y_bias, z_bias, c00, c01, c02, c10, c11, c12,
- c20, c21, c22, min_batch_window_in_micros, min_num_diverse_vectors,
- max_num_max_distance, var_threshold, max_min_threshold,
- local_field, threshold_tuning_param, max_distance_tuning_param);
+void initMagCalSphere(
+ struct MagCalSphere *mocs,
+ const struct MagCalParameters *mag_cal_parameters,
+ const struct DiversityCheckerParameters *diverse_parameters,
+ float default_odr_in_hz) {
+ initMagCal(&mocs->moc, mag_cal_parameters, diverse_parameters);
mocs->inv_data_size = 1.0f / (float)NUM_SPHERE_FIT_DATA;
mocs->batch_time_in_sec =
- (float)(min_batch_window_in_micros) * FROM_MICRO_SEC_TO_SEC;
+ (float)(mag_cal_parameters->min_batch_window_in_micros) *
+ FROM_MICRO_SEC_TO_SEC;
// Initialize to take every sample, default setting.
mocs->sample_drop = 0;
magCalSphereReset(mocs);
@@ -63,6 +55,9 @@
sphereFitSetSolverData(&mocs->sphere_fit.sphere_cal,
&mocs->sphere_fit.lm_data);
calDataReset(&mocs->sphere_fit.sphere_param);
+
+ // Initializes the starting output data rate estimate.
+ magCalSphereOdrUpdate(mocs, default_odr_in_hz);
}
void magCalSphereDestroy(struct MagCalSphere *mocs) { (void)mocs; }
@@ -91,7 +86,7 @@
if (mocs->number_of_data_samples < NUM_SPHERE_FIT_DATA) {
memcpy(&mocs->sphere_data[mocs->number_of_data_samples *
THREE_AXIS_DATA_DIM],
- vec, sizeof(float) * 3);
+ vec, sizeof(float) * THREE_AXIS_DATA_DIM);
// counting the numbers of samples in the data set.
mocs->number_of_data_samples++;
}
@@ -114,8 +109,7 @@
// Running the sphere fit and checking if successful.
if (sphereFitRunCal(&mocs->sphere_fit.sphere_cal, &data, sample_time_us)) {
- // Updating Sphere parameters. Can use "calDataCorrectData" function to
- // correct data.
+ // Updating sphere parameters.
sphereFitGetLatestCal(&mocs->sphere_fit.sphere_cal,
&mocs->sphere_fit.sphere_param);
diff --git a/firmware/os/algos/calibration/magnetometer/mag_sphere_fit.h b/firmware/os/algos/calibration/magnetometer/mag_sphere_fit_cal/mag_sphere_fit.h
similarity index 63%
rename from firmware/os/algos/calibration/magnetometer/mag_sphere_fit.h
rename to firmware/os/algos/calibration/magnetometer/mag_sphere_fit_cal/mag_sphere_fit.h
index 85df48f..3ae4687 100644
--- a/firmware/os/algos/calibration/magnetometer/mag_sphere_fit.h
+++ b/firmware/os/algos/calibration/magnetometer/mag_sphere_fit_cal/mag_sphere_fit.h
@@ -13,11 +13,12 @@
* See the License for the specific language governing permissions and
* limitations under the License.
*/
-#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_SPHERE_FIT_H_
-#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_SPHERE_FIT_H_
-#include "calibration/common/sphere_fit_calibration.h"
-#include "calibration/magnetometer/mag_cal.h"
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_SPHERE_FIT_CAL_MAG_SPHERE_FIT_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_SPHERE_FIT_CAL_MAG_SPHERE_FIT_H_
+
+#include "calibration/magnetometer/mag_cal/mag_cal.h"
+#include "calibration/sphere_fit/sphere_fit_calibration.h"
#define NUM_SPHERE_FIT_DATA 50
@@ -50,24 +51,17 @@
float sphere_data[THREE_AXIS_DATA_DIM * NUM_SPHERE_FIT_DATA];
};
-void initMagCalSphere(struct MagCalSphere *mocs,
- float x_bias, float y_bias, float z_bias,
- float c00, float c01, float c02, float c10, float c11,
- float c12, float c20, float c21, float c22,
- uint32_t min_batch_window_in_micros,
- size_t min_num_diverse_vectors,
- size_t max_num_max_distance,
- float var_threshold,
- float max_min_threshold,
- float local_field,
- float threshold_tuning_param,
- float max_distance_tuning_param);
+void initMagCalSphere(
+ struct MagCalSphere *mocs,
+ const struct MagCalParameters *mag_cal_parameters,
+ const struct DiversityCheckerParameters *diverse_parameters,
+ float default_odr_in_hz);
void magCalSphereDestroy(struct MagCalSphere *mocs);
enum MagUpdate magCalSphereUpdate(struct MagCalSphere *mocs,
- uint64_t sample_time_us,
- float x, float y, float z);
+ uint64_t sample_time_us, float x, float y,
+ float z);
void magCalSphereOdrUpdate(struct MagCalSphere *mocs, float odr_in_hz);
@@ -75,4 +69,4 @@
}
#endif
-#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_SPHERE_FIT_H_
+#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_MAGNETOMETER_MAG_SPHERE_FIT_CAL_MAG_SPHERE_FIT_H_
diff --git a/firmware/os/algos/calibration/nano_calibration/aosp_nano_cal_parameters.h b/firmware/os/algos/calibration/nano_calibration/aosp_nano_cal_parameters.h
new file mode 100644
index 0000000..12b8470
--- /dev/null
+++ b/firmware/os/algos/calibration/nano_calibration/aosp_nano_cal_parameters.h
@@ -0,0 +1,125 @@
+/*
+ * Copyright (C) 2018 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_NANO_CALIBRATION_AOSP_NANO_CAL_PARAMETERS_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_NANO_CALIBRATION_AOSP_NANO_CAL_PARAMETERS_H_
+
+#ifdef ACCEL_CAL_ENABLED
+#include "calibration/accelerometer/accel_cal.h"
+#endif // ACCEL_CAL_ENABLED
+
+#ifdef GYRO_CAL_ENABLED
+#include "calibration/gyroscope/gyro_cal.h"
+#include "calibration/over_temp/over_temp_cal.h"
+#endif // GYRO_CAL_ENABLED
+
+#ifdef MAG_CAL_ENABLED
+#include "calibration/diversity_checker/diversity_checker.h"
+#include "calibration/magnetometer/mag_cal/mag_cal.h"
+#endif // MAG_CAL_ENABLED
+
+#include "common/math/macros.h"
+
+namespace nano_calibration {
+
+//////// ACCELEROMETER CALIBRATION ////////
+#ifdef ACCEL_CAL_ENABLED
+constexpr AccelCalParameters kAccelCalParameters{
+ MSEC_TO_NANOS(800), // t0
+ 5, // n_s
+ 15, // fx
+ 15, // fxb
+ 15, // fy
+ 15, // fyb
+ 15, // fz
+ 15, // fzb
+ 15, // fle
+ 0.00025f // th
+};
+#endif // ACCEL_CAL_ENABLED
+
+//////// GYROSCOPE CALIBRATION ////////
+#ifdef GYRO_CAL_ENABLED
+constexpr GyroCalParameters kGyroCalParameters{
+ SEC_TO_NANOS(1.4), // min_still_duration_nanos
+ SEC_TO_NANOS(1.4), // max_still_duration_nanos [see, NOTE 1]
+ 0, // calibration_time_nanos
+ MSEC_TO_NANOS(500), // window_time_duration_nanos
+ 0, // bias_x
+ 0, // bias_y
+ 0, // bias_z
+ 0.95f, // stillness_threshold
+ MDEG_TO_RAD * 60.0f, // stillness_mean_delta_limit [rad/sec]
+ 3.0e-5f, // gyro_var_threshold [rad/sec]^2
+ 3.0e-6f, // gyro_confidence_delta [rad/sec]^2
+ 4.5e-3f, // accel_var_threshold [m/sec^2]^2
+ 9.0e-4f, // accel_confidence_delta [m/sec^2]^2
+ 5.0f, // mag_var_threshold [uTesla]^2
+ 1.0f, // mag_confidence_delta [uTesla]^2
+ 1.5f, // temperature_delta_limit_celsius
+ true // gyro_calibration_enable
+};
+// [NOTE 1]: 'max_still_duration_nanos' is set to 1.4 seconds to achieve a max
+// stillness period of 1.5 seconds and avoid buffer boundary conditions that
+// could push the max stillness to the next multiple of the analysis window
+// length (i.e., 2.0 seconds).
+
+constexpr OverTempCalParameters kGyroOtcParameters{
+ MSEC_TO_NANOS(100), // min_temp_update_period_nanos
+ DAYS_TO_NANOS(2), // age_limit_nanos
+ 0.75f, // delta_temp_per_bin
+ 40.0f * MDEG_TO_RAD, // jump_tolerance
+ 100.0f * MDEG_TO_RAD, // outlier_limit
+ 250.0f * MDEG_TO_RAD, // temp_sensitivity_limit
+ 8.0e3f * MDEG_TO_RAD, // sensor_intercept_limit
+ 0.1f * MDEG_TO_RAD, // significant_offset_change
+ 5, // min_num_model_pts
+ true // over_temp_enable
+};
+#endif // GYRO_CAL_ENABLED
+
+//////// MAGNETOMETER CALIBRATION ////////
+#ifdef MAG_CAL_ENABLED
+constexpr MagCalParameters kMagCalParameters{
+ 3000000, // min_batch_window_in_micros
+ 0.0f, // x_bias
+ 0.0f, // y_bias
+ 0.0f, // z_bias
+ 1.0f, // c00
+ 0.0f, // c01
+ 0.0f, // c02
+ 0.0f, // c10
+ 1.0f, // c11
+ 0.0f, // c12
+ 0.0f, // c20
+ 0.0f, // c21
+ 1.0f // c22
+};
+
+constexpr DiversityCheckerParameters kMagDiversityParameters{
+ 6.0f, // var_threshold
+ 10.0f, // max_min_threshold
+ 48.0f, // local_field
+ 0.5f, // threshold_tuning_param
+ 2.552f, // max_distance_tuning_param
+ 8, // min_num_diverse_vectors
+ 1 // max_num_max_distance
+};
+#endif // MAG_CAL_ENABLED
+
+} // namespace nano_calibration
+
+#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_NANO_CALIBRATION_AOSP_NANO_CAL_PARAMETERS_H_
diff --git a/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc b/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc
new file mode 100644
index 0000000..0df2ae6
--- /dev/null
+++ b/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc
@@ -0,0 +1,416 @@
+/*
+ * Copyright (C) 2017 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "calibration/nano_calibration/nano_calibration.h"
+
+#include <cstring>
+
+#include "chre/util/nanoapp/log.h"
+
+namespace nano_calibration {
+namespace {
+
+using ::online_calibration::CalibrationDataThreeAxis;
+using ::online_calibration::CalibrationTypeFlags;
+using ::online_calibration::SensorData;
+using ::online_calibration::SensorIndex;
+using ::online_calibration::SensorType;
+
+// NanoSensorCal logging macros.
+#ifdef NANO_SENSOR_CAL_DBG_ENABLED
+#ifndef LOG_TAG
+#define LOG_TAG "[ImuCal]"
+#endif
+#define NANO_CAL_LOGD(tag, format, ...) LOGD("%s " format, tag, ##__VA_ARGS__)
+#define NANO_CAL_LOGI(tag, format, ...) LOGI("%s " format, tag, ##__VA_ARGS__)
+#define NANO_CAL_LOGW(tag, format, ...) LOGW("%s " format, tag, ##__VA_ARGS__)
+#define NANO_CAL_LOGE(tag, format, ...) LOGE("%s " format, tag, ##__VA_ARGS__)
+#else
+#define NANO_CAL_LOGD(tag, format, ...) CHRE_LOG_NULL(format, ##__VA_ARGS__)
+#define NANO_CAL_LOGI(tag, format, ...) CHRE_LOG_NULL(format, ##__VA_ARGS__)
+#define NANO_CAL_LOGW(tag, format, ...) CHRE_LOG_NULL(format, ##__VA_ARGS__)
+#define NANO_CAL_LOGE(tag, format, ...) CHRE_LOG_NULL(format, ##__VA_ARGS__)
+#endif // NANO_SENSOR_CAL_DBG_ENABLED
+
+} // namespace
+
+void NanoSensorCal::Initialize(OnlineCalibrationThreeAxis *accel_cal,
+ OnlineCalibrationThreeAxis *gyro_cal,
+ OnlineCalibrationThreeAxis *mag_cal) {
+ // Loads stored calibration data and initializes the calibration algorithms.
+ accel_cal_ = accel_cal;
+ if (accel_cal_ != nullptr) {
+ if (accel_cal_->get_sensor_type() == SensorType::kAccelerometerMps2) {
+ LoadAshCalibration(CHRE_SENSOR_TYPE_ACCELEROMETER, accel_cal_,
+ &accel_cal_update_flags_, kAccelTag);
+ NANO_CAL_LOGI(kAccelTag,
+ "Accelerometer runtime calibration initialized.");
+ } else {
+ accel_cal_ = nullptr;
+ NANO_CAL_LOGE(kAccelTag, "Failed to initialize: wrong sensor type.");
+ }
+ }
+
+ gyro_cal_ = gyro_cal;
+ if (gyro_cal_ != nullptr) {
+ if (gyro_cal_->get_sensor_type() == SensorType::kGyroscopeRps) {
+ LoadAshCalibration(CHRE_SENSOR_TYPE_GYROSCOPE, gyro_cal_,
+ &gyro_cal_update_flags_, kGyroTag);
+ NANO_CAL_LOGI(kGyroTag, "Gyroscope runtime calibration initialized.");
+ } else {
+ gyro_cal_ = nullptr;
+ NANO_CAL_LOGE(kGyroTag, "Failed to initialize: wrong sensor type.");
+ }
+ }
+
+ mag_cal_ = mag_cal;
+ if (mag_cal != nullptr) {
+ if (mag_cal->get_sensor_type() == SensorType::kMagnetometerUt) {
+ LoadAshCalibration(CHRE_SENSOR_TYPE_GEOMAGNETIC_FIELD, mag_cal_,
+ &mag_cal_update_flags_, kMagTag);
+ NANO_CAL_LOGI(kMagTag, "Magnetometer runtime calibration initialized.");
+ } else {
+ mag_cal_ = nullptr;
+ NANO_CAL_LOGE(kMagTag, "Failed to initialize: wrong sensor type.");
+ }
+ }
+}
+
+void NanoSensorCal::HandleSensorSamples(
+ uint16_t event_type, const chreSensorThreeAxisData *event_data) {
+ // Converts CHRE Event -> SensorData::SensorType.
+ SensorData sample;
+ switch (event_type) {
+ case CHRE_EVENT_SENSOR_UNCALIBRATED_ACCELEROMETER_DATA:
+ sample.type = SensorType::kAccelerometerMps2;
+ break;
+ case CHRE_EVENT_SENSOR_UNCALIBRATED_GYROSCOPE_DATA:
+ sample.type = SensorType::kGyroscopeRps;
+ break;
+ case CHRE_EVENT_SENSOR_UNCALIBRATED_GEOMAGNETIC_FIELD_DATA:
+ sample.type = SensorType::kMagnetometerUt;
+ break;
+ default:
+ // This sensor type is not used.
+ NANO_CAL_LOGW("[NanoSensorCal]",
+ "Unexpected 3-axis sensor type received.");
+ return;
+ }
+
+ // Sends the sensor payload to the calibration algorithms and checks for
+ // calibration updates.
+ const auto &header = event_data->header;
+ const auto *data = event_data->readings;
+ sample.timestamp_nanos = header.baseTimestamp;
+ for (size_t i = 0; i < header.readingCount; i++) {
+ sample.timestamp_nanos += data[i].timestampDelta;
+ memcpy(sample.data, data[i].v, sizeof(sample.data));
+ ProcessSample(sample);
+ }
+}
+
+void NanoSensorCal::HandleTemperatureSamples(
+ uint16_t event_type, const chreSensorFloatData *event_data) {
+ // Computes the mean of the batched temperature samples and delivers it to the
+ // calibration algorithms. Note, the temperature sensor batch size determines
+ // its minimum update interval.
+ if (event_type == CHRE_EVENT_SENSOR_ACCELEROMETER_TEMPERATURE_DATA &&
+ event_data->header.readingCount > 0) {
+ const auto header = event_data->header;
+ const auto *data = event_data->readings;
+
+ SensorData sample;
+ sample.type = SensorType::kTemperatureCelsius;
+ sample.timestamp_nanos = header.baseTimestamp;
+
+ float accum_temperature_celsius = 0.0f;
+ for (size_t i = 0; i < header.readingCount; i++) {
+ sample.timestamp_nanos += data[i].timestampDelta;
+ accum_temperature_celsius += data[i].value;
+ }
+ sample.data[SensorIndex::kSingleAxis] =
+ accum_temperature_celsius / header.readingCount;
+ ProcessSample(sample);
+ } else {
+ NANO_CAL_LOGW("[NanoSensorCal]",
+ "Unexpected single-axis sensor type received.");
+ }
+}
+
+void NanoSensorCal::ProcessSample(const SensorData &sample) {
+ // Sends a new sensor sample to each active calibration algorithm and sends
+ // out notifications for new calibration updates.
+ if (accel_cal_ != nullptr) {
+ const CalibrationTypeFlags new_cal_flags =
+ accel_cal_->SetMeasurement(sample);
+ if (new_cal_flags != CalibrationTypeFlags::NONE) {
+ accel_cal_update_flags_ |= new_cal_flags;
+ NotifyAshCalibration(CHRE_SENSOR_TYPE_ACCELEROMETER,
+ accel_cal_->GetSensorCalibration(),
+ accel_cal_update_flags_, kAccelTag);
+ PrintCalibration(accel_cal_->GetSensorCalibration(),
+ accel_cal_update_flags_, kAccelTag);
+ }
+ }
+
+ if (gyro_cal_ != nullptr) {
+ const CalibrationTypeFlags new_cal_flags =
+ gyro_cal_->SetMeasurement(sample);
+ if (new_cal_flags != CalibrationTypeFlags::NONE) {
+ gyro_cal_update_flags_ |= new_cal_flags;
+ if (NotifyAshCalibration(CHRE_SENSOR_TYPE_GYROSCOPE,
+ gyro_cal_->GetSensorCalibration(),
+ gyro_cal_update_flags_, kGyroTag)) {
+ // Limits the log messaging update rate for the gyro calibrations since
+ // these can occur frequently with rapid temperature changes.
+ if (NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(
+ sample.timestamp_nanos, gyro_notification_time_nanos_,
+ kNanoSensorCalMessageIntervalNanos)) {
+ gyro_notification_time_nanos_ = sample.timestamp_nanos;
+ PrintCalibration(gyro_cal_->GetSensorCalibration(),
+ gyro_cal_update_flags_, kGyroTag);
+ }
+ }
+ }
+ }
+
+ if (mag_cal_ != nullptr) {
+ const CalibrationTypeFlags new_cal_flags = mag_cal_->SetMeasurement(sample);
+ if (new_cal_flags != CalibrationTypeFlags::NONE) {
+ mag_cal_update_flags_ |= new_cal_flags;
+ NotifyAshCalibration(CHRE_SENSOR_TYPE_GEOMAGNETIC_FIELD,
+ mag_cal_->GetSensorCalibration(),
+ mag_cal_update_flags_, kMagTag);
+ PrintCalibration(mag_cal_->GetSensorCalibration(), mag_cal_update_flags_,
+ kMagTag);
+ }
+ }
+}
+
+bool NanoSensorCal::NotifyAshCalibration(
+ uint8_t chreSensorType, const CalibrationDataThreeAxis &cal_data,
+ CalibrationTypeFlags flags, const char *sensor_tag) {
+ // Updates the sensor offset calibration using the ASH API.
+ ashCalInfo ash_cal_info;
+ memset(&ash_cal_info, 0, sizeof(ashCalInfo));
+ ash_cal_info.compMatrix[0] = 1.0f; // Sets diagonal to unity (scale factor).
+ ash_cal_info.compMatrix[4] = 1.0f;
+ ash_cal_info.compMatrix[8] = 1.0f;
+ memcpy(ash_cal_info.bias, cal_data.offset, sizeof(ash_cal_info.bias));
+
+ // Maps CalibrationQualityLevel to ASH calibration accuracy.
+ switch (cal_data.calibration_quality.level) {
+ case online_calibration::CalibrationQualityLevel::HIGH_QUALITY:
+ ash_cal_info.accuracy = ASH_CAL_ACCURACY_HIGH;
+ break;
+
+ case online_calibration::CalibrationQualityLevel::MEDIUM_QUALITY:
+ ash_cal_info.accuracy = ASH_CAL_ACCURACY_MEDIUM;
+ break;
+
+ case online_calibration::CalibrationQualityLevel::LOW_QUALITY:
+ ash_cal_info.accuracy = ASH_CAL_ACCURACY_LOW;
+ break;
+
+ default:
+ ash_cal_info.accuracy = ASH_CAL_ACCURACY_UNRELIABLE;
+ break;
+ }
+
+ if (!ashSetCalibration(chreSensorType, &ash_cal_info)) {
+ NANO_CAL_LOGE(sensor_tag, "ASH failed to apply calibration update.");
+ return false;
+ }
+
+ // Uses the ASH API to store all calibration parameters relevant to a given
+ // algorithm as indicated by the input calibration type flags.
+ ashCalParams ash_cal_parameters;
+ memset(&ash_cal_parameters, 0, sizeof(ashCalParams));
+ if (flags & CalibrationTypeFlags::BIAS) {
+ ash_cal_parameters.offsetTempCelsius = cal_data.offset_temp_celsius;
+ memcpy(ash_cal_parameters.offset, cal_data.offset,
+ sizeof(ash_cal_parameters.offset));
+ ash_cal_parameters.offsetSource = ASH_CAL_PARAMS_SOURCE_RUNTIME;
+ ash_cal_parameters.offsetTempCelsiusSource = ASH_CAL_PARAMS_SOURCE_RUNTIME;
+ }
+
+ if (flags & CalibrationTypeFlags::OVER_TEMP) {
+ memcpy(ash_cal_parameters.tempSensitivity, cal_data.temp_sensitivity,
+ sizeof(ash_cal_parameters.tempSensitivity));
+ memcpy(ash_cal_parameters.tempIntercept, cal_data.temp_intercept,
+ sizeof(ash_cal_parameters.tempIntercept));
+ ash_cal_parameters.tempSensitivitySource = ASH_CAL_PARAMS_SOURCE_RUNTIME;
+ ash_cal_parameters.tempInterceptSource = ASH_CAL_PARAMS_SOURCE_RUNTIME;
+ }
+
+ if (!ashSaveCalibrationParams(chreSensorType, &ash_cal_parameters)) {
+ NANO_CAL_LOGE(sensor_tag, "ASH failed to write calibration update.");
+ return false;
+ }
+
+ return true;
+}
+
+bool NanoSensorCal::LoadAshCalibration(uint8_t chreSensorType,
+ OnlineCalibrationThreeAxis *online_cal,
+ CalibrationTypeFlags* flags,
+ const char *sensor_tag) {
+ ashCalParams recalled_ash_cal_parameters;
+ if (ashLoadCalibrationParams(chreSensorType, ASH_CAL_STORAGE_ASH,
+ &recalled_ash_cal_parameters)) {
+ // Checks whether a valid set of runtime calibration parameters was received
+ // and can be used for initialization.
+ if (DetectRuntimeCalibration(chreSensorType, sensor_tag, flags,
+ &recalled_ash_cal_parameters)) {
+ CalibrationDataThreeAxis cal_data;
+ cal_data.type = online_cal->get_sensor_type();
+ cal_data.cal_update_time_nanos = chreGetTime();
+
+ // Analyzes the calibration flags and sets only the runtime calibration
+ // values that were received.
+ if (*flags & CalibrationTypeFlags::BIAS) {
+ cal_data.offset_temp_celsius =
+ recalled_ash_cal_parameters.offsetTempCelsius;
+ memcpy(cal_data.offset, recalled_ash_cal_parameters.offset,
+ sizeof(cal_data.offset));
+ }
+
+ if (*flags & CalibrationTypeFlags::OVER_TEMP) {
+ memcpy(cal_data.temp_sensitivity,
+ recalled_ash_cal_parameters.tempSensitivity,
+ sizeof(cal_data.temp_sensitivity));
+ memcpy(cal_data.temp_intercept,
+ recalled_ash_cal_parameters.tempIntercept,
+ sizeof(cal_data.temp_intercept));
+ }
+
+ // Sets the algorithm's initial calibration data and notifies ASH to apply
+ // the recalled calibration data.
+ if (online_cal->SetInitialCalibration(cal_data)) {
+ return NotifyAshCalibration(chreSensorType,
+ online_cal->GetSensorCalibration(), *flags,
+ sensor_tag);
+ } else {
+ NANO_CAL_LOGE(sensor_tag,
+ "Calibration data failed to initialize algorithm.");
+ }
+ }
+ } else {
+ NANO_CAL_LOGE(sensor_tag, "ASH failed to recall calibration data.");
+ }
+
+ return false;
+}
+
+bool NanoSensorCal::DetectRuntimeCalibration(uint8_t chreSensorType,
+ const char *sensor_tag,
+ CalibrationTypeFlags *flags,
+ ashCalParams *ash_cal_parameters) {
+ // Analyzes calibration source flags to determine whether runtime
+ // calibration values have been loaded and may be used for initialization. A
+ // valid runtime calibration source will include at least an offset.
+ *flags = CalibrationTypeFlags::NONE; // Resets the calibration flags.
+
+ // Uses the ASH calibration source flags to set the appropriate
+ // CalibrationTypeFlags. These will be used to determine which values to copy
+ // from 'ash_cal_parameters' and provide to the calibration algorithms for
+ // initialization.
+ bool runtime_cal_detected = false;
+ if (ash_cal_parameters->offsetSource == ASH_CAL_PARAMS_SOURCE_RUNTIME &&
+ ash_cal_parameters->offsetTempCelsiusSource ==
+ ASH_CAL_PARAMS_SOURCE_RUNTIME) {
+ runtime_cal_detected = true;
+ *flags = CalibrationTypeFlags::BIAS;
+ }
+
+ if (ash_cal_parameters->tempSensitivitySource ==
+ ASH_CAL_PARAMS_SOURCE_RUNTIME &&
+ ash_cal_parameters->tempInterceptSource ==
+ ASH_CAL_PARAMS_SOURCE_RUNTIME) {
+ *flags |= CalibrationTypeFlags::OVER_TEMP;
+ }
+
+ if (runtime_cal_detected) {
+ // Prints the retrieved runtime calibration data.
+ NANO_CAL_LOGI(sensor_tag, "Runtime calibration data detected.");
+ PrintAshCalParams(*ash_cal_parameters, sensor_tag);
+ } else {
+ // This is a warning (not an error) since the runtime algorithms will
+ // function correctly with no recalled calibration values. They will
+ // eventually trigger and update the system with valid calibration data.
+ NANO_CAL_LOGW(sensor_tag, "No runtime offset calibration data found.");
+ }
+
+ return runtime_cal_detected;
+}
+
+// Helper functions for logging calibration information.
+void NanoSensorCal::PrintAshCalParams(const ashCalParams &cal_params,
+ const char *sensor_tag) {
+ if (cal_params.offsetSource == ASH_CAL_PARAMS_SOURCE_RUNTIME) {
+ NANO_CAL_LOGI(sensor_tag,
+ "Offset | Temperature [C]: %.6f, %.6f, %.6f | %.2f",
+ cal_params.offset[0], cal_params.offset[1],
+ cal_params.offset[2], cal_params.offsetTempCelsius);
+ }
+
+ if (cal_params.tempSensitivitySource == ASH_CAL_PARAMS_SOURCE_RUNTIME) {
+ NANO_CAL_LOGI(sensor_tag, "Temp Sensitivity [units/C]: %.6f, %.6f, %.6f",
+ cal_params.tempSensitivity[0], cal_params.tempSensitivity[1],
+ cal_params.tempSensitivity[2]);
+ }
+
+ if (cal_params.tempInterceptSource == ASH_CAL_PARAMS_SOURCE_RUNTIME) {
+ NANO_CAL_LOGI(sensor_tag, "Temp Intercept [units]: %.6f, %.6f, %.6f",
+ cal_params.tempIntercept[0], cal_params.tempIntercept[1],
+ cal_params.tempIntercept[2]);
+ }
+
+ if (cal_params.scaleFactorSource == ASH_CAL_PARAMS_SOURCE_RUNTIME) {
+ NANO_CAL_LOGI(sensor_tag, "Scale Factor: %.6f, %.6f, %.6f",
+ cal_params.scaleFactor[0], cal_params.scaleFactor[1],
+ cal_params.scaleFactor[2]);
+ }
+
+ if (cal_params.crossAxisSource == ASH_CAL_PARAMS_SOURCE_RUNTIME) {
+ NANO_CAL_LOGI(sensor_tag,
+ "Cross-Axis in [yx, zx, zy] order: %.6f, %.6f, %.6f",
+ cal_params.crossAxis[0], cal_params.crossAxis[1],
+ cal_params.crossAxis[2]);
+ }
+}
+
+void NanoSensorCal::PrintCalibration(const CalibrationDataThreeAxis &cal_data,
+ CalibrationTypeFlags flags,
+ const char *sensor_tag) {
+ if (flags & CalibrationTypeFlags::BIAS) {
+ NANO_CAL_LOGI(sensor_tag,
+ "Offset | Temperature [C]: %.6f, %.6f, %.6f | %.2f",
+ cal_data.offset[0], cal_data.offset[1], cal_data.offset[2],
+ cal_data.offset_temp_celsius);
+ }
+
+ if (flags & CalibrationTypeFlags::OVER_TEMP) {
+ NANO_CAL_LOGI(sensor_tag, "Temp Sensitivity: %.6f, %.6f, %.6f",
+ cal_data.temp_sensitivity[0], cal_data.temp_sensitivity[1],
+ cal_data.temp_sensitivity[2]);
+ NANO_CAL_LOGI(sensor_tag, "Temp Intercept: %.6f, %.6f, %.6f",
+ cal_data.temp_intercept[0], cal_data.temp_intercept[1],
+ cal_data.temp_intercept[2]);
+ }
+}
+
+} // namespace nano_calibration
diff --git a/firmware/os/algos/calibration/nano_calibration/nano_calibration.h b/firmware/os/algos/calibration/nano_calibration/nano_calibration.h
new file mode 100644
index 0000000..d56d034
--- /dev/null
+++ b/firmware/os/algos/calibration/nano_calibration/nano_calibration.h
@@ -0,0 +1,165 @@
+/*
+ * Copyright (C) 2017 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+/*
+ * This module provides a containing class (NanoSensorCal) for dynamic runtime
+ * calibration algorithms that affect the following sensors:
+ * - Accelerometer (offset)
+ * - Gyroscope (offset, with over-temperature compensation)
+ * - Magnetometer (offset)
+ *
+ * Sensor Units:
+ * - Accelerometer [meters/sec^2]
+ * - Gyroscope [radian/sec]
+ * - Magnetometer [micro Tesla, uT]
+ * - Temperature [Celsius].
+ *
+ * NOTE1: Define NANO_SENSOR_CAL_DBG_ENABLED to enable debug messaging.
+ *
+ * NOTE2: This module uses pointers to runtime calibration algorithm objects.
+ * These must be constructed and initialized outside of this class. The owner
+ * bares the burden of managing the lifetime of these objects with respect to
+ * the NanoSensorCal class which depends on these objects and handles their
+ * interaction with the Android ASH/CHRE system. This arrangement makes it
+ * convenient to modify the specific algorithm implementations (i.e., choice of
+ * calibration algorithm, parameter tuning, etc.) at the nanoapp level without
+ * the need to specialize the standard functionality implemented here.
+ */
+
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_NANO_CALIBRATION_NANO_CALIBRATION_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_NANO_CALIBRATION_NANO_CALIBRATION_H_
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include <ash.h>
+#include <chre.h>
+
+#include "calibration/online_calibration/common_data/calibration_callback.h"
+#include "calibration/online_calibration/common_data/calibration_data.h"
+#include "calibration/online_calibration/common_data/online_calibration.h"
+#include "calibration/online_calibration/common_data/sensor_data.h"
+#include "common/math/macros.h"
+
+namespace nano_calibration {
+
+// Common log message sensor-specific identifiers.
+constexpr char kAccelTag[] = {"[NanoSensorCal:ACCEL_MPS2]"};
+constexpr char kGyroTag[] = {"[NanoSensorCal:GYRO_RPS]"};
+constexpr char kMagTag[] = {"[NanoSensorCal:MAG_UT]"};
+
+// Limits NanoSensorCal notifications to once every minute.
+constexpr uint64_t kNanoSensorCalMessageIntervalNanos = MIN_TO_NANOS(1);
+
+/*
+ * NanoSensorCal is a container class for dynamic runtime calibration sensor
+ * algorithms used by the IMU_Cal CHRE nanoapp. The main purpose of this class
+ * is to transfer sensor data to the sensor calibration algorithms and provide
+ * calibration updates to CHRE using the ASH API.
+ */
+class NanoSensorCal {
+ public:
+ // Alias used to reference the three-axis OnlineCalibration baseclass used by
+ // the runtime calibration sensor wrappers. This is for convenience and to
+ // help with code readability.
+ using OnlineCalibrationThreeAxis = online_calibration::OnlineCalibration<
+ online_calibration::CalibrationDataThreeAxis>;
+
+ NanoSensorCal() = default;
+
+ // Sets the sensor calibration object pointers and initializes the algorithms
+ // using runtime values recalled using Android Sensor Hub (ASH). A nullptr may
+ // be passed in to disable a particular sensor calibration.
+ void Initialize(OnlineCalibrationThreeAxis *accel_cal,
+ OnlineCalibrationThreeAxis *gyro_cal,
+ OnlineCalibrationThreeAxis *mag_cal);
+
+ // Sends new sensor samples to the calibration algorithms.
+ void HandleSensorSamples(uint16_t event_type,
+ const chreSensorThreeAxisData *event_data);
+
+ // Provides temperature updates to the calibration algorithms.
+ void HandleTemperatureSamples(uint16_t event_type,
+ const chreSensorFloatData *event_data);
+
+ private:
+ // Passes sensor data to the runtime calibration algorithms.
+ void ProcessSample(const online_calibration::SensorData &sample);
+
+ // Loads runtime calibration data using the Android Sensor Hub API. Returns
+ // 'true' when runtime calibration values were successfully recalled and used
+ // for algorithm initialization. 'sensor_tag' is a string that identifies a
+ // sensor-specific identifier for log messages. Updates 'flags' to indicate
+ // which runtime calibration parameters were recalled.
+ bool LoadAshCalibration(uint8_t chreSensorType,
+ OnlineCalibrationThreeAxis *online_cal,
+ online_calibration::CalibrationTypeFlags* flags,
+ const char *sensor_tag);
+
+ // Provides sensor calibration updates using the ASH API for the specified
+ // sensor type. 'cal_data' contains the new calibration data. 'flags' is used
+ // to indicate all of the valid calibration values that should be provided
+ // with the update. Returns 'true' with a successful ASH update.
+ bool NotifyAshCalibration(
+ uint8_t chreSensorType,
+ const online_calibration::CalibrationDataThreeAxis &cal_data,
+ online_calibration::CalibrationTypeFlags flags, const char *sensor_tag);
+
+ // Checks whether 'ash_cal_parameters' is a valid set of runtime calibration
+ // data and can be used for algorithm initialization. Updates 'flags' to
+ // indicate which runtime calibration parameters were detected.
+ bool DetectRuntimeCalibration(uint8_t chreSensorType, const char *sensor_tag,
+ online_calibration::CalibrationTypeFlags *flags,
+ ashCalParams *ash_cal_parameters);
+
+ // Helper functions for logging calibration information.
+ void PrintAshCalParams(const ashCalParams &cal_params,
+ const char *sensor_tag);
+
+ void PrintCalibration(
+ const online_calibration::CalibrationDataThreeAxis &cal_data,
+ online_calibration::CalibrationTypeFlags flags, const char *sensor_tag);
+
+ // Pointer to the accelerometer runtime calibration object.
+ OnlineCalibrationThreeAxis *accel_cal_ = nullptr;
+
+ // Pointer to the gyroscope runtime calibration object.
+ OnlineCalibrationThreeAxis *gyro_cal_ = nullptr;
+
+ // Limits the log messaging update rate for the gyro calibrations since these
+ // can occur frequently with rapid temperature changes.
+ uint64_t gyro_notification_time_nanos_ = 0;
+
+ // Pointer to the magnetometer runtime calibration object.
+ OnlineCalibrationThreeAxis *mag_cal_ = nullptr;
+
+ // Flags that determine which calibration elements are updated with the ASH
+ // API. These are reset during initialization, and latched when a particular
+ // calibration update is detected upon a valid recall of parameters and/or
+ // during runtime. The latching behavior is used to start sending calibration
+ // values of a given type (e.g., bias, over-temp model, etc.) once they are
+ // detected and thereafter.
+ online_calibration::CalibrationTypeFlags accel_cal_update_flags_ =
+ online_calibration::CalibrationTypeFlags::NONE;
+ online_calibration::CalibrationTypeFlags gyro_cal_update_flags_ =
+ online_calibration::CalibrationTypeFlags::NONE;
+ online_calibration::CalibrationTypeFlags mag_cal_update_flags_ =
+ online_calibration::CalibrationTypeFlags::NONE;
+};
+
+} // namespace nano_calibration
+
+#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_NANO_CALIBRATION_NANO_CALIBRATION_H_
diff --git a/firmware/os/algos/calibration/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.cc b/firmware/os/algos/calibration/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.cc
new file mode 100644
index 0000000..e9def4e
--- /dev/null
+++ b/firmware/os/algos/calibration/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.cc
@@ -0,0 +1,93 @@
+/*
+ * Copyright (C) 2018 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "calibration/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.h"
+
+#include "calibration/util/cal_log.h"
+
+namespace online_calibration {
+
+void AccelOffsetCal::Initialize(
+ const AccelCalParameters& accel_cal_parameters) {
+ accelCalInit(&accel_cal_, &accel_cal_parameters);
+ InitializeCalData();
+}
+
+CalibrationTypeFlags AccelOffsetCal::SetMeasurement(const SensorData& sample) {
+ // Routes the input sensor sample to the calibration algorithm.
+ switch (sample.type) {
+ case SensorType::kAccelerometerMps2:
+ accelCalRun(&accel_cal_, sample.timestamp_nanos,
+ sample.data[SensorIndex::kXAxis],
+ sample.data[SensorIndex::kYAxis],
+ sample.data[SensorIndex::kZAxis], // [m/sec^2]
+ temperature_celsius_);
+
+#ifdef ACCEL_CAL_DBG_ENABLED
+ // Prints debug data report.
+ accelCalDebPrint(&accel_cal_, temperature_celsius_);
+#endif
+ break;
+
+ case SensorType::kTemperatureCelsius:
+ temperature_celsius_ = sample.data[SensorIndex::kSingleAxis];
+ break;
+
+ default:
+ // This sample is not required.
+ return cal_update_polling_flags_;
+ }
+
+ // Checks for a new offset estimate, and updates the calibration data.
+ if (accelCalNewBiasAvailable(&accel_cal_)) {
+ accelCalUpdateBias(&accel_cal_, &cal_data_.offset[0], &cal_data_.offset[1],
+ &cal_data_.offset[2]);
+
+ cal_data_.calibration_quality.level = CalibrationQualityLevel::HIGH_QUALITY;
+ cal_data_.calibration_quality.value = kUndeterminedCalibrationQuality;
+ cal_data_.offset_temp_celsius = temperature_celsius_;
+ cal_data_.cal_update_time_nanos = sample.timestamp_nanos;
+ cal_update_polling_flags_ = CalibrationTypeFlags::BIAS;
+ OnNotifyCalibrationUpdate(CalibrationTypeFlags::BIAS);
+ }
+
+ return cal_update_polling_flags_;
+}
+
+bool AccelOffsetCal::SetInitialCalibration(
+ const CalibrationDataThreeAxis& input_cal_data) {
+ // Checks that the input calibration type matches the algorithm type.
+ if (input_cal_data.type != get_sensor_type()) {
+ CAL_DEBUG_LOG("[AccelOffsetCal]",
+ "SetInitialCalibration failed due to wrong sensor type.");
+ return false;
+ }
+
+ // Sets the accelerometer algorithm's calibration data.
+ accelCalBiasSet(&accel_cal_, input_cal_data.offset[0],
+ input_cal_data.offset[1], input_cal_data.offset[2]);
+
+ // Sync's all initial calibration data.
+ cal_data_ = input_cal_data;
+
+ // Sets the calibration quality.
+ cal_data_.calibration_quality.level = CalibrationQualityLevel::LOW_QUALITY;
+ cal_data_.calibration_quality.value = kUndeterminedCalibrationQuality;
+
+ return true;
+}
+
+} // namespace online_calibration
diff --git a/firmware/os/algos/calibration/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.h b/firmware/os/algos/calibration/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.h
new file mode 100644
index 0000000..0256495
--- /dev/null
+++ b/firmware/os/algos/calibration/online_calibration/accelerometer/accel_offset_cal/accel_offset_cal.h
@@ -0,0 +1,78 @@
+/*
+ * Copyright (C) 2018 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_ACCELEROMETER_ACCEL_OFFSET_CAL_ACCEL_OFFSET_CAL_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_ACCELEROMETER_ACCEL_OFFSET_CAL_ACCEL_OFFSET_CAL_H_
+
+#include "calibration/accelerometer/accel_cal.h"
+#include "calibration/online_calibration/common_data/calibration_callback.h"
+#include "calibration/online_calibration/common_data/calibration_data.h"
+#include "calibration/online_calibration/common_data/online_calibration.h"
+#include "calibration/online_calibration/common_data/sensor_data.h"
+
+namespace online_calibration {
+
+/*
+ * This class is a wrapper for accelerometer offset calibration.
+ *
+ * NOTE: Calibration quality reporting (quantitative metric is not used):
+ * Initialize --> CalibrationQualityLevel::UNDETERMINED
+ * CalibrationQuality.value =
+ * kUndeterminedCalibrationQuality
+ * SetInitialCalibration --> CalibrationQualityLevel::LOW_QUALITY
+ * CalibrationQuality.value =
+ * kUndeterminedCalibrationQuality
+ * New Calibration Update --> CalibrationQualityLevel::HIGH_QUALITY
+ * CalibrationQuality.value =
+ * kUndeterminedCalibrationQuality
+ */
+class AccelOffsetCal final
+ : public OnlineCalibration<CalibrationDataThreeAxis> {
+ public:
+ // Default constructor.
+ AccelOffsetCal() = default;
+
+ // Creates an AccelOffsetCal with specified algorithm parameters.
+ explicit AccelOffsetCal(const AccelCalParameters& accel_cal_parameters) {
+ Initialize(accel_cal_parameters);
+ }
+
+ // Initializes with specified algorithm parameters, and resets the calibration
+ // data.
+ void Initialize(const AccelCalParameters& accel_cal_parameters);
+
+ // Sends new sensor data to the calibration algorithm, and returns the state
+ // of the calibration update flags, 'cal_update_polling_flags_'.
+ CalibrationTypeFlags SetMeasurement(const SensorData& sample) final;
+
+ // Sets the initial calibration data of the calibration algorithm. Returns
+ // true if set successfully.
+ bool SetInitialCalibration(
+ const CalibrationDataThreeAxis& input_cal_data) final;
+
+ // Returns the calibration sensor type.
+ SensorType get_sensor_type() const final {
+ return SensorType::kAccelerometerMps2;
+ };
+
+ private:
+ // Accelerometer offset calibration algorithm data structure.
+ AccelCal accel_cal_;
+};
+
+} // namespace online_calibration
+
+#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_ACCELEROMETER_ACCEL_OFFSET_CAL_ACCEL_OFFSET_CAL_H_
diff --git a/firmware/os/algos/calibration/online_calibration/common_data/calibration_callback.h b/firmware/os/algos/calibration/online_calibration/common_data/calibration_callback.h
new file mode 100644
index 0000000..62bc443
--- /dev/null
+++ b/firmware/os/algos/calibration/online_calibration/common_data/calibration_callback.h
@@ -0,0 +1,84 @@
+/*
+ * Copyright (C) 2018 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+/*
+ * This module provides the callback functionality employed by the online sensor
+ * calibration algorithms.
+ */
+
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_CALIBRATION_CALLBACK_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_CALIBRATION_CALLBACK_H_
+
+#include "calibration/online_calibration/common_data/calibration_data.h"
+
+namespace online_calibration {
+
+/*
+ * This codebase avoids Standard Template Library (STL) containers to maximize
+ * its usefullness on embedded hardware with basic C++ compiler support. The
+ * following uses type erasure to implement callback functionality to a user's
+ * desired class method. The idea here is to store an object pointer by
+ * instantiating a class template (Callback) which implements a virtual
+ * interface function (CallbackInterface::Call).
+ *
+ * USAGE:
+ * See unit testing for a simple example of how to use this for callback
+ * functionality.
+ */
+
+// CalibrationType: Sets the calibration type (e.g., CalibrationDataThreeAxis).
+template <class CalibrationType>
+class CallbackInterface {
+ public:
+ // Interface function that is defined to implement the desired callback.
+ virtual void Call(const CalibrationType& cal_data,
+ CalibrationTypeFlags cal_update_flags) = 0;
+ virtual ~CallbackInterface() {}
+};
+
+// ClassCallerType: Sets the object's class type for the callback.
+// CalibrationType: Sets the calibration type (e.g., CalibrationDataThreeAxis).
+template <class ClassCallerType, class CalibrationType>
+class Callback : public CallbackInterface<CalibrationType> {
+ public:
+ // Constructors.
+ Callback() = delete;
+ Callback(ClassCallerType* obj,
+ void (ClassCallerType::*func)(const CalibrationType& cal_data,
+ CalibrationTypeFlags cal_update_flags))
+ : object_ptr_(obj), function_ptr_(func) {}
+
+ // Implements the callback to the desired class-method.
+ void Call(const CalibrationType& cal_data,
+ CalibrationTypeFlags cal_update_flags) final {
+ (object_ptr_->*function_ptr_)(cal_data, cal_update_flags);
+ }
+
+ private:
+ // Pointer to the class that owns the callback method.
+ ClassCallerType* object_ptr_;
+
+ // Templated function pointer with the required function signature.
+ // Calibration callbacks must accept:
+ // 1. Constant reference to the calibration.
+ // 2. Bitmask indicating which calibration components have been updated.
+ void (ClassCallerType::*function_ptr_)(const CalibrationType& cal_data,
+ CalibrationTypeFlags cal_update_flags);
+};
+
+} // namespace online_calibration
+
+#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_CALIBRATION_CALLBACK_H_
diff --git a/firmware/os/algos/calibration/online_calibration/common_data/calibration_data.cc b/firmware/os/algos/calibration/online_calibration/common_data/calibration_data.cc
new file mode 100644
index 0000000..8f1470a
--- /dev/null
+++ b/firmware/os/algos/calibration/online_calibration/common_data/calibration_data.cc
@@ -0,0 +1,38 @@
+/*
+ * Copyright (C) 2018 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "calibration/online_calibration/common_data/calibration_data.h"
+
+namespace online_calibration {
+
+CalibrationTypeFlags operator|(CalibrationTypeFlags lhs,
+ CalibrationTypeFlags rhs) {
+ return static_cast<CalibrationTypeFlags>(static_cast<char>(lhs) |
+ static_cast<char>(rhs));
+}
+
+bool operator&(CalibrationTypeFlags lhs, CalibrationTypeFlags rhs) {
+ return static_cast<char>(lhs) & static_cast<char>(rhs);
+}
+
+CalibrationTypeFlags& operator|=(CalibrationTypeFlags& lhs,
+ CalibrationTypeFlags rhs) {
+ lhs = static_cast<CalibrationTypeFlags>(static_cast<char>(lhs) |
+ static_cast<char>(rhs));
+ return lhs;
+}
+
+} // namespace online_calibration
diff --git a/firmware/os/algos/calibration/online_calibration/common_data/calibration_data.h b/firmware/os/algos/calibration/online_calibration/common_data/calibration_data.h
new file mode 100644
index 0000000..0404936
--- /dev/null
+++ b/firmware/os/algos/calibration/online_calibration/common_data/calibration_data.h
@@ -0,0 +1,228 @@
+/*
+ * Copyright (C) 2018 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+/*
+ * This module provides the component definitions used to represent sensor
+ * calibration data, labeled flags/enumerators, and the callback functionality
+ * employed by the online sensor calibration algorithms.
+ */
+
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_CALIBRATION_DATA_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_CALIBRATION_DATA_H_
+
+#include <stdint.h>
+#include <string.h>
+#include <sys/types.h>
+
+#include "calibration/online_calibration/common_data/calibration_quality.h"
+#include "calibration/online_calibration/common_data/sensor_data.h"
+#include "calibration/over_temp/over_temp_model.h"
+
+namespace online_calibration {
+
+/*
+ * Bitmask used to indicate which calibration values have changed for a given
+ * calibration update.
+ *
+ * [Bit Flag] | [Affected Sensor Calibration Value]
+ * BIAS - Quasi-static non-zero sensor output (offset), given
+ * conditions where the sensor should ideally output zero.
+ * Includes corrections for over-temperature compensation.
+ * SCALE_FACTOR - Sensor axis scaling (ideally unity).
+ * CROSS_AXIS - Output sensitivity due to variations of perpendicular
+ * sensing axes (ideally zero).
+ * OVER_TEMP - Model parameters that capture variations in sensor
+ * behavior with temperature (e.g., linear bias sensitivity
+ * model).
+ * QUALITY_DEGRADED - Indicates a degradation in calibration quality.
+ */
+enum class CalibrationTypeFlags : uint8_t {
+ NONE = 0x00,
+ BIAS = 0x01,
+ SCALE_FACTOR = 0x02,
+ CROSS_AXIS = 0x04,
+ OVER_TEMP = 0x08,
+ QUALITY_DEGRADED = 0x10,
+ ALL = 0xFF
+};
+
+// Logic operators to assist with common bitmask setting/checking.
+CalibrationTypeFlags operator|(CalibrationTypeFlags lhs,
+ CalibrationTypeFlags rhs);
+
+bool operator&(CalibrationTypeFlags lhs, CalibrationTypeFlags rhs);
+
+CalibrationTypeFlags& operator|=(CalibrationTypeFlags& lhs,
+ CalibrationTypeFlags rhs);
+
+/*
+ * Defines the calibration data specific to a prototypical three-axis sensing
+ * device (e.g., accelerometer, gyroscope, magnetometer).
+ *
+ * Calibration correction may be applied as:
+ * corrected_data = scale_skew_matrix * (input_data - offset);
+ *
+ * 'offset' is the sensor bias estimate (with temperature compensation applied
+ * when supported by the underlying calibration algorithm).
+ *
+ * The 'scale_skew_matrix' is assumed to be in lower diagonal form where the
+ * sensor frame reference definition is such that cross-axis sensitivities
+ * cross_axis_xy, cross_axis_xz, and cross_axis_yz are set to zero.
+ *
+ * scale_skew_matrix = [scale_factor_x 0 0
+ * cross_axis_yx scale_factor_y 0
+ * cross_axis_zx cross_axis_zy scale_factor_z]
+ *
+ * NOTE1: If over-temperature compensation is provided, then temperature
+ * compensation is already applied to the 'offset'. Coefficients representative
+ * of the sensor's temperature dependency model are provided, and may be used
+ * for model initialization after a system restart.
+ *
+ * temp_sensitivity - Modeled temperature sensitivity (i.e., linear slope).
+ * temp_intercept - Linear model intercept.
+ *
+ * The model equation for the over-temperature compensated offset:
+ * compensated_offset = temp_sensitivity * current_temp + temp_intercept
+ *
+ * NOTE2: Unless otherwise stated, 3-dimensional array indices: 0=x, 1=y, 2=z.
+ */
+struct CalibrationDataThreeAxis {
+ // Timestamp for the most recent calibration update.
+ uint64_t cal_update_time_nanos = 0;
+
+ // The sensor's offset (i.e., bias) in the x-, y-, z-axes at
+ // 'offset_temp_celsius'.
+ float offset[3];
+
+ // The temperature associated with the sensor offset.
+ float offset_temp_celsius;
+
+ // The temperature sensitivity of the offset.
+ float temp_sensitivity[3]; // [sensor_units/Celsius]
+
+ // The sensor's offset at zero degrees Celsius.
+ float temp_intercept[3]; // [sensor_units]
+
+ // The sensor's scale factor for each axis.
+ float scale_factor[3];
+
+ // The cross-axis factors in order: [0=yx, 1=zx, 2=zy].
+ float cross_axis[3];
+
+ // Metrics used for the reported calibration accuracy. The behavior and
+ // definition depends on the sensor calibration type. See the calibration
+ // algorithm documentation for details.
+ CalibrationQuality calibration_quality;
+
+ // Indicates the type of sensing device being calibrated.
+ SensorType type = SensorType::kUndefined;
+
+ // Optional pointer to an array of over-temperature model data (null when not
+ // used). For initialization, populating a model dataset will take precedence
+ // over the linear model parameters provided in the calibration data.
+ OverTempModelThreeAxis* otc_model_data = nullptr;
+ int16_t num_model_pts = 0;
+
+ // Helper function that resets the calibration data to a set of neutral
+ // reference values where no calibration correction would be applied if used.
+ void reset() {
+ otc_model_data = nullptr;
+ calibration_quality.reset();
+ offset_temp_celsius = 0.0f;
+ scale_factor[0] = 1.0f;
+ scale_factor[1] = 1.0f;
+ scale_factor[2] = 1.0f;
+ memset(offset, 0, sizeof(offset));
+ memset(temp_sensitivity, 0, sizeof(temp_sensitivity));
+ memset(temp_intercept, 0, sizeof(temp_intercept));
+ memset(cross_axis, 0, sizeof(cross_axis));
+ }
+
+ CalibrationDataThreeAxis() { reset(); }
+};
+
+/*
+ * Defines the calibration data for single dimensional sensing device (e.g.,
+ * thermometer, barometer).
+ *
+ * Calibration correction may be applied as:
+ * corrected_data = scale_factor * (input_data - offset);
+ *
+ * 'offset' is the sensor bias estimate (with temperature compensation applied,
+ * if supported by the underlying calibration algorithm).
+ *
+ * NOTE: If over-temperature compensation is provided, then temperature
+ * compensation is already applied to the 'offset'. Coefficients representative
+ * of the sensor's temperature dependency model are provided, and may be used
+ * for model initialization after a system restart.
+ *
+ * temp_sensitivity - Modeled temperature sensitivity (i.e., linear slope).
+ * temp_intercept - Linear model intercept.
+ *
+ * The model equation for the over-temperature compensated offset:
+ * compensated_offset = temp_sensitivity * current_temp + temp_intercept
+ */
+struct CalibrationDataSingleAxis {
+ // Timestamp for the most recent calibration update.
+ uint64_t cal_update_time_nanos = 0;
+
+ // The sensor's offset (i.e., bias) at temperature, 'offset_temp_celsius'.
+ float offset;
+
+ // The temperature associated with the sensor offset.
+ float offset_temp_celsius;
+
+ // The temperature sensitivity of the offset.
+ float temp_sensitivity; // [sensor_units/Celsius]
+
+ // The sensor's offset at zero degrees Celsius.
+ float temp_intercept; // [sensor_units]
+
+ // The sensor's scale factor.
+ float scale_factor;
+
+ // Metrics used for the reported calibration accuracy. The behavior and
+ // definition depends on the sensor calibration type. See the calibration
+ // algorithm documentation for details.
+ CalibrationQuality calibration_quality;
+
+ // Indicates the type of sensing device being calibrated.
+ SensorType type = SensorType::kUndefined;
+
+ // Optional pointer to an array of over-temperature model data (null when not
+ // used). For initialization, populating a model dataset will take precedence
+ // over the linear model parameters provided in the calibration data.
+ OverTempModelSingleAxis* otc_model_data = nullptr;
+ int16_t num_model_pts = 0;
+
+ // Helper function that resets the calibration data to a set of neutral
+ // reference values where no calibration correction would be applied if used.
+ void reset() {
+ otc_model_data = nullptr;
+ calibration_quality.reset();
+ scale_factor = 1.0f;
+ offset_temp_celsius = 0.0f;
+ offset = 0.0f;
+ temp_sensitivity = 0.0f;
+ temp_intercept = 0.0f;
+ }
+
+ CalibrationDataSingleAxis() { reset(); }
+};
+
+} // namespace online_calibration
+
+#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_CALIBRATION_DATA_H_
diff --git a/firmware/os/algos/calibration/online_calibration/common_data/calibration_quality.h b/firmware/os/algos/calibration/online_calibration/common_data/calibration_quality.h
new file mode 100644
index 0000000..d6475c7
--- /dev/null
+++ b/firmware/os/algos/calibration/online_calibration/common_data/calibration_quality.h
@@ -0,0 +1,96 @@
+/*
+ * Copyright (C) 2018 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+/*
+ * This module provides quality definitions that represent the accuracy
+ * associated with calibration data. This information may be used to affect how
+ * a system uses available sensor calibration data.
+ */
+
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_CALIBRATION_QUALITY_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_CALIBRATION_QUALITY_H_
+
+#include <float.h>
+#include <stdint.h>
+
+namespace online_calibration {
+
+/*
+ * In general, calibration quality values may be thought of in terms of
+ * something akin to an error standard deviation. That is, it's a non-negative
+ * value where lower values imply higher calibration quality, and larger values
+ * indicate poorer quality. However, the units and numerical interpretation is
+ * ultimately dictated by the sensor type and calibration algorithm. Consult the
+ * calibration code implementation for the actual calibration quality metric
+ * details.
+ */
+
+/*
+ * Bitmask used to provide a qualitative ranking of the calibration data's
+ * accuracy. Many systems use this sort of interpretation for calibration
+ * quality (e.g., Android).
+ *
+ * [Bit Flag] | [Qualitative Calibration Quality]
+ * UNDETERMINED - Calibration quality has not yet been determined (e.g., a
+ * calibration hasn't occurred, or a metric hasn't been
+ * established).
+ * LOW_QUALITY - Sensor calibration is needed. System properties have
+ * changed that may have affected the applicability of the
+ * current calibration (e.g., calibration data is old, anomaly
+ * detected, etc.).
+ * MEDIUM_QUALITY - The reported sensor calibration has an average level of
+ * accuracy, updated calibration may improve the readings.
+ * HIGH_QUALITY - The reported calibration has maximal accuracy.
+ */
+enum class CalibrationQualityLevel : uint8_t {
+ UNDETERMINED = 0x00,
+ LOW_QUALITY = 0x01,
+ MEDIUM_QUALITY = 0x02,
+ HIGH_QUALITY = 0x04,
+};
+
+// Sets the calibration quality value when this metric is either not
+// implemented, or has not yet been determined (e.g., a calibration hasn't
+// occurred).
+constexpr float kUndeterminedCalibrationQuality = -1.0f;
+
+/*
+ * Calibration quality structure that contains a quantitative (float) and
+ * qualitative (enum) measure of a sensor's calibration accuracy. Both entries
+ * should be co-defined by the algorithm providing the calibration update. The
+ * enum sets the qualitative interpretation of the float value, this is often
+ * used in systems (Android, etc.) to label quality and affect the use of the
+ * calibration data.
+ */
+struct CalibrationQuality {
+ // Provides a qualitative measure for sensor calibration accuracy.
+ CalibrationQualityLevel level = CalibrationQualityLevel::UNDETERMINED;
+
+ // Quantitative metric for the reported calibration accuracy. The behavior and
+ // definition depends on the sensor calibration type. See the calibration
+ // algorithm documentation for details.
+ float value = kUndeterminedCalibrationQuality;
+
+ // Helper function that resets the calibration quality to an initial state.
+ void reset() {
+ level = CalibrationQualityLevel::UNDETERMINED;
+ value = kUndeterminedCalibrationQuality;
+ }
+};
+
+} // namespace online_calibration
+
+#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_CALIBRATION_QUALITY_H_
diff --git a/firmware/os/algos/calibration/online_calibration/common_data/online_calibration.h b/firmware/os/algos/calibration/online_calibration/common_data/online_calibration.h
new file mode 100644
index 0000000..59e26ba
--- /dev/null
+++ b/firmware/os/algos/calibration/online_calibration/common_data/online_calibration.h
@@ -0,0 +1,133 @@
+/*
+ * Copyright (C) 2018 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_ONLINE_CALIBRATION_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_ONLINE_CALIBRATION_H_
+
+#include <string.h>
+
+#include "calibration/online_calibration/common_data/calibration_callback.h"
+#include "calibration/online_calibration/common_data/calibration_data.h"
+#include "calibration/online_calibration/common_data/sensor_data.h"
+
+namespace online_calibration {
+
+/*
+ * This abstract base class provides a set of general interface functions for
+ * calibration algorithms. The data structures used are intended to be lean and
+ * portable to a wide variety of software and hardware systems. Algorithm
+ * wrappers may use this as a basis for providing the following functionality:
+ *
+ * SetMeasurement - Delivers new sensor data.
+ * SetInitialCalibration - Initializes the algorithm's calibration data.
+ * GetSensorCalibration - Retrieves the latest calibration data set.
+ * new_calibration_ready - Used to poll for new calibration updates.
+ * SetCalibrationCallback - User provides a pointer its own Callback object.
+ * UpdateDynamicSystemSettings - Provides feedback to adjust system behavior.
+ * get_sensor_type - Returns the sensor type which is being calibrated.
+ *
+ * NOTE 1: This class accomodates two methods of providing calibration updates.
+ * Either, or both, may be used depending on system requirements. 1) Polling can
+ * be achieved with new_calibration_ready/GetSensorCalibration functions. 2)
+ * Callback notification of new calibration updates can managed using the
+ * SetCalibrationCallback function.
+ *
+ * NOTE 2: This code implementation specifically avoids using standard template
+ * libraries (STL) and other external API’s since they may not be fully
+ * supported on embedded hardware targets. Only basic C/C++ support will be
+ * assumed.
+ */
+
+// CalibrationType: Sets the calibration type (e.g., CalibrationDataThreeAxis).
+template <class CalibrationType>
+class OnlineCalibration {
+ public:
+ // Virtual destructor.
+ virtual ~OnlineCalibration() {}
+
+ // Sends new sensor data to the calibration algorithm, and returns the state
+ // of the calibration update flags, 'cal_update_polling_flags_'.
+ virtual CalibrationTypeFlags SetMeasurement(const SensorData& sample) = 0;
+
+ // Sets the initial calibration data of the calibration algorithm. Returns
+ // "true" if set successfully.
+ virtual bool SetInitialCalibration(const CalibrationType& cal_data) = 0;
+
+ // Polling Updates: New calibration updates are generated during
+ // SetMeasurement and the 'cal_update_polling_flags_' are set according to
+ // which calibration values have changed. To prevent missing updates in
+ // systems that use polling, this bitmask remains latched until the
+ // calibration data is retrieved with this function.
+ const CalibrationType& GetSensorCalibration() const {
+ cal_update_polling_flags_ = CalibrationTypeFlags::NONE;
+ return cal_data_;
+ }
+
+ // Polling Updates: This function returns 'cal_update_polling_flags_' to
+ // indicate which calibration components have a pending update. The updated
+ // calibration data may be retrieved with GetSensorCalibration, and the
+ // 'cal_update_polling_flags_' will reset.
+ CalibrationTypeFlags new_calibration_ready() const {
+ return cal_update_polling_flags_;
+ }
+
+ // Sets the pointer to the CallbackInterface object used for notification of
+ // new calibration updates.
+ void SetCalibrationCallback(
+ CallbackInterface<CalibrationType>* calibration_callback) {
+ calibration_callback_ = calibration_callback;
+ }
+
+ // Returns the sensor-type this calibration algorithm provides updates for.
+ virtual SensorType get_sensor_type() const = 0;
+
+ protected:
+ // Helper function that activates the registered callback.
+ void OnNotifyCalibrationUpdate(CalibrationTypeFlags cal_update_flags) const {
+ if (calibration_callback_ != nullptr) {
+ calibration_callback_->Call(cal_data_, cal_update_flags);
+ }
+ }
+
+ // Helper function used to initialize the calibration data.
+ void InitializeCalData() {
+ cal_data_.reset();
+ cal_data_.type = get_sensor_type();
+ cal_update_polling_flags_ = CalibrationTypeFlags::NONE;
+ }
+
+ // Stores the sensor calibration data.
+ CalibrationType cal_data_;
+
+ // Tracks the most recent sensor temperature value.
+ float temperature_celsius_ = kInvalidTemperatureCelsius;
+
+ // This bitmask indicates which subset of calibration parameters have changed
+ // and is used specifically for polling; the callback notification passes its
+ // own set of update flags which do not need this latching behavior. Marked
+ // mutable so that these flags may be reset when GetSensorCalibration is
+ // called.
+ mutable CalibrationTypeFlags cal_update_polling_flags_ =
+ CalibrationTypeFlags::NONE;
+
+ private:
+ // Pointer to a callback object.
+ CallbackInterface<CalibrationType>* calibration_callback_ = nullptr;
+};
+
+} // namespace online_calibration
+
+#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_ONLINE_CALIBRATION_H_
diff --git a/firmware/os/algos/calibration/online_calibration/common_data/sensor_data.h b/firmware/os/algos/calibration/online_calibration/common_data/sensor_data.h
new file mode 100644
index 0000000..1331153
--- /dev/null
+++ b/firmware/os/algos/calibration/online_calibration/common_data/sensor_data.h
@@ -0,0 +1,102 @@
+/*
+ * Copyright (C) 2018 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+/*
+ * This module provides the component definitions used to represent sensor
+ * data employed by the online sensor calibration algorithms.
+ */
+
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_SENSOR_DATA_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_SENSOR_DATA_H_
+
+#include <stdint.h>
+#include <string.h>
+#include <sys/types.h>
+
+#include "common/math/macros.h"
+
+namespace online_calibration {
+
+// Defines an invalid or uninitialized temperature value (referenced from
+// common/math/macros.h).
+constexpr float kInvalidTemperatureCelsius = INVALID_TEMPERATURE_CELSIUS;
+
+// Unit conversion from nanoseconds to microseconds.
+constexpr uint64_t NanoToMicroseconds(uint64_t x) { return x / 1000; }
+
+// Identifies the various sensing devices used by the calibration algorithms.
+enum class SensorType : int8_t {
+ kUndefined = 0,
+ kAccelerometerMps2 = 1, // 3-axis sensor (units = meter/sec^2).
+ kGyroscopeRps = 2, // 3-axis sensor (units = radian/sec).
+ kMagnetometerUt = 3, // 3-axis sensor (units = micro-Tesla).
+ kTemperatureCelsius = 4, // 1-axis sensor (units = degrees Celsius).
+ kBarometerHpa = 5, // 1-axis sensor (units = hecto-Pascal).
+ kWifiM = 6 // 3-axis sensor (units = meter).
+};
+
+/*
+ * SensorData is a generalized data structure used to represent sensor samples
+ * produced by either a single- or three-axis device. Usage is implied through
+ * the sensor type (i.e., Gyroscope is a three-axis sensor and would therefore
+ * use all elements of 'data'; a pressure sensor is single-dimensional and would
+ * use 'data[SensorIndex::kSingleAxis]'). This arbitration is determined
+ * at the algorithm wrapper level where knowledge of a sensor's dimensionality
+ * is clearly understood.
+ *
+ * NOTE: The unified dimensional representation makes it convenient to pass
+ * either type of data into the interface functions defined in the
+ * OnlineCalibration.
+ */
+
+// Axis index definitions for SensorData::data.
+enum SensorIndex : int8_t {
+ kSingleAxis = 0,
+ kXAxis = kSingleAxis,
+ kYAxis = 1,
+ kZAxis = 2,
+};
+
+struct SensorData {
+ // Indicates the type of sensor this data originated from.
+ SensorType type;
+
+ // Sensor sample timestamp.
+ uint64_t timestamp_nanos;
+
+ // Generalized sensor sample (represents either single- or three-axis data).
+ float data[3];
+
+ SensorData() : type(SensorType::kUndefined), timestamp_nanos(0) {
+ memset(data, 0, sizeof(data));
+ }
+
+ SensorData(SensorType type, uint64_t ts, float x, float y, float z)
+ : type(type), timestamp_nanos(ts) {
+ data[SensorIndex::kXAxis] = x;
+ data[SensorIndex::kYAxis] = y;
+ data[SensorIndex::kZAxis] = z;
+ }
+
+ SensorData(SensorType type, uint64_t ts, float single_axis_sample)
+ : type(type), timestamp_nanos(ts) {
+ data[SensorIndex::kSingleAxis] = single_axis_sample;
+ }
+};
+
+} // namespace online_calibration
+
+#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_SENSOR_DATA_H_
diff --git a/firmware/os/algos/calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.cc b/firmware/os/algos/calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.cc
new file mode 100644
index 0000000..26eef57
--- /dev/null
+++ b/firmware/os/algos/calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.cc
@@ -0,0 +1,176 @@
+/*
+ * Copyright (C) 2018 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.h"
+
+#include "calibration/util/cal_log.h"
+
+namespace online_calibration {
+
+// Estimated upper bounds on gyro offset error (i.e., 3-sigma values).
+constexpr float GyroOffsetOtcCal::kMediumQualityRps;
+constexpr float GyroOffsetOtcCal::kHighQualityRps;
+
+void GyroOffsetOtcCal::Initialize(const GyroCalParameters& gyro_cal_parameters,
+ const OverTempCalParameters& otc_parameters) {
+ gyroCalInit(&gyro_cal_, &gyro_cal_parameters);
+ overTempCalInit(&over_temp_cal_, &otc_parameters);
+ InitializeCalData();
+}
+
+CalibrationTypeFlags GyroOffsetOtcCal::SetMeasurement(
+ const SensorData& sample) {
+ // Routes the input sensor sample to the calibration algorithm.
+ switch (sample.type) {
+ case SensorType::kAccelerometerMps2:
+ gyroCalUpdateAccel(&gyro_cal_, sample.timestamp_nanos,
+ sample.data[SensorIndex::kXAxis],
+ sample.data[SensorIndex::kYAxis],
+ sample.data[SensorIndex::kZAxis]); // [m/sec^2]
+ break;
+
+ case SensorType::kGyroscopeRps:
+ gyroCalUpdateGyro(&gyro_cal_, sample.timestamp_nanos,
+ sample.data[SensorIndex::kXAxis],
+ sample.data[SensorIndex::kYAxis],
+ sample.data[SensorIndex::kZAxis], // [rad/sec]
+ temperature_celsius_);
+ break;
+
+ case SensorType::kMagnetometerUt:
+ gyroCalUpdateMag(&gyro_cal_, sample.timestamp_nanos,
+ sample.data[SensorIndex::kXAxis],
+ sample.data[SensorIndex::kYAxis],
+ sample.data[SensorIndex::kZAxis]); // [micro-Tesla]
+ break;
+
+ case SensorType::kTemperatureCelsius:
+ temperature_celsius_ = sample.data[SensorIndex::kSingleAxis];
+ overTempCalSetTemperature(&over_temp_cal_, sample.timestamp_nanos,
+ temperature_celsius_);
+ break;
+
+ default:
+ // This sample is not required.
+ return cal_update_polling_flags_;
+ }
+
+ // Checks for a new calibration, and updates the OTC.
+ if (gyroCalNewBiasAvailable(&gyro_cal_)) {
+ float offset[3];
+ float temperature_celsius = kInvalidTemperatureCelsius;
+ uint64_t calibration_time_nanos = 0;
+ gyroCalGetBias(&gyro_cal_, &offset[0], &offset[1], &offset[2],
+ &temperature_celsius, &calibration_time_nanos);
+ overTempCalUpdateSensorEstimate(&over_temp_cal_, calibration_time_nanos,
+ offset, temperature_celsius);
+ }
+
+ // Checks the OTC for a new calibration model update.
+ const bool new_otc_model_update =
+ overTempCalNewModelUpdateAvailable(&over_temp_cal_);
+
+ // Checks for a change in the temperature compensated offset estimate.
+ const bool new_otc_offset = overTempCalNewOffsetAvailable(&over_temp_cal_);
+
+ // Sets the new calibration data.
+ CalibrationTypeFlags cal_update_callback_flags = CalibrationTypeFlags::NONE;
+ if (new_otc_offset) {
+ overTempCalGetOffset(&over_temp_cal_, &cal_data_.offset_temp_celsius,
+ cal_data_.offset);
+ cal_data_.cal_update_time_nanos = sample.timestamp_nanos;
+ cal_update_callback_flags |= CalibrationTypeFlags::BIAS;
+ }
+
+ if (new_otc_model_update) {
+ // Sets the pointer to the OTC model dataset and the number of model points.
+ cal_data_.otc_model_data = over_temp_cal_.model_data;
+ cal_data_.num_model_pts = over_temp_cal_.num_model_pts;
+
+ cal_update_callback_flags |= CalibrationTypeFlags::OVER_TEMP;
+ overTempCalGetModel(&over_temp_cal_, cal_data_.offset,
+ &cal_data_.offset_temp_celsius,
+ &cal_data_.cal_update_time_nanos,
+ cal_data_.temp_sensitivity, cal_data_.temp_intercept);
+ }
+
+ // Sets the new calibration quality, polling flag, and notifies a calibration
+ // callback listener of the new update.
+ if (new_otc_model_update || new_otc_offset) {
+ cal_data_.calibration_quality.level = CalibrationQualityLevel::HIGH_QUALITY;
+ cal_data_.calibration_quality.value = kHighQualityRps;
+ cal_update_polling_flags_ |= cal_update_callback_flags;
+ OnNotifyCalibrationUpdate(cal_update_callback_flags);
+ }
+
+ // Print debug data reports.
+#ifdef GYRO_CAL_DBG_ENABLED
+ gyroCalDebugPrint(&gyro_cal_, sample.timestamp_nanos);
+#endif // GYRO_CAL_DBG_ENABLED
+#ifdef OVERTEMPCAL_DBG_ENABLED
+ overTempCalDebugPrint(&over_temp_cal_, sample.timestamp_nanos);
+#endif // OVERTEMPCAL_DBG_ENABLED
+
+ return cal_update_polling_flags_;
+}
+
+bool GyroOffsetOtcCal::SetInitialCalibration(
+ const CalibrationDataThreeAxis& input_cal_data) {
+ // Checks that the input calibration type matches the algorithm type.
+ if (input_cal_data.type != get_sensor_type()) {
+ CAL_DEBUG_LOG("[GyroOffsetOtcCal]",
+ "SetInitialCalibration failed due to wrong sensor type.");
+ return false;
+ }
+
+ // Sync's all initial calibration data.
+ cal_data_ = input_cal_data;
+
+ // Sets the initial calibration data (offset and OTC model parameters).
+ gyroCalSetBias(&gyro_cal_, cal_data_.offset[0], cal_data_.offset[1],
+ cal_data_.offset[2], cal_data_.offset_temp_celsius,
+ cal_data_.cal_update_time_nanos);
+
+ overTempCalSetModel(&over_temp_cal_, cal_data_.offset,
+ cal_data_.offset_temp_celsius,
+ cal_data_.cal_update_time_nanos,
+ cal_data_.temp_sensitivity, cal_data_.temp_intercept,
+ /*jump_start_model=*/false);
+
+ // Sets the calibration quality.
+ cal_data_.calibration_quality.level = CalibrationQualityLevel::MEDIUM_QUALITY;
+ cal_data_.calibration_quality.value = kMediumQualityRps;
+
+ const bool load_new_model_dataset =
+ (input_cal_data.otc_model_data != nullptr &&
+ input_cal_data.num_model_pts > 0);
+
+ if (load_new_model_dataset) {
+ // Loads the new model dataset and uses it to update the linear model
+ // parameters.
+ overTempCalSetModelData(&over_temp_cal_, input_cal_data.num_model_pts,
+ cal_data_.cal_update_time_nanos,
+ input_cal_data.otc_model_data);
+ }
+
+ // Sets the pointer to the OTC model dataset and the number of model points.
+ cal_data_.otc_model_data = over_temp_cal_.model_data;
+ cal_data_.num_model_pts = over_temp_cal_.num_model_pts;
+
+ return true;
+}
+
+} // namespace online_calibration
diff --git a/firmware/os/algos/calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.h b/firmware/os/algos/calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.h
new file mode 100644
index 0000000..e21007b
--- /dev/null
+++ b/firmware/os/algos/calibration/online_calibration/gyroscope/gyro_offset_over_temp_cal/gyro_offset_over_temp_cal.h
@@ -0,0 +1,87 @@
+/*
+ * Copyright (C) 2018 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_GYROSCOPE_GYRO_OFFSET_OVER_TEMP_CAL_GYRO_OFFSET_OVER_TEMP_CAL_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_GYROSCOPE_GYRO_OFFSET_OVER_TEMP_CAL_GYRO_OFFSET_OVER_TEMP_CAL_H_
+
+#include "calibration/gyroscope/gyro_cal.h"
+#include "calibration/online_calibration/common_data/calibration_callback.h"
+#include "calibration/online_calibration/common_data/calibration_data.h"
+#include "calibration/online_calibration/common_data/online_calibration.h"
+#include "calibration/online_calibration/common_data/sensor_data.h"
+#include "calibration/over_temp/over_temp_cal.h"
+
+namespace online_calibration {
+
+/*
+ * This class is a wrapper for the gyroscope offset calibration with
+ * over-temperature compensation (OTC).
+ *
+ * NOTE: Calibration quality reporting:
+ * Initialize --> CalibrationQualityLevel::UNDETERMINED
+ * CalibrationQuality.value =
+ * kUndeterminedCalibrationQuality
+ * SetInitialCalibration --> CalibrationQualityLevel::MEDIUM_QUALITY
+ * CalibrationQuality.value = kMediumQualityRps
+ * New Calibration Update --> CalibrationQualityLevel::HIGH_QUALITY
+ * CalibrationQuality.value = kHighQualityRps
+ */
+class GyroOffsetOtcCal final
+ : public OnlineCalibration<CalibrationDataThreeAxis> {
+ public:
+ // Estimated upper bounds on gyro offset error (i.e., 3-sigma values).
+ static constexpr float kMediumQualityRps = 5.23599e-3f; // 300 mDPS
+ static constexpr float kHighQualityRps = 1.04720e-3f; // 60 mDPS
+
+ // Default constructor.
+ GyroOffsetOtcCal() = default;
+
+ // Creates an GyroOffsetOtcCal with specified algorithm parameters.
+ GyroOffsetOtcCal(const GyroCalParameters& gyro_cal_parameters,
+ const OverTempCalParameters& otc_parameters) {
+ Initialize(gyro_cal_parameters, otc_parameters);
+ }
+
+ // Initializes with specified algorithm parameters, and resets the calibration
+ // data.
+ void Initialize(const GyroCalParameters& gyro_cal_parameters,
+ const OverTempCalParameters& otc_parameters);
+
+ // Sends new sensor data to the calibration algorithm, and returns the state
+ // of the calibration update flags, 'cal_update_polling_flags_'.
+ CalibrationTypeFlags SetMeasurement(const SensorData& sample) final;
+
+ // Sets the initial calibration data of the calibration algorithm. Returns
+ // true if set successfully.
+ bool SetInitialCalibration(
+ const CalibrationDataThreeAxis& input_cal_data) final;
+
+ // Returns the calibration sensor type.
+ SensorType get_sensor_type() const final {
+ return SensorType::kGyroscopeRps;
+ };
+
+ private:
+ // GyroCal algorithm data structure.
+ GyroCal gyro_cal_;
+
+ // Over-temperature offset compensation algorithm data structure.
+ OverTempCal over_temp_cal_;
+};
+
+} // namespace online_calibration
+
+#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_GYROSCOPE_GYRO_OFFSET_OVER_TEMP_CAL_GYRO_OFFSET_OVER_TEMP_CAL_H_
diff --git a/firmware/os/algos/calibration/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.cc b/firmware/os/algos/calibration/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.cc
new file mode 100644
index 0000000..fe787b1
--- /dev/null
+++ b/firmware/os/algos/calibration/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.cc
@@ -0,0 +1,94 @@
+/*
+ * Copyright (C) 2018 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "calibration/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.h"
+
+#include "calibration/util/cal_log.h"
+
+namespace online_calibration {
+
+// Empirically estimated upper bounds on offset error.
+constexpr float MagDiverseCal::kLowQualityUt;
+constexpr float MagDiverseCal::kHighQualityUt;
+
+void MagDiverseCal::Initialize(
+ const MagCalParameters& mag_cal_parameters,
+ const DiversityCheckerParameters& diversity_parameters) {
+ initMagCal(&mag_cal_, &mag_cal_parameters, &diversity_parameters);
+ InitializeCalData();
+}
+
+CalibrationTypeFlags MagDiverseCal::SetMeasurement(const SensorData& sample) {
+ // Routes the input sensor sample to the calibration algorithm.
+ MagUpdate new_calibration_update = MagUpdate::NO_UPDATE;
+ switch (sample.type) {
+ case SensorType::kMagnetometerUt:
+ new_calibration_update = magCalUpdate(
+ &mag_cal_, NanoToMicroseconds(sample.timestamp_nanos),
+ sample.data[SensorIndex::kXAxis], sample.data[SensorIndex::kYAxis],
+ sample.data[SensorIndex::kZAxis]);
+ break;
+
+ case SensorType::kTemperatureCelsius:
+ temperature_celsius_ = sample.data[SensorIndex::kSingleAxis];
+ break;
+
+ default:
+ // This sample is not required.
+ return cal_update_polling_flags_;
+ }
+
+ // Checks for a new offset estimate, and updates the calibration data.
+ if (MagUpdate::UPDATE_BIAS & new_calibration_update) {
+ magCalGetBias(&mag_cal_, &cal_data_.offset[0], &cal_data_.offset[1],
+ &cal_data_.offset[2]);
+
+ cal_data_.calibration_quality.level = CalibrationQualityLevel::HIGH_QUALITY;
+ cal_data_.calibration_quality.value = kHighQualityUt;
+ cal_data_.offset_temp_celsius = temperature_celsius_;
+ cal_data_.cal_update_time_nanos = sample.timestamp_nanos;
+ cal_update_polling_flags_ = CalibrationTypeFlags::BIAS;
+ OnNotifyCalibrationUpdate(CalibrationTypeFlags::BIAS);
+ }
+
+ return cal_update_polling_flags_;
+}
+
+bool MagDiverseCal::SetInitialCalibration(
+ const CalibrationDataThreeAxis& input_cal_data) {
+ // Checks that the input calibration type matches the algorithm type.
+ if (input_cal_data.type != get_sensor_type()) {
+ CAL_DEBUG_LOG("[MagDiverseCal]",
+ "SetInitialCalibration failed due to wrong sensor type.");
+ return false;
+ }
+
+ // Sets the magnetometer algorithm's calibration data.
+ magCalReset(&mag_cal_); // Resets the magnetometer's offset vector.
+ magCalAddBias(&mag_cal_, input_cal_data.offset[0], input_cal_data.offset[1],
+ input_cal_data.offset[2]);
+
+ // Sync's all initial calibration data.
+ cal_data_ = input_cal_data;
+
+ // Sets the calibration quality to undetermined (uncertain magnetic history
+ // makes the usefulness of the input calibration value unknown).
+ cal_data_.calibration_quality.reset();
+
+ return true;
+}
+
+} // namespace online_calibration
diff --git a/firmware/os/algos/calibration/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.h b/firmware/os/algos/calibration/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.h
new file mode 100644
index 0000000..11ede3d
--- /dev/null
+++ b/firmware/os/algos/calibration/online_calibration/magnetometer/mag_diverse_cal/mag_diverse_cal.h
@@ -0,0 +1,82 @@
+/*
+ * Copyright (C) 2018 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_MAGNETOMETER_MAG_DIVERSE_CAL_MAG_DIVERSE_CAL_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_MAGNETOMETER_MAG_DIVERSE_CAL_MAG_DIVERSE_CAL_H_
+
+#include "calibration/diversity_checker/diversity_checker.h"
+#include "calibration/magnetometer/mag_cal/mag_cal.h"
+#include "calibration/online_calibration/common_data/calibration_callback.h"
+#include "calibration/online_calibration/common_data/calibration_data.h"
+#include "calibration/online_calibration/common_data/online_calibration.h"
+#include "calibration/online_calibration/common_data/sensor_data.h"
+
+namespace online_calibration {
+
+/*
+ * This class is a wrapper for the magnetometer offset calibration with
+ * diversity checking.
+ *
+ * NOTE: Calibration quality reporting:
+ * Initialize --> CalibrationQualityLevel::UNDETERMINED
+ * CalibrationQuality.value =
+ * kUndeterminedCalibrationQuality
+ * SetInitialCalibration --> CalibrationQualityLevel::UNDETERMINED
+ * CalibrationQuality.value =
+ * kUndeterminedCalibrationQuality
+ * New Calibration Update --> CalibrationQualityLevel::HIGH_QUALITY
+ * CalibrationQuality.value = kHighQualityUt
+ */
+class MagDiverseCal final : public OnlineCalibration<CalibrationDataThreeAxis> {
+ public:
+ // Empirically estimated upper bounds on offset error.
+ static constexpr float kLowQualityUt = 1000.0f; // Units of micro Tesla
+ static constexpr float kHighQualityUt = 5.0f; // Units of micro Tesla
+
+ MagDiverseCal() = default;
+
+ // Creates an MagDiverseCal with specified algorithm parameters.
+ MagDiverseCal(const MagCalParameters& mag_cal_parameters,
+ const DiversityCheckerParameters& diversity_parameters) {
+ Initialize(mag_cal_parameters, diversity_parameters);
+ }
+
+ // Initializes with specified algorithm parameters.
+ void Initialize(const MagCalParameters& mag_cal_parameters,
+ const DiversityCheckerParameters& diversity_parameters);
+
+ // Sends new sensor data to the calibration algorithm, and returns the state
+ // of the calibration update flags, 'cal_update_polling_flags_'.
+ CalibrationTypeFlags SetMeasurement(const SensorData& sample) final;
+
+ // Sets the initial calibration data of the calibration algorithm. Returns
+ // true if set successfully.
+ bool SetInitialCalibration(
+ const CalibrationDataThreeAxis& input_cal_data) final;
+
+ // Returns the calibration sensor type.
+ SensorType get_sensor_type() const final {
+ return SensorType::kMagnetometerUt;
+ };
+
+ private:
+ // MagCal algorithm data structure.
+ MagCal mag_cal_;
+};
+
+} // namespace online_calibration
+
+#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_MAGNETOMETER_MAG_DIVERSE_CAL_MAG_DIVERSE_CAL_H_
diff --git a/firmware/os/algos/calibration/over_temp/over_temp_cal.c b/firmware/os/algos/calibration/over_temp/over_temp_cal.c
index d92f1da..04a9c92 100644
--- a/firmware/os/algos/calibration/over_temp/over_temp_cal.c
+++ b/firmware/os/algos/calibration/over_temp/over_temp_cal.c
@@ -17,12 +17,11 @@
#include "calibration/over_temp/over_temp_cal.h"
#include <float.h>
+#include <inttypes.h>
#include <math.h>
-#include <stdio.h>
#include <string.h>
#include "calibration/util/cal_log.h"
-#include "common/math/macros.h"
#include "util/nano_assert.h"
/////// DEFINITIONS AND MACROS ////////////////////////////////////////////////
@@ -32,40 +31,39 @@
// Defines the default weighting function for the linear model fit routine.
// Weighting = 10.0; for offsets newer than 5 minutes.
-static const struct OverTempCalWeightPt kOtcDefaultWeight0 = {
+static const struct OverTempCalWeight kOtcDefaultWeight0 = {
.offset_age_nanos = MIN_TO_NANOS(5),
.weight = 10.0f,
};
// Weighting = 0.1; for offsets newer than 15 minutes.
-static const struct OverTempCalWeightPt kOtcDefaultWeight1 = {
+static const struct OverTempCalWeight kOtcDefaultWeight1 = {
.offset_age_nanos = MIN_TO_NANOS(15),
.weight = 0.1f,
};
// The default weighting used for all older offsets.
-#define OTC_MIN_WEIGHT_VALUE (0.04f)
+#define OTC_MIN_WEIGHT_VALUE (0.04f)
#ifdef OVERTEMPCAL_DBG_ENABLED
// A debug version label to help with tracking results.
-#define OTC_DEBUG_VERSION_STRING "[July 05, 2017]"
+#define OTC_DEBUG_VERSION_STRING "[Jan 10, 2018]"
// The time interval used to throttle debug messaging (100msec).
#define OTC_WAIT_TIME_NANOS (SEC_TO_NANOS(0.1))
-// The time interval used to throttle temperture print messaging (1 second).
+// The time interval used to throttle temperature print messaging (1 second).
#define OTC_PRINT_TEMP_NANOS (SEC_TO_NANOS(1))
// Sensor axis label definition with index correspondence: 0=X, 1=Y, 2=Z.
-static const char kDebugAxisLabel[3] = "XYZ";
+static const char kDebugAxisLabel[3] = "XYZ";
#endif // OVERTEMPCAL_DBG_ENABLED
/////// FORWARD DECLARATIONS //////////////////////////////////////////////////
// Updates the latest received model estimate data.
static void setLatestEstimate(struct OverTempCal *over_temp_cal,
- const float *offset, float offset_temp_celsius,
- uint64_t timestamp_nanos);
+ const float *offset, float offset_temp_celsius);
/*
* Determines if a new over-temperature model fit should be performed, and then
@@ -103,11 +101,10 @@
* Since it may take a while for an empty model to build up enough data to start
* producing new model parameter updates, the model collection can be
* jump-started by using the new model parameters to insert "fake" data in place
- * of actual sensor offset data. 'timestamp_nanos' sets the timestamp for the
- * new model data.
+ * of actual sensor offset data. The new model data 'offset_age_nanos' is set to
+ * zero.
*/
-static bool jumpStartModelData(struct OverTempCal *over_temp_cal,
- uint64_t timestamp_nanos);
+static bool jumpStartModelData(struct OverTempCal *over_temp_cal);
/*
* Computes a new model fit and provides updated model parameters for the
@@ -116,7 +113,6 @@
*
* INPUTS:
* over_temp_cal: Over-temp data structure.
- * timestamp_nanos: Current timestamp for the model update.
* OUTPUTS:
* temp_sensitivity: Updated modeled temperature sensitivity (array).
* sensor_intercept: Updated model intercept (array).
@@ -127,8 +123,7 @@
* Numerical Recipes: The Art of Scientific Computing. Cambridge, 1992.
*/
static void updateModel(const struct OverTempCal *over_temp_cal,
- uint64_t timestamp_nanos, float *temp_sensitivity,
- float *sensor_intercept);
+ float *temp_sensitivity, float *sensor_intercept);
/*
* Computes a new over-temperature compensated offset estimate based on the
@@ -183,7 +178,8 @@
// Returns "true" if the candidate linear model parameters are within the valid
// range, and not all zeros.
static bool isValidOtcLinearModel(const struct OverTempCal *over_temp_cal,
- float temp_sensitivity, float sensor_intercept);
+ float temp_sensitivity,
+ float sensor_intercept);
// Returns "true" if 'offset' and 'offset_temp_celsius' is valid.
static bool isValidOtcOffset(const float *offset, float offset_temp_celsius);
@@ -191,8 +187,12 @@
// Returns the least-squares weight based on the age of a particular offset
// estimate.
static float evaluateWeightingFunction(const struct OverTempCal *over_temp_cal,
- uint64_t offset_timestamp_nanos,
- uint64_t current_timestamp_nanos);
+ uint64_t offset_age_nanos);
+
+// Computes the age increment, adds it to the age of each OTC model data point,
+// and resets the age update counter.
+static void modelDataSetAgeUpdate(struct OverTempCal *over_temp_cal,
+ uint64_t timestamp_nanos);
// Updates 'compensated_offset' using the linear OTC model.
static void compensateWithLinearModel(struct OverTempCal *over_temp_cal,
@@ -208,9 +208,10 @@
float delta_temp_celsius);
// Provides an over-temperature compensated offset based on the 'estimate'.
-static void compensateWithEstimate(
- struct OverTempCal *over_temp_cal, uint64_t timestamp_nanos,
- struct OverTempCalDataPt *estimate, float temperature_celsius);
+static void compensateWithEstimate(struct OverTempCal *over_temp_cal,
+ uint64_t timestamp_nanos,
+ struct OverTempModelThreeAxis *estimate,
+ float temperature_celsius);
// Evaluates the nearest-temperature compensation (with linear extrapolation
// term due to temperature), and compares it with the compensation due to
@@ -233,7 +234,7 @@
#ifdef OVERTEMPCAL_DBG_ENABLED
// This helper function stores all of the debug tracking information necessary
// for printing log messages.
-static void updateDebugData(struct OverTempCal* over_temp_cal);
+static void updateDebugData(struct OverTempCal *over_temp_cal);
// Helper function that creates tag strings useful for identifying specific
// debug output data (embedded system friendly; not all systems have 'sprintf').
@@ -251,12 +252,7 @@
/////// FUNCTION DEFINITIONS //////////////////////////////////////////////////
void overTempCalInit(struct OverTempCal *over_temp_cal,
- size_t min_num_model_pts,
- uint64_t min_temp_update_period_nanos,
- float delta_temp_per_bin, float jump_tolerance,
- float outlier_limit, uint64_t age_limit_nanos,
- float temp_sensitivity_limit, float sensor_intercept_limit,
- float significant_offset_change, bool over_temp_enable) {
+ const struct OverTempCalParameters *parameters) {
ASSERT_NOT_NULL(over_temp_cal);
// Clears OverTempCal memory.
@@ -264,7 +260,7 @@
// Initializes the pointers to important sensor offset estimates.
over_temp_cal->nearest_offset = &over_temp_cal->model_data[0];
- over_temp_cal->latest_offset = NULL;
+ over_temp_cal->latest_offset = NULL;
// Initializes the OTC linear model parameters.
resetOtcLinearModel(over_temp_cal);
@@ -272,24 +268,22 @@
// Initializes the model identification parameters.
over_temp_cal->new_overtemp_model_available = false;
over_temp_cal->new_overtemp_offset_available = false;
- over_temp_cal->min_num_model_pts = min_num_model_pts;
- over_temp_cal->min_temp_update_period_nanos = min_temp_update_period_nanos;
- over_temp_cal->delta_temp_per_bin = delta_temp_per_bin;
- over_temp_cal->jump_tolerance = jump_tolerance;
- over_temp_cal->outlier_limit = outlier_limit;
- over_temp_cal->age_limit_nanos = age_limit_nanos;
- over_temp_cal->temp_sensitivity_limit = temp_sensitivity_limit;
- over_temp_cal->sensor_intercept_limit = sensor_intercept_limit;
- over_temp_cal->significant_offset_change = significant_offset_change;
- over_temp_cal->over_temp_enable = over_temp_enable;
+ over_temp_cal->min_num_model_pts = parameters->min_num_model_pts;
+ over_temp_cal->min_temp_update_period_nanos =
+ parameters->min_temp_update_period_nanos;
+ over_temp_cal->delta_temp_per_bin = parameters->delta_temp_per_bin;
+ over_temp_cal->jump_tolerance = parameters->jump_tolerance;
+ over_temp_cal->outlier_limit = parameters->outlier_limit;
+ over_temp_cal->age_limit_nanos = parameters->age_limit_nanos;
+ over_temp_cal->temp_sensitivity_limit = parameters->temp_sensitivity_limit;
+ over_temp_cal->sensor_intercept_limit = parameters->sensor_intercept_limit;
+ over_temp_cal->significant_offset_change =
+ parameters->significant_offset_change;
+ over_temp_cal->over_temp_enable = parameters->over_temp_enable;
// Initializes the over-temperature compensated offset temperature.
over_temp_cal->compensated_offset.offset_temp_celsius =
- OTC_TEMP_INVALID_CELSIUS;
-
- // Defines the default weighting function for the linear model fit routine.
- overTempSetWeightingFunction(over_temp_cal, 0, &kOtcDefaultWeight0);
- overTempSetWeightingFunction(over_temp_cal, 1, &kOtcDefaultWeight1);
+ INVALID_TEMPERATURE_CELSIUS;
#ifdef OVERTEMPCAL_DBG_ENABLED
// Sets the default sensor descriptors for debugging.
@@ -305,6 +299,10 @@
"Over-temperature compensation DISABLED.");
}
#endif // OVERTEMPCAL_DBG_ENABLED
+
+ // Defines the default weighting function for the linear model fit routine.
+ overTempValidateAndSetWeight(over_temp_cal, 0, &kOtcDefaultWeight0);
+ overTempValidateAndSetWeight(over_temp_cal, 1, &kOtcDefaultWeight1);
}
void overTempCalSetModel(struct OverTempCal *over_temp_cal, const float *offset,
@@ -332,8 +330,7 @@
// Model "Jump-Start".
const bool model_jump_started =
- (jump_start_model) ? jumpStartModelData(over_temp_cal, timestamp_nanos)
- : false;
+ jump_start_model ? jumpStartModelData(over_temp_cal) : false;
if (!model_jump_started) {
// Checks that the new offset data is valid.
@@ -342,7 +339,7 @@
memcpy(over_temp_cal->model_data[0].offset, offset,
sizeof(over_temp_cal->model_data[0].offset));
over_temp_cal->model_data[0].offset_temp_celsius = offset_temp_celsius;
- over_temp_cal->model_data[0].timestamp_nanos = timestamp_nanos;
+ over_temp_cal->model_data[0].offset_age_nanos = 0;
over_temp_cal->num_model_pts = 1;
} else {
// No valid offset data to load.
@@ -361,7 +358,7 @@
memcpy(over_temp_cal->compensated_offset.offset, offset,
sizeof(over_temp_cal->compensated_offset.offset));
over_temp_cal->compensated_offset.offset_temp_celsius = offset_temp_celsius;
- over_temp_cal->compensated_offset.timestamp_nanos = timestamp_nanos;
+ over_temp_cal->compensated_offset.offset_age_nanos = 0;
}
// Resets the latest offset pointer. There are no new offset estimates to
@@ -435,7 +432,7 @@
void overTempCalSetModelData(struct OverTempCal *over_temp_cal,
size_t data_length, uint64_t timestamp_nanos,
- const struct OverTempCalDataPt *model_data) {
+ const struct OverTempModelThreeAxis *model_data) {
ASSERT_NOT_NULL(over_temp_cal);
ASSERT_NOT_NULL(model_data);
@@ -446,11 +443,7 @@
if (isValidOtcOffset(model_data[i].offset,
model_data[i].offset_temp_celsius)) {
memcpy(&over_temp_cal->model_data[i], &model_data[i],
- sizeof(struct OverTempCalDataPt));
-
- // Updates the model time stamps to the current load time.
- over_temp_cal->model_data[i].timestamp_nanos = timestamp_nanos;
-
+ sizeof(struct OverTempModelThreeAxis));
valid_data_count++;
}
}
@@ -491,11 +484,11 @@
void overTempCalGetModelData(struct OverTempCal *over_temp_cal,
size_t *data_length,
- struct OverTempCalDataPt *model_data) {
+ struct OverTempModelThreeAxis *model_data) {
ASSERT_NOT_NULL(over_temp_cal);
*data_length = over_temp_cal->num_model_pts;
memcpy(model_data, over_temp_cal->model_data,
- over_temp_cal->num_model_pts * sizeof(struct OverTempCalDataPt));
+ over_temp_cal->num_model_pts * sizeof(struct OverTempModelThreeAxis));
}
void overTempCalGetOffset(struct OverTempCal *over_temp_cal,
@@ -559,6 +552,9 @@
ASSERT_NOT_NULL(offset);
ASSERT(over_temp_cal->delta_temp_per_bin > 0);
+ // Updates the age of each OTC model data point.
+ modelDataSetAgeUpdate(over_temp_cal, timestamp_nanos);
+
// Checks that the new offset data is valid, returns if bad.
if (!isValidOtcOffset(offset, temperature_celsius)) {
return;
@@ -586,14 +582,13 @@
createDebugTag(over_temp_cal, ":OUTLIER]");
CAL_DEBUG_LOG(
over_temp_cal->otc_debug_tag,
- "Offset|Temperature|Time [%s|C|nsec]: "
- CAL_FORMAT_3DIGITS_TRIPLET ", " CAL_FORMAT_3DIGITS ", %llu",
+ "Offset|Temperature|Time [%s|C|nsec]: " CAL_FORMAT_3DIGITS_TRIPLET
+ ", " CAL_FORMAT_3DIGITS ", %" PRIu64,
over_temp_cal->otc_unit_tag,
CAL_ENCODE_FLOAT(offset[0] * over_temp_cal->otc_unit_conversion, 3),
CAL_ENCODE_FLOAT(offset[1] * over_temp_cal->otc_unit_conversion, 3),
CAL_ENCODE_FLOAT(offset[2] * over_temp_cal->otc_unit_conversion, 3),
- CAL_ENCODE_FLOAT(temperature_celsius, 3),
- (unsigned long long int)timestamp_nanos);
+ CAL_ENCODE_FLOAT(temperature_celsius, 3), timestamp_nanos);
#endif // OVERTEMPCAL_DBG_ENABLED
return; // Outlier detected: skips adding this offset to the model.
@@ -648,8 +643,8 @@
// oldest data with the incoming one.
over_temp_cal->latest_offset = &over_temp_cal->model_data[0];
for (size_t i = 1; i < over_temp_cal->num_model_pts; i++) {
- if (over_temp_cal->latest_offset->timestamp_nanos <
- over_temp_cal->model_data[i].timestamp_nanos) {
+ if (over_temp_cal->latest_offset->offset_age_nanos <
+ over_temp_cal->model_data[i].offset_age_nanos) {
over_temp_cal->latest_offset = &over_temp_cal->model_data[i];
}
}
@@ -657,8 +652,7 @@
}
// Updates the latest model estimate data.
- setLatestEstimate(over_temp_cal, offset, temperature_celsius,
- timestamp_nanos);
+ setLatestEstimate(over_temp_cal, offset, temperature_celsius);
// The latest offset estimate is the nearest temperature offset.
over_temp_cal->nearest_offset = over_temp_cal->latest_offset;
@@ -705,13 +699,20 @@
// Prints out temperature and the current timestamp.
createDebugTag(over_temp_cal, ":TEMP]");
CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag,
- "Temperature|Time [C|nsec] = " CAL_FORMAT_3DIGITS ", %llu",
- CAL_ENCODE_FLOAT(temperature_celsius, 3),
- (unsigned long long int)timestamp_nanos);
+ "Temperature|Time [C|nsec] = " CAL_FORMAT_3DIGITS
+ ", %" PRIu64,
+ CAL_ENCODE_FLOAT(temperature_celsius, 3), timestamp_nanos);
}
#endif // OVERTEMPCAL_DBG_LOG_TEMP
#endif // OVERTEMPCAL_DBG_ENABLED
+ // Updates the age of each OTC model data point.
+ if (NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(
+ timestamp_nanos, over_temp_cal->last_age_update_nanos,
+ OTC_MODEL_AGE_UPDATE_NANOS)) {
+ modelDataSetAgeUpdate(over_temp_cal, timestamp_nanos);
+ }
+
// This check throttles new OTC offset compensation updates so that high data
// rate temperature samples do not cause excessive computational burden. Note,
// temperature sensor updates are expected to potentially increase the data
@@ -720,7 +721,7 @@
if (!NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(
timestamp_nanos, over_temp_cal->last_offset_update_nanos,
over_temp_cal->min_temp_update_period_nanos)) {
- return; // Time interval too short, skip further data processing.
+ return; // Time interval too short, skip further data processing.
}
// Checks that the offset temperature is within a valid range, saturates if
@@ -746,8 +747,8 @@
}
void overTempGetModelError(const struct OverTempCal *over_temp_cal,
- const float *temp_sensitivity, const float *sensor_intercept,
- float *max_error) {
+ const float *temp_sensitivity,
+ const float *sensor_intercept, float *max_error) {
ASSERT_NOT_NULL(over_temp_cal);
ASSERT_NOT_NULL(temp_sensitivity);
ASSERT_NOT_NULL(sensor_intercept);
@@ -770,14 +771,34 @@
}
}
-// TODO(davejacobs): Refactor to implement a compliance check on the storage of
-// 'offset_age_nanos' to ensure a monotonically increasing order with index.
-void overTempSetWeightingFunction(
+bool overTempValidateAndSetWeight(
struct OverTempCal *over_temp_cal, size_t index,
- const struct OverTempCalWeightPt *new_otc_weight) {
- if (index < OTC_NUM_WEIGHT_LEVELS) {
- over_temp_cal->weighting_function[index] = *new_otc_weight;
+ const struct OverTempCalWeight *new_otc_weight) {
+ ASSERT_NOT_NULL(over_temp_cal);
+ ASSERT_NOT_NULL(new_otc_weight);
+
+ // The input weighting coefficient must be positive.
+ if (new_otc_weight->weight <= 0.0f) {
+#ifdef OVERTEMPCAL_DBG_ENABLED
+ createDebugTag(over_temp_cal, ":WEIGHT_FUNCTION]");
+ CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag, "Invalid weight: Must > 0.");
+#endif // OVERTEMPCAL_DBG_ENABLED
+ return false;
}
+
+ // Ensures that the 'index-1' weight's age is younger.
+ if (index == 0 ||
+ over_temp_cal->weighting_function[index - 1].offset_age_nanos <
+ new_otc_weight->offset_age_nanos) {
+ over_temp_cal->weighting_function[index] = *new_otc_weight;
+ return true;
+ }
+
+#ifdef OVERTEMPCAL_DBG_ENABLED
+ createDebugTag(over_temp_cal, ":WEIGHT_FUNCTION]");
+ CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag, "Non monotonic weight age.");
+#endif // OVERTEMPCAL_DBG_ENABLED
+ return false;
}
/////// LOCAL HELPER FUNCTION DEFINITIONS /////////////////////////////////////
@@ -827,9 +848,10 @@
}
}
-void compensateWithEstimate(
- struct OverTempCal *over_temp_cal, uint64_t timestamp_nanos,
- struct OverTempCalDataPt *estimate, float temperature_celsius) {
+void compensateWithEstimate(struct OverTempCal *over_temp_cal,
+ uint64_t timestamp_nanos,
+ struct OverTempModelThreeAxis *estimate,
+ float temperature_celsius) {
ASSERT_NOT_NULL(over_temp_cal);
ASSERT_NOT_NULL(estimate);
@@ -838,7 +860,7 @@
memcpy(compensated_offset, estimate->offset, sizeof(compensated_offset));
// Checks that the offset temperature is valid.
- if (estimate->offset_temp_celsius > OTC_TEMP_INVALID_CELSIUS) {
+ if (estimate->offset_temp_celsius > INVALID_TEMPERATURE_CELSIUS) {
const float delta_temp_celsius =
temperature_celsius - estimate->offset_temp_celsius;
@@ -886,7 +908,7 @@
// Adds a delta term to the compensated offset using the temperature
// difference defined by 'delta_temp_celsius'.
if (over_temp_cal->compensated_offset.offset_temp_celsius <=
- OTC_TEMP_INVALID_CELSIUS) {
+ INVALID_TEMPERATURE_CELSIUS) {
// If temperature is invalid, then skip further processing.
break;
}
@@ -918,7 +940,7 @@
// If 'temperature_celsius' is invalid, then no changes to the compensated
// offset are computed.
- if (temperature_celsius <= OTC_TEMP_INVALID_CELSIUS) {
+ if (temperature_celsius <= INVALID_TEMPERATURE_CELSIUS) {
return;
}
@@ -927,9 +949,9 @@
// than one estimate in the model (i.e., don't want to remove all data, even
// if it is very old [something is likely better than nothing]).
if ((timestamp_nanos >=
- OTC_STALE_CHECK_TIME_NANOS + over_temp_cal->stale_data_timer) &&
+ OTC_STALE_CHECK_TIME_NANOS + over_temp_cal->stale_data_timer_nanos) &&
over_temp_cal->num_model_pts > 1) {
- over_temp_cal->stale_data_timer = timestamp_nanos; // Resets timer.
+ over_temp_cal->stale_data_timer_nanos = timestamp_nanos; // Resets timer.
removeStaleModelData(over_temp_cal, timestamp_nanos);
}
@@ -944,12 +966,32 @@
// not empty.
const bool model_points_available = (over_temp_cal->num_model_pts > 0);
+ // To properly evaluate the logic paths that use the latest and nearest offset
+ // data below, the current age of the nearest and latest offset estimates are
+ // computed.
+ uint64_t latest_offset_age_nanos = 0;
+ if (over_temp_cal->latest_offset != NULL) {
+ latest_offset_age_nanos =
+ (over_temp_cal->last_age_update_nanos < timestamp_nanos)
+ ? over_temp_cal->latest_offset->offset_age_nanos +
+ timestamp_nanos - over_temp_cal->last_age_update_nanos
+ : over_temp_cal->latest_offset->offset_age_nanos;
+ }
+
+ uint64_t nearest_offset_age_nanos = 0;
+ if (over_temp_cal->nearest_offset != NULL) {
+ nearest_offset_age_nanos =
+ (over_temp_cal->last_age_update_nanos < timestamp_nanos)
+ ? over_temp_cal->nearest_offset->offset_age_nanos +
+ timestamp_nanos - over_temp_cal->last_age_update_nanos
+ : over_temp_cal->nearest_offset->offset_age_nanos;
+ }
+
// True when the latest offset estimate will be used to compute a sensor
// offset calibration estimate.
const bool use_latest_offset_compensation =
- over_temp_cal->latest_offset && model_points_available &&
- timestamp_nanos < over_temp_cal->latest_offset->timestamp_nanos +
- OTC_USE_RECENT_OFFSET_TIME_NANOS;
+ over_temp_cal->latest_offset != NULL && model_points_available &&
+ latest_offset_age_nanos <= OTC_USE_RECENT_OFFSET_TIME_NANOS;
// True when the conditions are met to use the nearest-temperature offset to
// compute a sensor offset calibration estimate.
@@ -958,7 +1000,7 @@
// ii. Offset temperature must be within a small neighborhood of the
// current measured temperature (+/- 'delta_temp_per_bin').
const bool can_compensate_with_nearest =
- model_points_available && over_temp_cal->nearest_offset &&
+ model_points_available && over_temp_cal->nearest_offset != NULL &&
NANO_ABS(temperature_celsius -
over_temp_cal->nearest_offset->offset_temp_celsius) <
over_temp_cal->delta_temp_per_bin;
@@ -966,18 +1008,14 @@
// True if the last received sensor offset estimate is old or non-existent.
const bool latest_model_point_not_relevant =
(over_temp_cal->latest_offset == NULL) ||
- (over_temp_cal->latest_offset &&
- NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(
- timestamp_nanos, over_temp_cal->latest_offset->timestamp_nanos,
- OTC_OFFSET_IS_STALE_NANOS));
+ (over_temp_cal->latest_offset != NULL &&
+ latest_offset_age_nanos >= OTC_OFFSET_IS_STALE_NANOS);
// True if the nearest-temperature offset estimate is old or non-existent.
const bool nearest_model_point_not_relevant =
(over_temp_cal->nearest_offset == NULL) ||
- (over_temp_cal->nearest_offset &&
- NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(
- timestamp_nanos, over_temp_cal->nearest_offset->timestamp_nanos,
- OTC_OFFSET_IS_STALE_NANOS));
+ (over_temp_cal->nearest_offset != NULL &&
+ nearest_offset_age_nanos >= OTC_OFFSET_IS_STALE_NANOS);
// ---------------------------------------------------------------------------
// The following conditional expressions govern new OTC offset updates.
@@ -1051,26 +1089,25 @@
over_temp_cal->new_overtemp_offset_available |= new_overtemp_offset_available;
// If the offset has changed significantly, then the offset compensation
- // vector and timestamp are updated.
+ // vector is updated.
if (new_overtemp_offset_available) {
memcpy(over_temp_cal->compensated_offset.offset, compensated_offset,
sizeof(over_temp_cal->compensated_offset.offset));
- over_temp_cal->compensated_offset.timestamp_nanos = timestamp_nanos;
over_temp_cal->compensated_offset.offset_temp_celsius = temperature_celsius;
}
}
void setLatestEstimate(struct OverTempCal *over_temp_cal, const float *offset,
- float offset_temp_celsius, uint64_t timestamp_nanos) {
+ float offset_temp_celsius) {
ASSERT_NOT_NULL(over_temp_cal);
ASSERT_NOT_NULL(offset);
- if (over_temp_cal->latest_offset) {
+ if (over_temp_cal->latest_offset != NULL) {
// Sets the latest over-temp calibration estimate.
memcpy(over_temp_cal->latest_offset->offset, offset,
sizeof(over_temp_cal->latest_offset->offset));
over_temp_cal->latest_offset->offset_temp_celsius = offset_temp_celsius;
- over_temp_cal->latest_offset->timestamp_nanos = timestamp_nanos;
+ over_temp_cal->latest_offset->offset_age_nanos = 0;
}
}
@@ -1098,19 +1135,17 @@
// Ensures that the minimum number of points required for a model fit has been
// satisfied.
- if (over_temp_cal->num_model_pts < over_temp_cal->min_num_model_pts)
- return;
+ if (over_temp_cal->num_model_pts < over_temp_cal->min_num_model_pts) return;
// Updates the linear model fit.
float temp_sensitivity[3];
float sensor_intercept[3];
- updateModel(over_temp_cal, timestamp_nanos, temp_sensitivity,
- sensor_intercept);
+ updateModel(over_temp_cal, temp_sensitivity, sensor_intercept);
// 2) A new set of model parameters are accepted if:
// i. The model fit parameters must be within certain absolute bounds:
- // a. NANO_ABS(temp_sensitivity) < temp_sensitivity_limit
- // b. NANO_ABS(sensor_intercept) < sensor_intercept_limit
+ // a. |temp_sensitivity| < temp_sensitivity_limit
+ // b. |sensor_intercept| < sensor_intercept_limit
// NOTE: Model parameter updates are not qualified against model fit error
// here to protect against the case where there is large change in the
// temperature characteristic either during runtime (e.g., temperature
@@ -1131,14 +1166,14 @@
CAL_DEBUG_LOG(
over_temp_cal->otc_debug_tag,
"%c-Axis Parameters|Time [%s/C|%s|nsec]: " CAL_FORMAT_3DIGITS
- ", " CAL_FORMAT_3DIGITS ", %llu",
+ ", " CAL_FORMAT_3DIGITS ", %" PRIu64,
kDebugAxisLabel[i], over_temp_cal->otc_unit_tag,
over_temp_cal->otc_unit_tag,
CAL_ENCODE_FLOAT(
temp_sensitivity[i] * over_temp_cal->otc_unit_conversion, 3),
CAL_ENCODE_FLOAT(
sensor_intercept[i] * over_temp_cal->otc_unit_conversion, 3),
- (unsigned long long int)timestamp_nanos);
+ timestamp_nanos);
#endif // OVERTEMPCAL_DBG_ENABLED
}
}
@@ -1161,7 +1196,7 @@
ASSERT_NOT_NULL(over_temp_cal);
// If 'temperature_celsius' is invalid, then do not search.
- if (temperature_celsius <= OTC_TEMP_INVALID_CELSIUS) {
+ if (temperature_celsius <= INVALID_TEMPERATURE_CELSIUS) {
return;
}
@@ -1186,9 +1221,8 @@
bool removed_one = false;
for (size_t i = 0; i < over_temp_cal->num_model_pts; i++) {
- if (timestamp_nanos > over_temp_cal->model_data[i].timestamp_nanos &&
- timestamp_nanos > over_temp_cal->age_limit_nanos +
- over_temp_cal->model_data[i].timestamp_nanos) {
+ if (over_temp_cal->model_data[i].offset_age_nanos >=
+ over_temp_cal->age_limit_nanos) {
// If the latest offset was removed, then indicate this by setting it to
// NULL.
if (over_temp_cal->latest_offset == &over_temp_cal->model_data[i]) {
@@ -1223,8 +1257,8 @@
createDebugTag(over_temp_cal, ":REMOVE]");
CAL_DEBUG_LOG(
over_temp_cal->otc_debug_tag,
- "Offset|Temp|Time [%s|C|nsec]: " CAL_FORMAT_3DIGITS_TRIPLET
- ", " CAL_FORMAT_3DIGITS ", %llu",
+ "Offset|Temp|Age [%s|C|nsec]: " CAL_FORMAT_3DIGITS_TRIPLET
+ ", " CAL_FORMAT_3DIGITS ", %" PRIu64,
over_temp_cal->otc_unit_tag,
CAL_ENCODE_FLOAT(over_temp_cal->model_data[model_index].offset[0] *
over_temp_cal->otc_unit_conversion,
@@ -1237,22 +1271,20 @@
3),
CAL_ENCODE_FLOAT(
over_temp_cal->model_data[model_index].offset_temp_celsius, 3),
- (unsigned long long int)over_temp_cal->model_data[model_index]
- .timestamp_nanos);
+ over_temp_cal->model_data[model_index].offset_age_nanos);
#endif // OVERTEMPCAL_DBG_ENABLED
// Remove the model data at 'model_index'.
for (size_t i = model_index; i < over_temp_cal->num_model_pts - 1; i++) {
memcpy(&over_temp_cal->model_data[i], &over_temp_cal->model_data[i + 1],
- sizeof(struct OverTempCalDataPt));
+ sizeof(struct OverTempModelThreeAxis));
}
over_temp_cal->num_model_pts--;
return true;
}
-bool jumpStartModelData(struct OverTempCal *over_temp_cal,
- uint64_t timestamp_nanos) {
+bool jumpStartModelData(struct OverTempCal *over_temp_cal) {
ASSERT_NOT_NULL(over_temp_cal);
ASSERT(over_temp_cal->delta_temp_per_bin > 0);
@@ -1291,7 +1323,7 @@
over_temp_cal->sensor_intercept[j];
}
over_temp_cal->model_data[i].offset_temp_celsius = offset_temp_celsius;
- over_temp_cal->model_data[i].timestamp_nanos = timestamp_nanos;
+ over_temp_cal->model_data[i].offset_age_nanos = 0;
offset_temp_celsius += over_temp_cal->delta_temp_per_bin;
over_temp_cal->num_model_pts++;
@@ -1301,8 +1333,8 @@
createDebugTag(over_temp_cal, ":INIT]");
if (over_temp_cal->num_model_pts > 0) {
CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag,
- "Model Jump-Start: #Points = %lu.",
- (unsigned long int)over_temp_cal->num_model_pts);
+ "Model Jump-Start: #Points = %zu.",
+ over_temp_cal->num_model_pts);
}
#endif // OVERTEMPCAL_DBG_ENABLED
@@ -1310,8 +1342,7 @@
}
void updateModel(const struct OverTempCal *over_temp_cal,
- uint64_t timestamp_nanos, float *temp_sensitivity,
- float *sensor_intercept) {
+ float *temp_sensitivity, float *sensor_intercept) {
ASSERT_NOT_NULL(over_temp_cal);
ASSERT_NOT_NULL(temp_sensitivity);
ASSERT_NOT_NULL(sensor_intercept);
@@ -1328,8 +1359,7 @@
const size_t n = over_temp_cal->num_model_pts;
for (size_t i = 0; i < n; ++i) {
weight = evaluateWeightingFunction(
- over_temp_cal, over_temp_cal->model_data[i].timestamp_nanos,
- timestamp_nanos);
+ over_temp_cal, over_temp_cal->model_data[i].offset_age_nanos);
sw += weight;
st += over_temp_cal->model_data[i].offset_temp_celsius * weight;
@@ -1343,13 +1373,11 @@
const float inv_sw = 1.0f / sw;
for (size_t i = 0; i < n; ++i) {
weight = evaluateWeightingFunction(
- over_temp_cal, over_temp_cal->model_data[i].timestamp_nanos,
- timestamp_nanos);
+ over_temp_cal, over_temp_cal->model_data[i].offset_age_nanos);
const float t =
- over_temp_cal->model_data[i].offset_temp_celsius -
- st * inv_sw;
- stt += weight * t * t;
+ over_temp_cal->model_data[i].offset_temp_celsius - st * inv_sw;
+ stt += weight * t * t;
stsx += t * over_temp_cal->model_data[i].offset[0] * weight;
stsy += t * over_temp_cal->model_data[i].offset[1] * weight;
stsz += t * over_temp_cal->model_data[i].offset[2] * weight;
@@ -1440,13 +1468,11 @@
}
float evaluateWeightingFunction(const struct OverTempCal *over_temp_cal,
- uint64_t offset_timestamp_nanos,
- uint64_t current_timestamp_nanos) {
+ uint64_t offset_age_nanos) {
ASSERT_NOT_NULL(over_temp_cal);
for (size_t i = 0; i < OTC_NUM_WEIGHT_LEVELS; i++) {
- if (current_timestamp_nanos <=
- offset_timestamp_nanos +
- over_temp_cal->weighting_function[i].offset_age_nanos) {
+ if (offset_age_nanos <=
+ over_temp_cal->weighting_function[i].offset_age_nanos) {
return over_temp_cal->weighting_function[i].weight;
}
}
@@ -1455,6 +1481,26 @@
return OTC_MIN_WEIGHT_VALUE;
}
+void modelDataSetAgeUpdate(struct OverTempCal *over_temp_cal,
+ uint64_t timestamp_nanos) {
+ ASSERT_NOT_NULL(over_temp_cal);
+ if (over_temp_cal->last_age_update_nanos >= timestamp_nanos) {
+ // Age updates must be monotonic.
+ return;
+ }
+
+ uint64_t age_increment_nanos =
+ timestamp_nanos - over_temp_cal->last_age_update_nanos;
+
+ // Resets the age update counter.
+ over_temp_cal->last_age_update_nanos = timestamp_nanos;
+
+ // Updates the model dataset ages.
+ for (size_t i = 0; i < over_temp_cal->num_model_pts; i++) {
+ over_temp_cal->model_data[i].offset_age_nanos += age_increment_nanos;
+ }
+}
+
/////// DEBUG FUNCTION DEFINITIONS ////////////////////////////////////////////
#ifdef OVERTEMPCAL_DBG_ENABLED
@@ -1468,7 +1514,7 @@
new_debug_tag, strlen(new_debug_tag) + 1);
}
-void updateDebugData(struct OverTempCal* over_temp_cal) {
+void updateDebugData(struct OverTempCal *over_temp_cal) {
ASSERT_NOT_NULL(over_temp_cal);
// Only update this data if debug printing is not currently in progress
@@ -1502,13 +1548,13 @@
// If 'latest_offset' is defined the copy the data for debug printing.
// Otherwise, the current compensated offset will be printed.
- if (over_temp_cal->latest_offset) {
+ if (over_temp_cal->latest_offset != NULL) {
memcpy(&over_temp_cal->debug_overtempcal.latest_offset,
- over_temp_cal->latest_offset, sizeof(struct OverTempCalDataPt));
+ over_temp_cal->latest_offset, sizeof(struct OverTempModelThreeAxis));
} else {
memcpy(&over_temp_cal->debug_overtempcal.latest_offset,
&over_temp_cal->compensated_offset,
- sizeof(struct OverTempCalDataPt));
+ sizeof(struct OverTempModelThreeAxis));
}
// Total number of OTC model data points.
@@ -1516,9 +1562,9 @@
// Computes the maximum error over all of the model data.
overTempGetModelError(over_temp_cal,
- over_temp_cal->debug_overtempcal.temp_sensitivity,
- over_temp_cal->debug_overtempcal.sensor_intercept,
- over_temp_cal->debug_overtempcal.max_error);
+ over_temp_cal->debug_overtempcal.temp_sensitivity,
+ over_temp_cal->debug_overtempcal.sensor_intercept,
+ over_temp_cal->debug_overtempcal.max_error);
}
void overTempCalDebugPrint(struct OverTempCal *over_temp_cal,
@@ -1554,10 +1600,9 @@
// Prints out the latest offset estimate (input data).
CAL_DEBUG_LOG(
over_temp_cal->otc_debug_tag,
- "Cal#|Offset|Temp|Time [%s|C|nsec]: %lu, " CAL_FORMAT_3DIGITS_TRIPLET
- ", " CAL_FORMAT_3DIGITS ", %llu",
- over_temp_cal->otc_unit_tag,
- (unsigned long int)over_temp_cal->debug_num_estimates,
+ "Cal#|Offset|Temp|Age [%s|C|nsec]: %zu, " CAL_FORMAT_3DIGITS_TRIPLET
+ ", " CAL_FORMAT_3DIGITS ", %" PRIu64,
+ over_temp_cal->otc_unit_tag, over_temp_cal->debug_num_estimates,
CAL_ENCODE_FLOAT(
over_temp_cal->debug_overtempcal.latest_offset.offset[0] *
over_temp_cal->otc_unit_conversion,
@@ -1573,40 +1618,40 @@
CAL_ENCODE_FLOAT(over_temp_cal->debug_overtempcal.latest_offset
.offset_temp_celsius,
3),
- (unsigned long long int)
- over_temp_cal->debug_overtempcal.latest_offset.timestamp_nanos);
+ over_temp_cal->debug_overtempcal.latest_offset.offset_age_nanos);
+ // clang-format off
over_temp_cal->wait_timer_nanos =
timestamp_nanos; // Starts the wait timer.
over_temp_cal->next_state =
OTC_PRINT_MODEL_PARAMETERS; // Sets the next state.
over_temp_cal->debug_state = OTC_WAIT_STATE; // First, go to wait state.
+ // clang-format on
break;
case OTC_PRINT_MODEL_PARAMETERS:
// Prints out the model parameters.
- CAL_DEBUG_LOG(
- over_temp_cal->otc_debug_tag,
- "Cal#|Sensitivity [%s/C]: %lu, " CAL_FORMAT_3DIGITS_TRIPLET,
- over_temp_cal->otc_unit_tag,
- (unsigned long int)over_temp_cal->debug_num_estimates,
- CAL_ENCODE_FLOAT(
- over_temp_cal->debug_overtempcal.temp_sensitivity[0] *
- over_temp_cal->otc_unit_conversion,
- 3),
- CAL_ENCODE_FLOAT(
- over_temp_cal->debug_overtempcal.temp_sensitivity[1] *
- over_temp_cal->otc_unit_conversion,
- 3),
- CAL_ENCODE_FLOAT(
- over_temp_cal->debug_overtempcal.temp_sensitivity[2] *
- over_temp_cal->otc_unit_conversion,
- 3));
+ CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag,
+ "Cal#|Sensitivity [%s/C]: %zu, " CAL_FORMAT_3DIGITS_TRIPLET,
+ over_temp_cal->otc_unit_tag,
+ over_temp_cal->debug_num_estimates,
+ CAL_ENCODE_FLOAT(
+ over_temp_cal->debug_overtempcal.temp_sensitivity[0] *
+ over_temp_cal->otc_unit_conversion,
+ 3),
+ CAL_ENCODE_FLOAT(
+ over_temp_cal->debug_overtempcal.temp_sensitivity[1] *
+ over_temp_cal->otc_unit_conversion,
+ 3),
+ CAL_ENCODE_FLOAT(
+ over_temp_cal->debug_overtempcal.temp_sensitivity[2] *
+ over_temp_cal->otc_unit_conversion,
+ 3));
CAL_DEBUG_LOG(over_temp_cal->otc_debug_tag,
- "Cal#|Intercept [%s]: %lu, " CAL_FORMAT_3DIGITS_TRIPLET,
+ "Cal#|Intercept [%s]: %zu, " CAL_FORMAT_3DIGITS_TRIPLET,
over_temp_cal->otc_unit_tag,
- (unsigned long int)over_temp_cal->debug_num_estimates,
+ over_temp_cal->debug_num_estimates,
CAL_ENCODE_FLOAT(
over_temp_cal->debug_overtempcal.sensor_intercept[0] *
over_temp_cal->otc_unit_conversion,
@@ -1621,8 +1666,9 @@
3));
over_temp_cal->wait_timer_nanos =
- timestamp_nanos; // Starts the wait timer.
- over_temp_cal->next_state = OTC_PRINT_MODEL_ERROR; // Sets the next state.
+ timestamp_nanos; // Starts the wait timer.
+ over_temp_cal->next_state =
+ OTC_PRINT_MODEL_ERROR; // Sets the next state.
over_temp_cal->debug_state = OTC_WAIT_STATE; // First, go to wait state.
break;
@@ -1630,12 +1676,11 @@
// Computes the maximum error over all of the model data.
CAL_DEBUG_LOG(
over_temp_cal->otc_debug_tag,
- "Cal#|#Updates|#ModelPts|Model Error [%s]: %lu, "
- "%lu, %lu, " CAL_FORMAT_3DIGITS_TRIPLET,
- over_temp_cal->otc_unit_tag,
- (unsigned long int)over_temp_cal->debug_num_estimates,
- (unsigned long int)over_temp_cal->debug_num_model_updates,
- (unsigned long int)over_temp_cal->debug_overtempcal.num_model_pts,
+ "Cal#|#Updates|#ModelPts|Model Error [%s]: %zu, "
+ "%zu, %zu, " CAL_FORMAT_3DIGITS_TRIPLET,
+ over_temp_cal->otc_unit_tag, over_temp_cal->debug_num_estimates,
+ over_temp_cal->debug_num_model_updates,
+ over_temp_cal->debug_overtempcal.num_model_pts,
CAL_ENCODE_FLOAT(over_temp_cal->debug_overtempcal.max_error[0] *
over_temp_cal->otc_unit_conversion,
3),
@@ -1648,7 +1693,7 @@
over_temp_cal->model_counter = 0; // Resets the model data print counter.
over_temp_cal->wait_timer_nanos =
- timestamp_nanos; // Starts the wait timer.
+ timestamp_nanos; // Starts the wait timer.
over_temp_cal->next_state = OTC_PRINT_MODEL_DATA; // Sets the next state.
over_temp_cal->debug_state = OTC_WAIT_STATE; // First, go to wait state.
break;
@@ -1658,10 +1703,9 @@
if (over_temp_cal->model_counter < over_temp_cal->num_model_pts) {
CAL_DEBUG_LOG(
over_temp_cal->otc_debug_tag,
- " Model[%lu] [%s|C|nsec] = " CAL_FORMAT_3DIGITS_TRIPLET
- ", " CAL_FORMAT_3DIGITS ", %llu",
- (unsigned long int)over_temp_cal->model_counter,
- over_temp_cal->otc_unit_tag,
+ " Model[%zu] [%s|C|nsec] = " CAL_FORMAT_3DIGITS_TRIPLET
+ ", " CAL_FORMAT_3DIGITS ", %" PRIu64,
+ over_temp_cal->model_counter, over_temp_cal->otc_unit_tag,
CAL_ENCODE_FLOAT(
over_temp_cal->model_data[over_temp_cal->model_counter]
.offset[0] *
@@ -1681,24 +1725,23 @@
over_temp_cal->model_data[over_temp_cal->model_counter]
.offset_temp_celsius,
3),
- (unsigned long long int)over_temp_cal
- ->model_data[over_temp_cal->model_counter]
- .timestamp_nanos);
+ over_temp_cal->model_data[over_temp_cal->model_counter]
+ .offset_age_nanos);
over_temp_cal->model_counter++;
over_temp_cal->wait_timer_nanos =
- timestamp_nanos; // Starts the wait timer.
+ timestamp_nanos; // Starts the wait timer.
over_temp_cal->next_state =
- OTC_PRINT_MODEL_DATA; // Sets the next state.
+ OTC_PRINT_MODEL_DATA; // Sets the next state.
over_temp_cal->debug_state =
- OTC_WAIT_STATE; // First, go to wait state.
+ OTC_WAIT_STATE; // First, go to wait state.
} else {
// Sends this state machine to its idle state.
over_temp_cal->wait_timer_nanos =
- timestamp_nanos; // Starts the wait timer.
- over_temp_cal->next_state = OTC_IDLE; // Sets the next state.
+ timestamp_nanos; // Starts the wait timer.
+ over_temp_cal->next_state = OTC_IDLE; // Sets the next state.
over_temp_cal->debug_state =
- OTC_WAIT_STATE; // First, go to wait state.
+ OTC_WAIT_STATE; // First, go to wait state.
}
break;
diff --git a/firmware/os/algos/calibration/over_temp/over_temp_cal.h b/firmware/os/algos/calibration/over_temp/over_temp_cal.h
index 8a404d3..8f6f0a4 100644
--- a/firmware/os/algos/calibration/over_temp/over_temp_cal.h
+++ b/firmware/os/algos/calibration/over_temp/over_temp_cal.h
@@ -100,13 +100,13 @@
#include <stddef.h>
#include <stdint.h>
+#include "calibration/over_temp/over_temp_model.h"
+#include "common/math/macros.h"
+
#ifdef __cplusplus
extern "C" {
#endif
-// Defines the maximum size of the 'model_data' array.
-#define OTC_MODEL_SIZE (40)
-
// A common sensor operating temperature at which to begin the model jump-start
// data.
#define JUMPSTART_START_TEMP_CELSIUS (30.0f)
@@ -122,13 +122,13 @@
#define OTC_TEMP_MIN_CELSIUS (-40.0f)
#define OTC_TEMP_MAX_CELSIUS (85.0f)
-// Invalid sensor temperature.
-#define OTC_TEMP_INVALID_CELSIUS (-274.0f)
-
// Number of time-interval levels used to define the least-squares weighting
// function.
#define OTC_NUM_WEIGHT_LEVELS (2)
+// The time interval used to update the model data age.
+#define OTC_MODEL_AGE_UPDATE_NANOS (MIN_TO_NANOS(1))
+
// Rate-limits the check of old data to every 2 hours.
#define OTC_STALE_CHECK_TIME_NANOS (HRS_TO_NANOS(2))
@@ -143,7 +143,7 @@
#define OTC_REFRESH_MODEL_NANOS (SEC_TO_NANOS(30))
// Defines a weighting function value for the linear model fit routine.
-struct OverTempCalWeightPt {
+struct OverTempCalWeight {
// The age limit below which an offset will use this weight value.
uint64_t offset_age_nanos;
@@ -151,14 +151,6 @@
float weight;
};
-// Over-temperature sensor offset estimate structure.
-struct OverTempCalDataPt {
- // Sensor offset estimate, temperature, and timestamp.
- uint64_t timestamp_nanos; // [nanoseconds]
- float offset_temp_celsius; // [Celsius]
- float offset[3];
-};
-
#ifdef OVERTEMPCAL_DBG_ENABLED
// Debug printout state enumeration.
enum OverTempCalDebugState {
@@ -173,7 +165,7 @@
// OverTempCal debug information/data tracking structure.
struct DebugOverTempCal {
// The latest received offset estimate data.
- struct OverTempCalDataPt latest_offset;
+ struct OverTempModelThreeAxis latest_offset;
// The maximum model error over all model_data points.
float max_error[3];
@@ -184,38 +176,55 @@
};
#endif // OVERTEMPCAL_DBG_ENABLED
+// OverTempCal algorithm parameters (see OverTempCal for details).
+struct OverTempCalParameters {
+ uint64_t min_temp_update_period_nanos;
+ uint64_t age_limit_nanos;
+ float delta_temp_per_bin; // [Celsius/bin]
+ float jump_tolerance; // [sensor units]
+ float outlier_limit; // [sensor units/Celsius]
+ float temp_sensitivity_limit; // [sensor units/Celsius]
+ float sensor_intercept_limit; // [sensor units]
+ float significant_offset_change; // [sensor units]
+ size_t min_num_model_pts;
+ bool over_temp_enable;
+};
+
// The following data structure contains all of the necessary components for
// modeling a sensor's temperature dependency and providing over-temperature
// offset corrections.
struct OverTempCal {
// Storage for over-temperature model data.
- struct OverTempCalDataPt model_data[OTC_MODEL_SIZE];
+ struct OverTempModelThreeAxis model_data[OTC_MODEL_SIZE];
// Implements a weighting function to emphasize fitting a linear model to
// younger offset estimates.
- struct OverTempCalWeightPt weighting_function[OTC_NUM_WEIGHT_LEVELS];
+ struct OverTempCalWeight weighting_function[OTC_NUM_WEIGHT_LEVELS];
// The active over-temperature compensated offset estimate data. Contains the
// current sensor temperature at which offset compensation is performed.
- struct OverTempCalDataPt compensated_offset;
+ struct OverTempModelThreeAxis compensated_offset;
// Timer used to limit the rate at which old estimates are removed from
// the 'model_data' collection.
- uint64_t stale_data_timer; // [nanoseconds]
+ uint64_t stale_data_timer_nanos;
// Duration beyond which data will be removed to avoid corrupting the model
// with drift-compromised data.
- uint64_t age_limit_nanos; // [nanoseconds]
+ uint64_t age_limit_nanos;
// Timestamp of the last OTC offset compensation update.
- uint64_t last_offset_update_nanos; // [nanoseconds]
+ uint64_t last_offset_update_nanos;
// Timestamp of the last OTC model update.
- uint64_t last_model_update_nanos; // [nanoseconds]
+ uint64_t last_model_update_nanos;
+
+ // Timestamp of the last OTC model dataset age update.
+ uint64_t last_age_update_nanos;
// Limit on the minimum interval for offset update calculations resulting from
// an arbitrarily high temperature sampling rate.
- uint64_t min_temp_update_period_nanos; // [nanoseconds]
+ uint64_t min_temp_update_period_nanos;
///// Online Model Identification Parameters ////////////////////////////////
//
@@ -229,17 +238,17 @@
// model_temp_span >= 'num_model_pts' * delta_temp_per_bin
// 2) A new set of model parameters are accepted if:
// i. The model fit parameters must be within certain absolute bounds:
- // a. ABS(temp_sensitivity) < temp_sensitivity_limit
- // b. ABS(sensor_intercept) < sensor_intercept_limit
- float temp_sensitivity_limit; // [sensor units/Celsius]
- float sensor_intercept_limit; // [sensor units]
+ // a. |temp_sensitivity| < temp_sensitivity_limit
+ // b. |sensor_intercept| < sensor_intercept_limit
+ float temp_sensitivity_limit; // [sensor units/Celsius]
+ float sensor_intercept_limit; // [sensor units]
size_t min_num_model_pts;
// Pointer to the offset estimate closest to the current sensor temperature.
- struct OverTempCalDataPt *nearest_offset;
+ struct OverTempModelThreeAxis *nearest_offset;
// Pointer to the most recent offset estimate.
- struct OverTempCalDataPt *latest_offset;
+ struct OverTempModelThreeAxis *latest_offset;
// Modeled temperature sensitivity, dOffset/dTemp [sensor_units/Celsius].
float temp_sensitivity[3];
@@ -251,15 +260,15 @@
// above which the model fit is preferred for providing offset compensation
// (also applies to checks between the nearest-temperature and the current
// compensated estimate).
- float jump_tolerance; // [sensor units]
+ float jump_tolerance; // [sensor units]
// A limit used to reject new offset estimates that deviate from the current
// model fit.
- float outlier_limit; // [sensor units]
+ float outlier_limit; // [sensor units]
// This parameter is used to detect offset changes that require updates to
// system calibration and persistent memory storage.
- float significant_offset_change; // [sensor units]
+ float significant_offset_change; // [sensor units]
// Used to track the previous significant change in temperature.
float last_temp_check_celsius;
@@ -282,7 +291,7 @@
// recent estimates in cases where they arrive frequently near a given
// temperature, and prevents model oversampling (i.e., dominance of estimates
// concentrated at a given set of temperatures).
- float delta_temp_per_bin; // [Celsius/bin]
+ float delta_temp_per_bin; // [Celsius/bin]
// Total number of model data points collected.
size_t num_model_pts;
@@ -299,8 +308,7 @@
// overTempCalNewModelUpdateAvailable() is called. This variable indicates
// that the following should be stored in persistent system memory:
// 1) 'temp_sensitivity' and 'sensor_intercept'.
- // 2) The 'compensated_offset' offset data
- // (saving timestamp information is not required).
+ // 2) The 'compensated_offset' offset data.
bool new_overtemp_model_available;
// True when a new offset estimate has been computed and registers as a
@@ -320,15 +328,15 @@
uint64_t temperature_print_timer;
#endif // OVERTEMPCAL_DBG_LOG_TEMP
- size_t model_counter; // Model output print counter.
- float otc_unit_conversion; // Unit conversion for debug display.
- char otc_unit_tag[16]; // Unit descriptor (e.g., "mDPS").
- char otc_sensor_tag[16]; // OTC sensor descriptor (e.g., "GYRO").
- char otc_debug_tag[32]; // Temporary string descriptor.
- size_t debug_num_model_updates; // Total number of model updates.
- size_t debug_num_estimates; // Total number of offset estimates.
- bool debug_print_trigger; // Flag used to trigger data printout.
-#endif // OVERTEMPCAL_DBG_ENABLED
+ size_t model_counter; // Model output print counter.
+ float otc_unit_conversion; // Unit conversion for debug display.
+ char otc_unit_tag[16]; // Unit descriptor (e.g., "mDPS").
+ char otc_sensor_tag[16]; // OTC sensor descriptor (e.g., "GYRO").
+ char otc_debug_tag[32]; // Temporary string descriptor.
+ size_t debug_num_model_updates; // Total number of model updates.
+ size_t debug_num_estimates; // Total number of offset estimates.
+ bool debug_print_trigger; // Flag used to trigger data printout.
+#endif // OVERTEMPCAL_DBG_ENABLED
};
/////// FUNCTION PROTOTYPES ///////////////////////////////////////////////////
@@ -338,6 +346,9 @@
*
* INPUTS:
* over_temp_cal: Over-temp main data structure.
+ * parameters: An algorithm parameters that contains the
+ * following initialization variables.
+ * [parameters]:
* min_num_model_pts: Minimum number of model points per model
* calculation update.
* min_temp_update_period_nanos: Limits the rate of offset updates due to an
@@ -351,19 +362,14 @@
* temp_sensitivity_limit: Values that define the upper limits for the
* sensor_intercept_limit: model parameters. The acceptance of new model
* parameters must satisfy:
- * i. ABS(temp_sensitivity) < temp_sensitivity_limit
- * ii. ABS(sensor_intercept) < sensor_intercept_limit
+ * i. |temp_sensitivity| < temp_sensitivity_limit
+ * ii. |sensor_intercept| < sensor_intercept_limit
* significant_offset_change Minimum limit that triggers offset updates.
* over_temp_enable: Flag that determines whether over-temp sensor
* offset compensation is applied.
*/
void overTempCalInit(struct OverTempCal *over_temp_cal,
- size_t min_num_model_pts,
- uint64_t min_temp_update_period_nanos,
- float delta_temp_per_bin, float jump_tolerance,
- float outlier_limit, uint64_t age_limit_nanos,
- float temp_sensitivity_limit, float sensor_intercept_limit,
- float significant_offset_change, bool over_temp_enable);
+ const struct OverTempCalParameters *parameters);
/*
* Sets the over-temp calibration model parameters.
@@ -417,7 +423,7 @@
*/
void overTempCalSetModelData(struct OverTempCal *over_temp_cal,
size_t data_length, uint64_t timestamp_nanos,
- const struct OverTempCalDataPt *model_data);
+ const struct OverTempModelThreeAxis *model_data);
/*
* Gets the over-temp compensation model data set.
@@ -432,7 +438,7 @@
*/
void overTempCalGetModelData(struct OverTempCal *over_temp_cal,
size_t *data_length,
- struct OverTempCalDataPt *model_data);
+ struct OverTempModelThreeAxis *model_data);
/*
* Gets the current over-temp compensated offset estimate data.
@@ -472,8 +478,8 @@
bool overTempCalNewOffsetAvailable(struct OverTempCal *over_temp_cal);
/*
- * Updates the sensor's offset estimate and conditionally assimilates it into
- * the over-temp model data set, 'model_data'.
+ * Updates the sensor's offset estimate and conditionally incorporates it into
+ * the over-temp model data set, 'model_data'. Updates the model dataset age.
*
* INPUTS:
* over_temp_cal: Over-temp data structure.
@@ -491,7 +497,9 @@
// Updates the temperature at which the offset compensation is performed (i.e.,
// the current measured temperature value). This function is provided mainly for
// flexibility since temperature updates may come in from a source other than
-// the sensor itself, and at a different rate.
+// the sensor itself, and at a different rate. Since this function is
+// periodically called, it is also used to update the age of the model
+// estimates.
void overTempCalSetTemperature(struct OverTempCal *over_temp_cal,
uint64_t timestamp_nanos,
float temperature_celsius);
@@ -527,7 +535,9 @@
* age is less than 'offset_age_nanos'. NOTE: The ordering of the
* 'offset_age_nanos' values in the weight function array should be
* monotonically increasing from lowest index to highest so that weighting
- * selection can be conveniently evaluated.
+ * selection can be conveniently evaluated. A simple compliance check is
+ * applied, and 'true' is returned when the input weight is positive and older
+ * than the 'index-1' weight's age.
*
* INPUTS:
* over_temp_cal: Over-temp data structure.
@@ -536,9 +546,9 @@
* value and corresponding age limit below which an offset
* will use the weight.
*/
-void overTempSetWeightingFunction(
+bool overTempValidateAndSetWeight(
struct OverTempCal *over_temp_cal, size_t index,
- const struct OverTempCalWeightPt *new_otc_weight);
+ const struct OverTempCalWeight *new_otc_weight);
#ifdef OVERTEMPCAL_DBG_ENABLED
// This debug printout function assumes the input sensor data is a gyroscope
diff --git a/firmware/os/algos/calibration/over_temp/over_temp_model.h b/firmware/os/algos/calibration/over_temp/over_temp_model.h
new file mode 100644
index 0000000..f359c3b
--- /dev/null
+++ b/firmware/os/algos/calibration/over_temp/over_temp_model.h
@@ -0,0 +1,57 @@
+/*
+ * Copyright (C) 2018 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_OVER_TEMP_OVER_TEMP_MODEL_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_OVER_TEMP_OVER_TEMP_MODEL_H_
+
+#include <stdint.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// Defines the maximum size of the OverTempCal 'model_data' array.
+#define OTC_MODEL_SIZE (40)
+
+/*
+ * Over-temperature data structures that contain a modeled sensor offset
+ * estimate, an associated temperature, and the age of the data point since it
+ * first entered the model.
+ */
+
+struct OverTempModelThreeAxis {
+ // Sensor offset estimate, temperature, and age timestamp.
+ uint64_t offset_age_nanos; // [nanoseconds]
+ float offset_temp_celsius; // [Celsius]
+
+ // Three-axis offset estimate (indices: 0=x, 1=y, 2=z).
+ float offset[3];
+};
+
+struct OverTempModelSingleAxis {
+ // Sensor offset estimate, temperature, and age timestamp.
+ uint64_t offset_age_nanos; // [nanoseconds]
+ float offset_temp_celsius; // [Celsius]
+
+ // Single-axis offset estimate.
+ float offset;
+};
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_OVER_TEMP_OVER_TEMP_MODEL_H_
diff --git a/firmware/os/algos/calibration/sample_rate_estimator/sample_rate_estimator.c b/firmware/os/algos/calibration/sample_rate_estimator/sample_rate_estimator.c
new file mode 100644
index 0000000..d1b6619
--- /dev/null
+++ b/firmware/os/algos/calibration/sample_rate_estimator/sample_rate_estimator.c
@@ -0,0 +1,97 @@
+/*
+ * Copyright (C) 2018 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "calibration/sample_rate_estimator/sample_rate_estimator.h"
+
+#include <string.h>
+
+#include "common/math/macros.h"
+#include "util/nano_assert.h"
+
+// Helper function used to reset the sampling rate estimator accumulator.
+static void sampleRateEstimatorResetAccumulator(
+ struct SampleRateEstimator* sample_rate_estimator) {
+ sample_rate_estimator->last_timestamp_nanos = 0.0f;
+ sample_rate_estimator->interval_accumulator_nanos = 0.0f;
+ sample_rate_estimator->num_intervals_collected = 0;
+}
+
+void sampleRateEstimatorInit(struct SampleRateEstimator* sample_rate_estimator,
+ size_t num_intervals_to_collect,
+ float max_interval_sec) {
+ ASSERT_NOT_NULL(sample_rate_estimator);
+ memset(sample_rate_estimator, 0, sizeof(struct SampleRateEstimator));
+ sample_rate_estimator->mean_sampling_rate_estimate_hz =
+ SAMPLE_RATE_ESTIMATOR_INVALID_SAMPLE_RATE_HZ;
+ sample_rate_estimator->num_intervals_to_collect = num_intervals_to_collect;
+ sample_rate_estimator->max_interval_nanos =
+ max_interval_sec * SEC_TO_NANOS(1);
+}
+
+float sampleRateEstimatorGetHz(
+ struct SampleRateEstimator* sample_rate_estimator) {
+ sample_rate_estimator->new_sampling_rate_estimate_ready = false;
+ return sample_rate_estimator->mean_sampling_rate_estimate_hz;
+}
+
+void sampleRateEstimatorUpdate(
+ struct SampleRateEstimator* sample_rate_estimator,
+ uint64_t timestamp_nanos) {
+ // Resets the current interval capture and returns if:
+ // 1. A bad timestamp was received (i.e., time not monotonic).
+ // 2. 'last_timestamp_nanos' is zero. NOTE: 'last_timestamp_nanos' = 0
+ // indicates that the first complete time interval has not been captured
+ // yet (so, set it and return).
+ if (timestamp_nanos <= sample_rate_estimator->last_timestamp_nanos ||
+ sample_rate_estimator->last_timestamp_nanos == 0) {
+ sample_rate_estimator->last_timestamp_nanos = timestamp_nanos;
+ return;
+ }
+
+ // Computes the current sampling interval. This conversion will be very fast
+ // for intervals less than ~4.3 seconds (i.e., 2^32 nano-seconds).
+ const float next_interval_nanos = floatFromUint64(
+ timestamp_nanos - sample_rate_estimator->last_timestamp_nanos);
+
+ // Helps prevent corruption of the estimator when there are gaps in the input
+ // sampling intervals greater than 'max_interval_nanos' (i.e., intermittant
+ // periods where there are no input timestamps).
+ if (next_interval_nanos >= sample_rate_estimator->max_interval_nanos) {
+ // Resets the estimator and returns.
+ sampleRateEstimatorResetAccumulator(sample_rate_estimator);
+ return;
+ }
+
+ // Accumulates the next sampling interval.
+ sample_rate_estimator->interval_accumulator_nanos += next_interval_nanos;
+ sample_rate_estimator->last_timestamp_nanos = timestamp_nanos;
+ sample_rate_estimator->num_intervals_collected++;
+
+ // If the number of collected time intervals exceed the target number, then
+ // this computes a new sample rate estimate.
+ if (sample_rate_estimator->num_intervals_collected >
+ sample_rate_estimator->num_intervals_to_collect) {
+ sample_rate_estimator->mean_sampling_rate_estimate_hz =
+ sample_rate_estimator->num_intervals_collected *
+ (SEC_TO_NANOS(1) / sample_rate_estimator->interval_accumulator_nanos);
+
+ // Sets the polling flag to indicate that a new estimate is ready.
+ sample_rate_estimator->new_sampling_rate_estimate_ready = true;
+
+ // Resets the estimator variables.
+ sampleRateEstimatorResetAccumulator(sample_rate_estimator);
+ }
+}
diff --git a/firmware/os/algos/calibration/sample_rate_estimator/sample_rate_estimator.h b/firmware/os/algos/calibration/sample_rate_estimator/sample_rate_estimator.h
new file mode 100644
index 0000000..f506eea
--- /dev/null
+++ b/firmware/os/algos/calibration/sample_rate_estimator/sample_rate_estimator.h
@@ -0,0 +1,123 @@
+/*
+ * Copyright (C) 2018 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+/*
+ * [Sample Rate Estimator]
+ * This module periodically estimates the mean sampling rate of a uniformly
+ * sampled signal. Note, this estimator uses a floating point accumulator for
+ * the timing intervals. This trades-off some accumulation of finite precision
+ * error to enhance the range of estimated sampling rates and prevent overflow
+ * when an equivalent 32bit integer accumulator is used. In typical use cases
+ * (sample rates: 5Hz - 2kHz, num_intervals_to_collect 10 - 100), the sample
+ * rate accuracy is typically much better than 0.1Hz.
+ *
+ * FUNCTIONALITY:
+ * sampleRateEstimatorInit -- Initializes the estimator. Sets the number of time
+ * intervals require to compute the sample rate mean, and the upper limit for
+ * the acceptable time interval.
+ *
+ * new_sampling_rate_estimate_ready -- Check this boolean flag if polling for
+ * new estimates is desired.
+ *
+ * sampleRateEstimatorGetHz -- Returns the latest sample rate estimate and
+ * resets the polling flag.
+ *
+ * sampleRateEstimatorUpdate -- Processes new timestamp data and computes new
+ * sample rate estimates.
+ */
+
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_SAMPLE_RATE_ESTIMATOR_SAMPLE_RATE_ESTIMATOR_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_SAMPLE_RATE_ESTIMATOR_SAMPLE_RATE_ESTIMATOR_H_
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <sys/types.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// Designates the value used to indicate an invalid sample rate estimate.
+#define SAMPLE_RATE_ESTIMATOR_INVALID_SAMPLE_RATE_HZ (-1.0f)
+
+// Sample rate estimator data structure.
+struct SampleRateEstimator {
+ // Used to compute sample intervals to estimate the sampling rate.
+ uint64_t last_timestamp_nanos;
+
+ // Accumulator used for computing the mean sampling interval.
+ float interval_accumulator_nanos;
+
+ // The upper limit on the acceptance of valid time intervals (in nanoseconds).
+ // Larger time deltas result in a reset of the interval accumulator.
+ float max_interval_nanos;
+
+ // The most recent mean sampling rate estimate.
+ float mean_sampling_rate_estimate_hz;
+
+ /*
+ * The targeted number of time intervals required for a new sample rate mean
+ * calculation.
+ *
+ * NOTE: Assuming that the time interval noise is independent and identically
+ * distributed and drawn from a zero-mean normal distribution with variance
+ * 'Sigma^2'. The expected noise reduction is related to the choice of
+ * 'num_intervals_to_collect' as:
+ *
+ * Output RMS Noise = Sigma / sqrt(num_intervals_to_collect).
+ *
+ * Example, a value of 100 for a 100Hz signal would provide updates once every
+ * second and provide a 90% reduction (i.e., [1 - 1/sqrt(100)] * 100%) in time
+ * interval noise.
+ */
+ size_t num_intervals_to_collect;
+
+ // The number of time intervals currently collected.
+ size_t num_intervals_collected;
+
+ // Polling flag used to signal that a new estimate is ready. This flag is
+ // reset when 'sampleRateEstimatorGetHz' is called.
+ bool new_sampling_rate_estimate_ready;
+};
+
+// Initializes the sample rate estimator, sets the number of time intervals
+// for the mean sample rate calculation, and defines the tolerable gap in timing
+// data (in seconds).
+void sampleRateEstimatorInit(struct SampleRateEstimator* sample_rate_estimator,
+ size_t num_intervals_to_collect,
+ float max_interval_sec);
+
+// Rather than using a function to poll for new sample rate estimates, which
+// would incur an additional function call, simply check
+// 'new_sampling_rate_estimate_ready' directly.
+
+// Returns the most recent sampling rate estimate, and resets the
+// 'new_sampling_rate_estimate_ready' polling flag. Note, if polling is not
+// used, one may access the sample rate estimate directly from the struct and
+// avoid this function call.
+float sampleRateEstimatorGetHz(
+ struct SampleRateEstimator* sample_rate_estimator);
+
+// Takes in a new timestamp and updates the sampling rate estimator.
+void sampleRateEstimatorUpdate(
+ struct SampleRateEstimator* sample_rate_estimator,
+ uint64_t timestamp_nanos);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_SAMPLE_RATE_ESTIMATOR_SAMPLE_RATE_ESTIMATOR_H_
diff --git a/firmware/os/algos/calibration/sphere_fit/calibration_data.c b/firmware/os/algos/calibration/sphere_fit/calibration_data.c
new file mode 100644
index 0000000..2de4a8c
--- /dev/null
+++ b/firmware/os/algos/calibration/sphere_fit/calibration_data.c
@@ -0,0 +1,52 @@
+/*
+ * Copyright (C) 2016 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "calibration/sphere_fit/calibration_data.h"
+
+#include <string.h>
+
+#include "common/math/vec.h"
+
+// FUNCTION IMPLEMENTATIONS
+//////////////////////////////////////////////////////////////////////////////
+
+// Set calibration data to identity scale factors, zero skew and
+// zero bias.
+void calDataReset(struct ThreeAxisCalData* calstruct) {
+ memset(calstruct, 0, sizeof(struct ThreeAxisCalData));
+ calstruct->scale_factor_x = 1.0f;
+ calstruct->scale_factor_y = 1.0f;
+ calstruct->scale_factor_z = 1.0f;
+}
+
+void calDataCorrectData(const struct ThreeAxisCalData* calstruct,
+ const float x_impaired[THREE_AXIS_DIM],
+ float* x_corrected) {
+ // x_temp = (x_impaired - bias).
+ float x_temp[THREE_AXIS_DIM];
+ vecSub(x_temp, x_impaired, calstruct->bias, THREE_AXIS_DIM);
+
+ // x_corrected = scale_skew_mat * x_temp, where:
+ // scale_skew_mat = [scale_factor_x 0 0
+ // skew_yx scale_factor_y 0
+ // skew_zx skew_zy scale_factor_z].
+ x_corrected[0] = calstruct->scale_factor_x * x_temp[0];
+ x_corrected[1] =
+ calstruct->skew_yx * x_temp[0] + calstruct->scale_factor_y * x_temp[1];
+ x_corrected[2] = calstruct->skew_zx * x_temp[0] +
+ calstruct->skew_zy * x_temp[1] +
+ calstruct->scale_factor_z * x_temp[2];
+}
diff --git a/firmware/os/algos/calibration/common/calibration_data.h b/firmware/os/algos/calibration/sphere_fit/calibration_data.h
similarity index 84%
rename from firmware/os/algos/calibration/common/calibration_data.h
rename to firmware/os/algos/calibration/sphere_fit/calibration_data.h
index b0e6809..405d0ac 100644
--- a/firmware/os/algos/calibration/common/calibration_data.h
+++ b/firmware/os/algos/calibration/sphere_fit/calibration_data.h
@@ -13,7 +13,7 @@
* See the License for the specific language governing permissions and
* limitations under the License.
*/
-/////////////////////////////////////////////////////////////////////////////
+
/*
* This module contains a data structure and corresponding helper functions for
* a three-axis sensor calibration. The calibration consists of a bias vector,
@@ -23,8 +23,9 @@
*
* corrected_data = scale_skew_mat * (impaired_data - bias).
*/
-#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_COMMON_CALIBRATION_DATA_H_
-#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_COMMON_CALIBRATION_DATA_H_
+
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_SPHERE_FIT_CALIBRATION_DATA_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_SPHERE_FIT_CALIBRATION_DATA_H_
#include <stdint.h>
@@ -57,7 +58,7 @@
// Set calibration data to identity scale factors, zero skew and
// zero bias.
-void calDataReset(struct ThreeAxisCalData *calstruct);
+void calDataReset(struct ThreeAxisCalData* calstruct);
// Apply a stored calibration to correct a single sample of impaired sensor
// data.
@@ -69,4 +70,4 @@
}
#endif
-#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_COMMON_CALIBRATION_DATA_H_
+#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_SPHERE_FIT_CALIBRATION_DATA_H_
diff --git a/firmware/os/algos/calibration/common/sphere_fit_calibration.c b/firmware/os/algos/calibration/sphere_fit/sphere_fit_calibration.c
similarity index 78%
rename from firmware/os/algos/calibration/common/sphere_fit_calibration.c
rename to firmware/os/algos/calibration/sphere_fit/sphere_fit_calibration.c
index 2c26af5..853a73d 100644
--- a/firmware/os/algos/calibration/common/sphere_fit_calibration.c
+++ b/firmware/os/algos/calibration/sphere_fit/sphere_fit_calibration.c
@@ -1,4 +1,20 @@
-#include "calibration/common/sphere_fit_calibration.h"
+/*
+ * Copyright (C) 2016 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "calibration/sphere_fit/sphere_fit_calibration.h"
#include <errno.h>
#include <stdarg.h>
@@ -19,7 +35,7 @@
const struct SphereFitData *data,
uint64_t timestamp_nanos);
-#define MIN_VALID_DATA_NORM (1e-4)
+#define MIN_VALID_DATA_NORM (1e-4f)
// FUNCTION IMPLEMENTATIONS
//////////////////////////////////////////////////////////////////////////////
@@ -103,7 +119,7 @@
ASSERT_NOT_NULL(f_data);
ASSERT_NOT_NULL(residual);
- const struct SphereFitData *data = (const struct SphereFitData*)f_data;
+ const struct SphereFitData *data = (const struct SphereFitData *)f_data;
// Verify that expected norm is non-zero, else use default of 1.0.
float expected_norm = 1.0;
@@ -204,10 +220,9 @@
float x_sol[SF_STATE_DIM];
// Run calibration
- const enum LmStatus status = lmSolverSolve(&sphere_cal->lm_solver,
- sphere_cal->x0, (void *)data,
- SF_STATE_DIM, data->num_fit_points,
- x_sol);
+ const enum LmStatus status =
+ lmSolverSolve(&sphere_cal->lm_solver, sphere_cal->x0, (void *)data,
+ SF_STATE_DIM, data->num_fit_points, x_sol);
// Check if solver was successful
if (status == RELATIVE_STEP_SUFFICIENTLY_SMALL ||
@@ -215,32 +230,34 @@
// TODO(dvitus): Check quality of new fit before using.
// Store new fit.
#ifdef SPHERE_FIT_DBG_ENABLED
- CAL_DEBUG_LOG(
- "[SPHERE CAL]",
- "Solution found in %d iterations with status %d.\n",
- sphere_cal->lm_solver.num_iter, status);
- CAL_DEBUG_LOG(
- "[SPHERE CAL]",
- "Solution:\n {%s%d.%06d [M(1,1)], %s%d.%06d [M(2,1)], "
- "%s%d.%06d [M(2,2)], \n"
- "%s%d.%06d [M(3,1)], %s%d.%06d [M(3,2)], %s%d.%06d [M(3,3)], \n"
- "%s%d.%06d [b(1)], %s%d.%06d [b(2)], %s%d.%06d [b(3)]}.",
- CAL_ENCODE_FLOAT(x_sol[0], 6), CAL_ENCODE_FLOAT(x_sol[1], 6),
- CAL_ENCODE_FLOAT(x_sol[2], 6), CAL_ENCODE_FLOAT(x_sol[3], 6),
- CAL_ENCODE_FLOAT(x_sol[4], 6), CAL_ENCODE_FLOAT(x_sol[5], 6),
- CAL_ENCODE_FLOAT(x_sol[6], 6), CAL_ENCODE_FLOAT(x_sol[7], 6),
- CAL_ENCODE_FLOAT(x_sol[8], 6));
-#endif
+ CAL_DEBUG_LOG("[SPHERE CAL]",
+ "Solution found in %d iterations with status %d.\n",
+ sphere_cal->lm_solver.num_iter, status);
+ CAL_DEBUG_LOG("[SPHERE CAL]", "Solution:\n {"
+ CAL_FORMAT_6DIGITS " [M(1,1)], "
+ CAL_FORMAT_6DIGITS " [M(2,1)], "
+ CAL_FORMAT_6DIGITS " [M(2,2)], \n"
+ CAL_FORMAT_6DIGITS " [M(3,1)], "
+ CAL_FORMAT_6DIGITS " [M(3,2)], "
+ CAL_FORMAT_6DIGITS " [M(3,3)], \n"
+ CAL_FORMAT_6DIGITS " [b(1)], "
+ CAL_FORMAT_6DIGITS " [b(2)], "
+ CAL_FORMAT_6DIGITS " [b(3)]}.",
+ CAL_ENCODE_FLOAT(x_sol[0], 6), CAL_ENCODE_FLOAT(x_sol[1], 6),
+ CAL_ENCODE_FLOAT(x_sol[2], 6), CAL_ENCODE_FLOAT(x_sol[3], 6),
+ CAL_ENCODE_FLOAT(x_sol[4], 6), CAL_ENCODE_FLOAT(x_sol[5], 6),
+ CAL_ENCODE_FLOAT(x_sol[6], 6), CAL_ENCODE_FLOAT(x_sol[7], 6),
+ CAL_ENCODE_FLOAT(x_sol[8], 6));
+#endif // SPHERE_FIT_DBG_ENABLED
memcpy(sphere_cal->x, x_sol, sizeof(x_sol));
sphere_cal->estimate_time_nanos = timestamp_nanos;
return true;
} else {
#ifdef SPHERE_FIT_DBG_ENABLED
- CAL_DEBUG_LOG(
- "[SPHERE CAL]",
- "Solution failed in %d iterations with status %d.\n",
- sphere_cal->lm_solver.num_iter, status);
-#endif
+ CAL_DEBUG_LOG("[SPHERE CAL]",
+ "Solution failed in %d iterations with status %d.\n",
+ sphere_cal->lm_solver.num_iter, status);
+#endif // SPHERE_FIT_DBG_ENABLED
}
return false;
diff --git a/firmware/os/algos/calibration/common/sphere_fit_calibration.h b/firmware/os/algos/calibration/sphere_fit/sphere_fit_calibration.h
similarity index 92%
rename from firmware/os/algos/calibration/common/sphere_fit_calibration.h
rename to firmware/os/algos/calibration/sphere_fit/sphere_fit_calibration.h
index e49f225..d3bbf7f 100644
--- a/firmware/os/algos/calibration/common/sphere_fit_calibration.h
+++ b/firmware/os/algos/calibration/sphere_fit/sphere_fit_calibration.h
@@ -13,7 +13,7 @@
* See the License for the specific language governing permissions and
* limitations under the License.
*/
-/////////////////////////////////////////////////////////////////////////////
+
/*
* This module contains an algorithm for performing a sphere fit calibration.
* A sphere fit calibration solves the following non-linear least squares
@@ -34,13 +34,14 @@
* M and b. M is assumed to be a lower diagonal, consisting of 6 parameters.
*
*/
-#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_COMMON_SPHERE_FIT_CALIBRATION_H_
-#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_COMMON_SPHERE_FIT_CALIBRATION_H_
+
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_SPHERE_FIT_SPHERE_FIT_CALIBRATION_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_SPHERE_FIT_SPHERE_FIT_CALIBRATION_H_
#include <stdbool.h>
#include <stdint.h>
-#include "calibration/common/calibration_data.h"
+#include "calibration/sphere_fit/calibration_data.h"
#include "common/math/levenberg_marquardt.h"
#ifdef __cplusplus
@@ -140,4 +141,4 @@
}
#endif
-#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_COMMON_SPHERE_FIT_CALIBRATION_H_
+#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_SPHERE_FIT_SPHERE_FIT_CALIBRATION_H_
diff --git a/firmware/os/algos/calibration/util/cal_log.h b/firmware/os/algos/calibration/util/cal_log.h
index 1bd80fd..46297db 100644
--- a/firmware/os/algos/calibration/util/cal_log.h
+++ b/firmware/os/algos/calibration/util/cal_log.h
@@ -16,12 +16,13 @@
///////////////////////////////////////////////////////////////
/*
- * Logging macros for printing formatted strings to Nanohub.
+ * Logging macros for printing user-debug messages.
*/
///////////////////////////////////////////////////////////////
#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_UTIL_CAL_LOG_H_
#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_UTIL_CAL_LOG_H_
+// clang-format off
#ifdef GCC_DEBUG_LOG
# include <stdio.h>
# define CAL_DEBUG_LOG(tag, fmt, ...) \
@@ -35,31 +36,61 @@
LOG_FUNC(LOG_DEBUG, "%s " fmt "\n", tag, ##__VA_ARGS__)
# define CAL_DEBUG_LOG(tag, fmt, ...) \
osLog(LOG_DEBUG, "%s " fmt, tag, ##__VA_ARGS__);
-#else // _OS_BUILD_
+#elif NANOHUB_DEBUG_LOG
# include <chre.h>
# define CAL_DEBUG_LOG(tag, fmt, ...) \
chreLog(CHRE_LOG_INFO, "%s " fmt, tag, ##__VA_ARGS__)
+#else
+// CHRE/SLPI Nanoapp Logging.
+# include "chre/util/nanoapp/log.h"
+# ifndef LOG_TAG
+# define LOG_TAG ""
+# endif // LOG_TAG
+# define CAL_DEBUG_LOG(tag, format, ...) \
+ LOGI("%s " format, tag, ##__VA_ARGS__)
#endif // GCC_DEBUG_LOG
+// clang-format on
#ifdef __cplusplus
extern "C" {
#endif
-// Using this macro because floorf() is not currently implemented by the Nanohub
-// firmware.
+// Floor macro implementation for platforms that do not supply the standard
+// floorf() math function.
#define CAL_FLOOR(x) ((int)(x) - ((x) < (int)(x))) // NOLINT
+/*
+ * On some embedded software platforms numerical string formatting is not fully
+ * supported. Defining CAL_NO_FLOAT_FORMAT_STRINGS will enable a workaround that
+ * prints floating-point values having a specified number of digits using the
+ * CAL_ENCODE_FLOAT macro.
+ * Examples:
+ * - Nanohub does not support floating-point format strings.
+ * - CHRE/SLPI allows %.Xf for printing float values.
+ */
+#ifdef CAL_NO_FLOAT_FORMAT_STRINGS
// Macro used to print floating point numbers with a specified number of digits.
-#define CAL_ENCODE_FLOAT(x, num_digits) \
- ((x < 0) ? "-" : ""), \
- (int)CAL_FLOOR(fabsf(x)), (int)((fabsf(x) - CAL_FLOOR(fabsf(x))) * powf(10, num_digits)) // NOLINT
+# define CAL_ENCODE_FLOAT(x, num_digits) \
+ ((x < 0) ? "-" : ""), (int)CAL_FLOOR(fabsf(x)), \
+ (int)((fabsf(x) - CAL_FLOOR(fabsf(x))) * \
+ powf(10, num_digits)) // NOLINT
// Helper definitions for CAL_ENCODE_FLOAT to specify the print format with
// desired significant digits.
-#define CAL_FORMAT_3DIGITS "%s%d.%03d"
-#define CAL_FORMAT_6DIGITS "%s%d.%06d"
-#define CAL_FORMAT_3DIGITS_TRIPLET "%s%d.%03d, %s%d.%03d, %s%d.%03d"
-#define CAL_FORMAT_6DIGITS_TRIPLET "%s%d.%06d, %s%d.%06d, %s%d.%06d"
+# define CAL_FORMAT_3DIGITS "%s%d.%03d"
+# define CAL_FORMAT_6DIGITS "%s%d.%06d"
+# define CAL_FORMAT_3DIGITS_TRIPLET "%s%d.%03d, %s%d.%03d, %s%d.%03d"
+# define CAL_FORMAT_6DIGITS_TRIPLET "%s%d.%06d, %s%d.%06d, %s%d.%06d"
+#else
+// Pass-through when float string formatting (e.g., %.6f) is available.
+# define CAL_ENCODE_FLOAT(x, num_digits) (x)
+
+// Float string formatting helpers.
+# define CAL_FORMAT_3DIGITS "%.3f"
+# define CAL_FORMAT_6DIGITS "%.6f"
+# define CAL_FORMAT_3DIGITS_TRIPLET "%.3f, %.3f, %.3f"
+# define CAL_FORMAT_6DIGITS_TRIPLET "%.6f, %.6f, %.6f"
+#endif // CAL_NO_FLOAT_FORMAT_STRINGS
#ifdef __cplusplus
}
diff --git a/firmware/os/algos/common/math/kasa.c b/firmware/os/algos/common/math/kasa.c
new file mode 100644
index 0000000..a24a31b
--- /dev/null
+++ b/firmware/os/algos/common/math/kasa.c
@@ -0,0 +1,120 @@
+#include "common/math/kasa.h"
+
+#include <stdint.h>
+#include <sys/types.h>
+
+#include "common/math/mat.h"
+
+void kasaReset(struct KasaFit *kasa) {
+ kasa->acc_x = kasa->acc_y = kasa->acc_z = kasa->acc_w = 0.0f;
+ kasa->acc_xx = kasa->acc_xy = kasa->acc_xz = kasa->acc_xw = 0.0f;
+ kasa->acc_yy = kasa->acc_yz = kasa->acc_yw = 0.0f;
+ kasa->acc_zz = kasa->acc_zw = 0.0f;
+ kasa->nsamples = 0;
+}
+
+void kasaInit(struct KasaFit *kasa) { kasaReset(kasa); }
+
+void kasaAccumulate(struct KasaFit *kasa, float x, float y, float z) {
+ float w = x * x + y * y + z * z;
+
+ kasa->acc_x += x;
+ kasa->acc_y += y;
+ kasa->acc_z += z;
+ kasa->acc_w += w;
+
+ kasa->acc_xx += x * x;
+ kasa->acc_xy += x * y;
+ kasa->acc_xz += x * z;
+ kasa->acc_xw += x * w;
+
+ kasa->acc_yy += y * y;
+ kasa->acc_yz += y * z;
+ kasa->acc_yw += y * w;
+
+ kasa->acc_zz += z * z;
+ kasa->acc_zw += z * w;
+
+ kasa->nsamples += 1;
+}
+
+bool kasaNormalize(struct KasaFit *kasa) {
+ if (kasa->nsamples == 0) {
+ return false;
+ }
+
+ float inv = 1.0f / kasa->nsamples;
+
+ kasa->acc_x *= inv;
+ kasa->acc_y *= inv;
+ kasa->acc_z *= inv;
+ kasa->acc_w *= inv;
+
+ kasa->acc_xx *= inv;
+ kasa->acc_xy *= inv;
+ kasa->acc_xz *= inv;
+ kasa->acc_xw *= inv;
+
+ kasa->acc_yy *= inv;
+ kasa->acc_yz *= inv;
+ kasa->acc_yw *= inv;
+
+ kasa->acc_zz *= inv;
+ kasa->acc_zw *= inv;
+
+ return true;
+}
+
+int kasaFit(struct KasaFit *kasa, struct Vec3 *bias, float *radius,
+ float max_fit, float min_fit) {
+ // A * out = b
+ // (4 x 4) (4 x 1) (4 x 1)
+ struct Mat44 A;
+ A.elem[0][0] = kasa->acc_xx;
+ A.elem[0][1] = kasa->acc_xy;
+ A.elem[0][2] = kasa->acc_xz;
+ A.elem[0][3] = kasa->acc_x;
+ A.elem[1][0] = kasa->acc_xy;
+ A.elem[1][1] = kasa->acc_yy;
+ A.elem[1][2] = kasa->acc_yz;
+ A.elem[1][3] = kasa->acc_y;
+ A.elem[2][0] = kasa->acc_xz;
+ A.elem[2][1] = kasa->acc_yz;
+ A.elem[2][2] = kasa->acc_zz;
+ A.elem[2][3] = kasa->acc_z;
+ A.elem[3][0] = kasa->acc_x;
+ A.elem[3][1] = kasa->acc_y;
+ A.elem[3][2] = kasa->acc_z;
+ A.elem[3][3] = 1.0f;
+
+ struct Vec4 b;
+ initVec4(&b, -kasa->acc_xw, -kasa->acc_yw, -kasa->acc_zw, -kasa->acc_w);
+
+ struct Size4 pivot;
+ mat44DecomposeLup(&A, &pivot);
+
+ struct Vec4 out;
+ mat44Solve(&A, &out, &b, &pivot);
+
+ // sphere: (x - xc)^2 + (y - yc)^2 + (z - zc)^2 = r^2
+ //
+ // xc = -out[0] / 2, yc = -out[1] / 2, zc = -out[2] / 2
+ // r = sqrt(xc^2 + yc^2 + zc^2 - out[3])
+
+ struct Vec3 v;
+ initVec3(&v, out.x, out.y, out.z);
+ vec3ScalarMul(&v, -0.5f);
+
+ float r_square = vec3Dot(&v, &v) - out.w;
+ float r = (r_square > 0) ? sqrtf(r_square) : 0;
+
+ initVec3(bias, v.x, v.y, v.z);
+ *radius = r;
+
+ int success = 0;
+ if (r > min_fit && r < max_fit) {
+ success = 1;
+ }
+
+ return success;
+}
diff --git a/firmware/os/algos/common/math/kasa.h b/firmware/os/algos/common/math/kasa.h
new file mode 100644
index 0000000..e9652d6
--- /dev/null
+++ b/firmware/os/algos/common/math/kasa.h
@@ -0,0 +1,54 @@
+/*
+ * This module provides a data structure, initialization, and fit
+ * routine for algorithms that use the Kasa method for determining the
+ * 3-dimensional offset vector from a set of points on a sphere.
+ *
+ * Reference: I. Kåsa, "A circle fitting procedure and its error analysis," in
+ * IEEE Transactions on Instrumentation and Measurement, vol. IM-25, no. 1, pp.
+ * 8-14, March 1976.
+ */
+
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_KASA_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_KASA_H_
+
+#include <stdbool.h>
+
+#include "common/math/vec.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct KasaFit {
+ float acc_x, acc_y, acc_z, acc_w;
+ float acc_xx, acc_xy, acc_xz, acc_xw;
+ float acc_yy, acc_yz, acc_yw, acc_zz, acc_zw;
+ size_t nsamples;
+};
+
+// Resets the KasaFit data structure (sets all variables to zero).
+void kasaReset(struct KasaFit *kasa);
+
+// Initializes the KasaFit data structure.
+void kasaInit(struct KasaFit *kasa);
+
+// Accumulates the Kasa acc_** variables with the input vector [x, y, z], and
+// updates the number of samples.
+void kasaAccumulate(struct KasaFit *kasa, float x, float y, float z);
+
+// Normalizes the Kasa acc_** variables. Returns 'false' if the number of
+// samples is zero, otherwise 'true'.
+bool kasaNormalize(struct KasaFit *kasa);
+
+// Uses the Kasa sphere-fit method to extract a 'bias' estimate (centroid) for
+// the best-fit sphere using the normal equations, and the sphere's 'radius'.
+// Returns '1' if the radius of the fit sphere is within the bounds
+// (min_fit, max_fit), otherwise '0'.
+int kasaFit(struct KasaFit *kasa, struct Vec3 *bias, float *radius,
+ float max_fit, float min_fit);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_KASA_H_
diff --git a/firmware/os/algos/common/math/levenberg_marquardt.h b/firmware/os/algos/common/math/levenberg_marquardt.h
index c01d2eb..d62308d 100644
--- a/firmware/os/algos/common/math/levenberg_marquardt.h
+++ b/firmware/os/algos/common/math/levenberg_marquardt.h
@@ -109,7 +109,7 @@
// Initializes LM solver with provided parameters and error function.
void lmSolverInit(struct LmSolver *solver, const struct LmParams *params,
- ResidualAndJacobianFunction error_func);
+ ResidualAndJacobianFunction func);
void lmSolverDestroy(struct LmSolver *solver);
@@ -133,7 +133,7 @@
*/
enum LmStatus lmSolverSolve(struct LmSolver *solver, const float *initial_state,
void *f_data, size_t state_dim, size_t meas_dim,
- float *est_state);
+ float *state);
////////////////////////// TEST UTILITIES ////////////////////////////////////
// This function is exposed here for testing purposes only.
diff --git a/firmware/os/algos/common/math/macros.h b/firmware/os/algos/common/math/macros.h
index 5c06e24..cb75595 100644
--- a/firmware/os/algos/common/math/macros.h
+++ b/firmware/os/algos/common/math/macros.h
@@ -14,30 +14,43 @@
* limitations under the License.
*/
-// This file contains helper macros and definitions.
+// This file contains frequently used constants and helper macros.
+
+#include <stdint.h>
#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_MACROS_H_
#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_MACROS_H_
-// Mathematical constants.
-#define NANO_PI (3.14159265359f)
+// Constants.
+#define NANO_PI (3.14159265359f)
+#define INVALID_TEMPERATURE_CELSIUS (-274.0f)
// Common math operations.
#define NANO_ABS(x) ((x) > 0 ? (x) : -(x))
#define NANO_MAX(a, b) ((a) > (b)) ? (a) : (b)
#define NANO_MIN(a, b) ((a) < (b)) ? (a) : (b)
+#define SIGMOID(x) (1 / (1 + expf(-x)))
// Timestamp conversion macros.
#ifdef __cplusplus
-#define MSEC_TO_NANOS(x) (static_cast<uint64_t>(x) * 1000000)
+#define MSEC_TO_NANOS(x) (static_cast<uint64_t>(x * UINT64_C(1000000)))
#else
-#define MSEC_TO_NANOS(x) ((uint64_t)(x) * 1000000) // NOLINT
+#define MSEC_TO_NANOS(x) ((uint64_t)(x * UINT64_C(1000000))) // NOLINT
#endif
-#define SEC_TO_NANOS(x) MSEC_TO_NANOS(x * 1000)
-#define MIN_TO_NANOS(x) SEC_TO_NANOS(x * 60)
-#define HRS_TO_NANOS(x) MIN_TO_NANOS(x * 60)
-#define DAYS_TO_NANOS(x) HRS_TO_NANOS(x * 24)
+#define SEC_TO_NANOS(x) MSEC_TO_NANOS(x * UINT64_C(1000))
+#define MIN_TO_NANOS(x) SEC_TO_NANOS (x * UINT64_C(60))
+#define HRS_TO_NANOS(x) MIN_TO_NANOS (x * UINT64_C(60))
+#define DAYS_TO_NANOS(x) HRS_TO_NANOS (x * UINT64_C(24))
+
+// Sample rate to period conversion.
+#ifdef __cplusplus
+#define HZ_TO_PERIOD_NANOS(hz) \
+ (SEC_TO_NANOS(1024) / (static_cast<uint64_t>(hz * 1024)))
+#else
+#define HZ_TO_PERIOD_NANOS(hz) \
+ (SEC_TO_NANOS(1024) / ((uint64_t)(hz * 1024))) // NOLINT
+#endif
// Unit conversion: nanoseconds to seconds.
#define NANOS_TO_SEC (1.0e-9f)
@@ -57,4 +70,26 @@
#define NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA(t1, t2, t_delta) \
(((t1) >= (t2) + (t_delta)) || ((t1) < (t2)))
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// This conversion function may be necessary for embedded hardware that can't
+// cast a uint64_t to a float directly. This conversion function was taken from:
+// [android]//device/google/contexthub/firmware/os/core/floatRt.c
+static inline float floatFromUint64(uint64_t v) {
+ uint32_t hi = v >> 32;
+ uint32_t lo = (uint32_t) v;
+
+ if (!hi) { // This is very fast for cases where 'v' fits into a uint32_t.
+ return (float)lo;
+ } else {
+ return ((float)hi) * 4294967296.0f + (float)lo;
+ }
+}
+
+#ifdef __cplusplus
+}
+#endif
+
#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_MACROS_H_
diff --git a/firmware/os/algos/common/math/mat.c b/firmware/os/algos/common/math/mat.c
index 34aaa51..8b62cce 100644
--- a/firmware/os/algos/common/math/mat.c
+++ b/firmware/os/algos/common/math/mat.c
@@ -32,8 +32,8 @@
#include <stddef.h>
#include <string.h>
-#define EPSILON 1E-5
-#define CHOLESKY_TOLERANCE 1E-6
+#define EPSILON 1E-5f
+#define CHOLESKY_TOLERANCE 1E-6f
// Forward declarations.
static void mat33SwapRows(struct Mat33 *A, uint32_t i, uint32_t j);
@@ -248,8 +248,8 @@
}
}
// divide by zero guard.
- ASSERT(fabs(tmp.elem[i][i]) > 0);
- if(!(fabs(tmp.elem[i][i]) > 0)) {
+ ASSERT(fabsf(tmp.elem[i][i]) > 0);
+ if(!(fabsf(tmp.elem[i][i]) > 0)) {
return;
}
t = 1.0f / tmp.elem[i][i];
@@ -407,6 +407,16 @@
initVec3(eigenvals, _eigenvals[0], _eigenvals[1], _eigenvals[2]);
}
+float mat33Determinant(const struct Mat33 *A) {
+ ASSERT_NOT_NULL(A);
+ return A->elem[0][0] *
+ (A->elem[1][1] * A->elem[2][2] - A->elem[1][2] * A->elem[2][1])
+ - A->elem[0][1] *
+ (A->elem[1][0] * A->elem[2][2] - A->elem[1][2] * A->elem[2][0])
+ + A->elem[0][2] *
+ (A->elem[1][0] * A->elem[2][1] - A->elem[1][1] * A->elem[2][0]);
+}
+
// index of largest off-diagonal element in row k
UNROLLED
uint32_t mat33Maxind(const struct Mat33 *A, uint32_t k) {
diff --git a/firmware/os/algos/common/math/mat.h b/firmware/os/algos/common/math/mat.h
index 13494f5..9d69405 100644
--- a/firmware/os/algos/common/math/mat.h
+++ b/firmware/os/algos/common/math/mat.h
@@ -125,6 +125,8 @@
void mat33GetEigenbasis(struct Mat33 *S, struct Vec3 *eigenvals,
struct Mat33 *eigenvecs);
+// Computes the determinant of a 3 by 3 matrix.
+float mat33Determinant(const struct Mat33 *A);
// 4x4 MATRIX MATH /////////////////////////////////////////////////////////////
// Updates out with the multiplication of A and v, i.e.:
diff --git a/firmware/os/algos/common/math/vec.h b/firmware/os/algos/common/math/vec.h
index 0a4c8b3..e839ad5 100644
--- a/firmware/os/algos/common/math/vec.h
+++ b/firmware/os/algos/common/math/vec.h
@@ -70,6 +70,17 @@
v->z += w->z;
}
+// Sets u as the sum of v and w.
+static inline void vec3AddVecs(struct Vec3 *u, const struct Vec3 *v,
+ const struct Vec3 *w) {
+ ASSERT_NOT_NULL(u);
+ ASSERT_NOT_NULL(v);
+ ASSERT_NOT_NULL(w);
+ u->x = v->x + w->x;
+ u->y = v->y + w->y;
+ u->z = v->z + w->z;
+}
+
// Updates v as the subtraction of w from v.
static inline void vec3Sub(struct Vec3 *v, const struct Vec3 *w) {
ASSERT_NOT_NULL(v);
@@ -79,6 +90,17 @@
v->z -= w->z;
}
+// Sets u as the difference of v and w.
+static inline void vec3SubVecs(struct Vec3 *u, const struct Vec3 *v,
+ const struct Vec3 *w) {
+ ASSERT_NOT_NULL(u);
+ ASSERT_NOT_NULL(v);
+ ASSERT_NOT_NULL(w);
+ u->x = v->x - w->x;
+ u->y = v->y - w->y;
+ u->z = v->z - w->z;
+}
+
// Scales v by the scalar c, i.e. v = c * v.
static inline void vec3ScalarMul(struct Vec3 *v, float c) {
ASSERT_NOT_NULL(v);
diff --git a/firmware/os/algos/util/array.h b/firmware/os/algos/util/array.h
new file mode 100644
index 0000000..9658be4
--- /dev/null
+++ b/firmware/os/algos/util/array.h
@@ -0,0 +1,6 @@
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_UTIL_ARRAY_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_UTIL_ARRAY_H_
+
+#define ARRAY_SIZE(a) (sizeof((a)) / sizeof((a)[0]))
+
+#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_UTIL_ARRAY_H_
diff --git a/firmware/os/algos/util/nano_assert.h b/firmware/os/algos/util/nano_assert.h
index da77749..cb3286e 100644
--- a/firmware/os/algos/util/nano_assert.h
+++ b/firmware/os/algos/util/nano_assert.h
@@ -1,7 +1,7 @@
#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_UTIL_NANO_ASSERT_H_
#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_UTIL_NANO_ASSERT_H_
-#if defined(__NANOHUB__) || defined(_OS_BUILD_)
+#ifndef GOOGLE3
// For external nanoapps (__NANOHUB__ defined), use SRC_FILENAME provided
// by the build system, which has the directory stripped. But allow the
@@ -32,7 +32,7 @@
#include <assert.h>
#define ASSERT_IMPL(x) assert(x)
-#endif
+#endif // GOOGLE3
#ifndef ASSERT
#ifdef NANO_ASSERT_ENABLED
diff --git a/firmware/os/algos/util/nano_log.h b/firmware/os/algos/util/nano_log.h
new file mode 100644
index 0000000..8ec8043
--- /dev/null
+++ b/firmware/os/algos/util/nano_log.h
@@ -0,0 +1,13 @@
+#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_UTIL_NANO_LOG_H_
+#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_UTIL_NANO_LOG_H_
+
+#ifdef GOOGLE3
+#include <stdio.h>
+
+// ignore log level argument
+#define LOG_FUNC(level, ...) printf(__VA_ARGS__)
+#endif
+
+#include "third_party/contexthub/nanoapps/util/log/log.h"
+
+#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_UTIL_NANO_LOG_H_
diff --git a/firmware/os/core/heap.c b/firmware/os/core/heap.c
index 2f38a0d..f6e5067 100644
--- a/firmware/os/core/heap.c
+++ b/firmware/os/core/heap.c
@@ -229,3 +229,50 @@
return count;
}
+
+int heapGetFreeSize(int *numChunks, int *largestChunk)
+{
+ struct HeapNode *node;
+ bool haveLock;
+ int bytes = 0;
+ *numChunks = *largestChunk = 0;
+
+ // this can only fail if called from interrupt
+ haveLock = trylockTryTake(&gHeapLock);
+ if (!haveLock)
+ return -1;
+
+ for (node = gHeapHead; node; node = heapPrvGetNext(node)) {
+ if (!node->used) {
+ if (node->size > *largestChunk)
+ *largestChunk = node->size;
+ bytes += node->size + sizeof(struct HeapNode);
+ (*numChunks)++;
+ }
+ }
+ trylockRelease(&gHeapLock);
+
+ return bytes;
+}
+
+int heapGetTaskSize(uint32_t tid)
+{
+ struct HeapNode *node;
+ bool haveLock;
+ int bytes = 0;
+
+ // this can only fail if called from interrupt
+ haveLock = trylockTryTake(&gHeapLock);
+ if (!haveLock)
+ return -1;
+
+ tid &= TIDX_MASK;
+ for (node = gHeapHead; node; node = heapPrvGetNext(node)) {
+ if (node->used && node->tidx == tid) {
+ bytes += node->size + sizeof(struct HeapNode);
+ }
+ }
+ trylockRelease(&gHeapLock);
+
+ return bytes;
+}
diff --git a/firmware/os/core/hostIntf.c b/firmware/os/core/hostIntf.c
index 084e508..ccd571d 100644
--- a/firmware/os/core/hostIntf.c
+++ b/firmware/os/core/hostIntf.c
@@ -35,6 +35,7 @@
#include <nanohubCommand.h>
#include <nanohubPacket.h>
#include <seos.h>
+#include <seos_priv.h>
#include <util.h>
#include <atomicBitset.h>
#include <atomic.h>
@@ -1097,18 +1098,36 @@
static void hostIntfNotifyReboot(uint32_t reason)
{
- struct NanohubHalRebootTx *resp = heapAlloc(sizeof(*resp));
__le32 raw_reason = htole32(reason);
+ struct NanohubHalSysMgmtTx *resp;
+ resp = heapAlloc(sizeof(*resp));
if (resp) {
- resp->hdr = (struct NanohubHalHdr){
+ resp->hdr = (struct NanohubHalHdr) {
.appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0),
- .len = sizeof(*resp) - sizeof(resp->hdr) + sizeof(resp->hdr.msg),
- .msg = NANOHUB_HAL_REBOOT,
+ .len = sizeof(*resp) - sizeof(resp->hdr),
};
- memcpy(&resp->reason, &raw_reason, sizeof(resp->reason));
- osEnqueueEvtOrFree(EVT_APP_TO_HOST, resp, heapFree);
+ resp->ret = (struct NanohubHalRet) {
+ .msg = NANOHUB_HAL_SYS_MGMT,
+ .status = raw_reason,
+ };
+ resp->cmd = NANOHUB_HAL_SYS_MGMT_REBOOT;
+ osEnqueueEvtOrFree(EVT_APP_TO_HOST_CHRE, resp, heapFree);
}
+
+#ifdef LEGACY_HAL_ENABLED
+ struct NanohubHalLegacyRebootTx *respLegacy;
+ respLegacy = heapAlloc(sizeof(*respLegacy));
+ if (respLegacy) {
+ respLegacy->hdr = (struct NanohubHalLegacyHdr) {
+ .appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0),
+ .len = sizeof(*respLegacy) - sizeof(respLegacy->hdr) + sizeof(respLegacy->hdr.msg),
+ .msg = NANOHUB_HAL_LEGACY_REBOOT,
+ };
+ memcpy(&respLegacy->reason, &raw_reason, sizeof(respLegacy->reason));
+ osEnqueueEvtOrFree(EVT_APP_TO_HOST, respLegacy, heapFree);
+ }
+#endif
}
static void queueFlush(struct ActiveSensor *sensor)
@@ -1151,9 +1170,10 @@
struct HostIntfDataBuffer *data;
osEventUnsubscribe(mHostIntfTid, EVT_APP_START);
- osEventSubscribe(mHostIntfTid, EVT_NO_SENSOR_CONFIG_EVENT);
- osEventSubscribe(mHostIntfTid, EVT_APP_TO_SENSOR_HAL_DATA);
- osEventSubscribe(mHostIntfTid, EVT_APP_TO_HOST);
+ osEventsSubscribe(4, EVT_NO_SENSOR_CONFIG_EVENT,
+ EVT_APP_TO_SENSOR_HAL_DATA,
+ EVT_APP_TO_HOST,
+ EVT_APP_TO_HOST_CHRE);
#ifdef DEBUG_LOG_EVT
osEventSubscribe(mHostIntfTid, EVT_DEBUG_LOG);
platEarlyLogFlush();
@@ -1186,12 +1206,59 @@
}
}
+static void onEvtAppToHostChre(const void *evtData)
+{
+ const struct HostHubChrePacket *hostMsg = evtData;
+
+ if (hostMsg->messageSize <= HOST_HUB_CHRE_PACKET_MAX_LEN) {
+ struct HostIntfDataBuffer *data = alloca(sizeof(uint32_t) + sizeof(*hostMsg) + hostMsg->messageSize);
+
+ data->sensType = SENS_TYPE_INVALID;
+ data->length = sizeof(*hostMsg) + hostMsg->messageSize;
+ data->dataType = HOSTINTF_DATA_TYPE_APP_TO_HOST;
+ data->interrupt = NANOHUB_INT_WAKEUP;
+ memcpy(data->buffer, evtData, data->length);
+ hostIntfAddBlock(data, false, true);
+ }
+}
+
+#ifdef LEGACY_HAL_ENABLED
+static void handleLegacyHalCmd(const uint8_t *halData, uint8_t size)
+{
+ const struct NanohubHalLegacyCommand *halCmd = nanohubHalLegacyFindCommand(halData[0]);
+ if (halCmd)
+ halCmd->handler((void *)&halData[1], size - 1);
+}
+
static void onEvtAppFromHost(const void *evtData)
{
const uint8_t *halMsg = evtData;
- const struct NanohubHalCommand *halCmd = nanohubHalFindCommand(halMsg[1]);
- if (halCmd)
- halCmd->handler((void *)&halMsg[2], halMsg[0] - 1);
+ handleLegacyHalCmd(&halMsg[1], halMsg[0]);
+}
+#endif
+
+static void onEvtAppFromHostChre(const void *evtData)
+{
+ const struct NanohubMsgChreHdr *halMsg = (const struct NanohubMsgChreHdr *)evtData;
+ const struct NanohubHalCommand *halCmd;
+ const uint8_t *halData = (const uint8_t *)(halMsg+1);
+ uint8_t len;
+ uint32_t transactionId;
+
+ memcpy(&transactionId, &halMsg->appEvent, sizeof(halMsg->appEvent));
+
+ if (halMsg->size >= 1) {
+ len = halMsg->size - 1;
+ halCmd = nanohubHalFindCommand(halData[0]);
+ if (halCmd) {
+ if (len >= halCmd->minDataLen && len <= halCmd->maxDataLen)
+ halCmd->handler((void *)&halData[1], len, transactionId);
+ return;
+ }
+ }
+#ifdef LEGACY_HAL_ENABLED
+ handleLegacyHalCmd(halData, halMsg->size);
+#endif
}
#ifdef DEBUG_LOG_EVT
@@ -1485,16 +1552,24 @@
static void hostIntfHandleEvent(uint32_t evtType, const void* evtData)
{
- switch (evtType) {
+ switch (EVENT_GET_EVENT(evtType)) {
case EVT_APP_START:
onEvtAppStart(evtData);
break;
case EVT_APP_TO_HOST:
onEvtAppToHost(evtData);
break;
+ case EVT_APP_TO_HOST_CHRE:
+ onEvtAppToHostChre(evtData);
+ break;
+#ifdef LEGACY_HAL_ENABLED
case EVT_APP_FROM_HOST:
onEvtAppFromHost(evtData);
break;
+#endif
+ case EVT_APP_FROM_HOST_CHRE:
+ onEvtAppFromHostChre(evtData);
+ break;
#ifdef DEBUG_LOG_EVT
case EVT_DEBUG_LOG:
onEvtDebugLog(evtData);
@@ -1510,7 +1585,7 @@
onEvtAppToSensorHalData(evtData);
break;
default:
- onEvtSensorData(evtType, evtData);
+ onEvtSensorData(EVENT_GET_EVENT(evtType), evtData);
break;
}
}
@@ -1613,4 +1688,4 @@
cpuIntsRestore(state);
}
-INTERNAL_APP_INIT(APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0), 0, hostIntfRequest, hostIntfRelease, hostIntfHandleEvent);
+INTERNAL_CHRE_APP_INIT(APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0), 0, hostIntfRequest, hostIntfRelease, hostIntfHandleEvent);
diff --git a/firmware/os/core/nanohubCommand.c b/firmware/os/core/nanohubCommand.c
index 6726118..d679e73 100644
--- a/firmware/os/core/nanohubCommand.c
+++ b/firmware/os/core/nanohubCommand.c
@@ -39,6 +39,7 @@
#include <nanohubPacket.h>
#include <eeData.h>
#include <seos.h>
+#include <seos_priv.h>
#include <util.h>
#include <mpu.h>
#include <heap.h>
@@ -49,6 +50,7 @@
#include <cpu.h>
#include <cpu/cpuMath.h>
#include <algos/ap_hub_sync.h>
+#include <sensors_priv.h>
#include <chre.h>
@@ -56,9 +58,13 @@
{ .reason = _reason, .fastHandler = _fastHandler, .handler = _handler, \
.minDataLen = sizeof(_minReqType), .maxDataLen = sizeof(_maxReqType) }
-#define NANOHUB_HAL_COMMAND(_msg, _handler) \
+#define NANOHUB_HAL_LEGACY_COMMAND(_msg, _handler) \
{ .msg = _msg, .handler = _handler }
+#define NANOHUB_HAL_COMMAND(_msg, _handler, _minReqType, _maxReqType) \
+ { .msg = _msg, .handler = _handler, \
+ .minDataLen = sizeof(_minReqType), .maxDataLen = sizeof(_maxReqType) }
+
// maximum number of bytes to feed into appSecRxData at once
// The bigger the number, the more time we block other event processing
// appSecRxData only feeds 16 bytes at a time into writeCbk, so large
@@ -91,6 +97,7 @@
static struct DownloadState *mDownloadState;
static AppSecErr mAppSecStatus;
+static struct AppHdr *mApp;
static struct SlabAllocator *mEventSlab;
static struct HostIntfDataBuffer mTxCurr, mTxNext;
static uint8_t mTxCurrLength, mTxNextLength;
@@ -263,7 +270,7 @@
mDownloadState = NULL;
}
-static void resetDownloadState(bool initial)
+static bool resetDownloadState(bool initial, bool erase)
{
bool doCreate = true;
@@ -280,14 +287,19 @@
else
doCreate = false;
}
+ mDownloadState->dstOffset = 0;
if (doCreate)
mDownloadState->start = osAppSegmentCreate(mDownloadState->size);
- if (!mDownloadState->start)
- mDownloadState->erase = true;
- mDownloadState->dstOffset = 0;
+ if (!mDownloadState->start) {
+ if (erase)
+ mDownloadState->erase = true;
+ else
+ return false;
+ }
+ return true;
}
-static bool doStartFirmwareUpload(struct NanohubStartFirmwareUploadRequest *req)
+static bool doStartFirmwareUpload(struct NanohubStartFirmwareUploadRequest *req, bool erase)
{
if (!mDownloadState) {
mDownloadState = heapAlloc(sizeof(struct DownloadState));
@@ -301,9 +313,7 @@
mDownloadState->size = le32toh(req->size);
mDownloadState->crc = le32toh(req->crc);
mDownloadState->chunkReply = NANOHUB_FIRMWARE_CHUNK_REPLY_ACCEPTED;
- resetDownloadState(true);
-
- return true;
+ return resetDownloadState(true, erase);
}
static uint32_t startFirmwareUpload(void *rx, uint8_t rx_len, void *tx, uint64_t timestamp)
@@ -311,7 +321,7 @@
struct NanohubStartFirmwareUploadRequest *req = rx;
struct NanohubStartFirmwareUploadResponse *resp = tx;
- resp->accepted = doStartFirmwareUpload(req);
+ resp->accepted = doStartFirmwareUpload(req, true);
return sizeof(*resp);
}
@@ -444,15 +454,18 @@
if (!osAppSegmentClose(app, mDownloadState->dstOffset, segState)) {
osLog(LOG_INFO, "%s: Failed to close segment\n", __func__);
valid = false;
+ mApp = NULL;
} else {
segState = osAppSegmentGetState(app);
+ mApp = app;
valid = (segState == SEG_ST_VALID);
}
osLog(LOG_INFO, "Loaded %s image type %" PRIu8 ": %" PRIu32
- " bytes @ %p; state=%02" PRIX32 "\n",
+ " bytes @ %p; state=%02" PRIX32 "; crc=%08" PRIX32 "\n",
valid ? "valid" : "invalid",
app->hdr.payInfoType, mDownloadState->size,
- mDownloadState->start, segState);
+ mDownloadState->start, segState,
+ mApp ? osAppSegmentGetCrc(mApp) : 0xFFFFFFFF);
freeDownloadState(); // no more access to mDownloadState
@@ -502,11 +515,30 @@
mDownloadState->eraseScheduled = false;
}
+SET_PACKED_STRUCT_MODE_ON
+struct FirmwareWriteCookie
+{
+ uint32_t evtType;
+ union {
+#ifdef LEGACY_HAL_ENABLED
+ struct NanohubHalLegacyContUploadTx respLegacy;
+#endif
+ struct NanohubHalContUploadTx resp;
+ };
+} ATTRIBUTE_PACKED;
+SET_PACKED_STRUCT_MODE_OFF
+
+static void writeCookieFree(void *ptr)
+{
+ struct FirmwareWriteCookie *buf = container_of(ptr, struct FirmwareWriteCookie, resp);
+ heapFree(buf);
+}
+
static void firmwareWrite(void *cookie)
{
bool valid;
bool finished = false;
- struct NanohubHalContUploadTx *resp = cookie;
+ struct FirmwareWriteCookie *resp = cookie;
// only check crc when cookie is NULL (write came from kernel, not HAL)
bool checkCrc = !cookie;
@@ -545,8 +577,15 @@
valid = false;
}
if (resp) {
- resp->success = valid;
- osEnqueueEvtOrFree(EVT_APP_TO_HOST, resp, heapFree);
+ if (resp->evtType == EVT_APP_TO_HOST) {
+#ifdef LEGACY_HAL_ENABLED
+ resp->respLegacy.success = valid;
+ osEnqueueEvtOrFree(EVT_APP_TO_HOST, &resp->respLegacy, writeCookieFree);
+#endif
+ } else {
+ resp->resp.ret.status = !valid;
+ osEnqueueEvtOrFree(EVT_APP_TO_HOST_CHRE, &resp->resp, writeCookieFree);
+ }
}
}
@@ -575,10 +614,10 @@
firmwareFinish(false);
} else if (offset != mDownloadState->srcOffset) {
reply = NANOHUB_FIRMWARE_CHUNK_REPLY_RESTART;
- resetDownloadState(false);
+ resetDownloadState(false, true);
} else {
if (!cookie)
- mDownloadState->srcCrc = crc32(data, len, mDownloadState->srcCrc);
+ mDownloadState->srcCrc = soft_crc32(data, len, mDownloadState->srcCrc);
mDownloadState->srcOffset += len;
memcpy(mDownloadState->data, data, len);
mDownloadState->lenLeft = mDownloadState->len = len;
@@ -602,12 +641,24 @@
return sizeof(*resp);
}
-static uint32_t doFinishFirmwareUpload()
+static uint32_t doFinishFirmwareUpload(uint32_t *addr, uint32_t *crc)
{
uint32_t reply;
if (!mDownloadState) {
reply = appSecErrToNanohubReply(mAppSecStatus);
+ if (addr) {
+ if (mApp)
+ *addr = (uint32_t)mApp;
+ else
+ *addr = 0xFFFFFFFF;
+ }
+ if (crc) {
+ if (mApp)
+ *crc = osAppSegmentGetCrc(mApp);
+ else
+ *crc = 0xFFFFFFFF;
+ }
} else if (mDownloadState->srcOffset == mDownloadState->size) {
reply = NANOHUB_FIRMWARE_UPLOAD_PROCESSING;
} else {
@@ -620,7 +671,7 @@
static uint32_t finishFirmwareUpload(void *rx, uint8_t rx_len, void *tx, uint64_t timestamp)
{
struct NanohubFinishFirmwareUploadResponse *resp = tx;
- resp->uploadReply = doFinishFirmwareUpload();
+ resp->uploadReply = doFinishFirmwareUpload(NULL, NULL);
if (resp->uploadReply != NANOHUB_FIRMWARE_UPLOAD_PROCESSING)
osLog(LOG_INFO, "%s: reply=%" PRIu8 "\n", __func__, resp->uploadReply);
return sizeof(*resp);
@@ -666,26 +717,12 @@
return sizeof(*resp);
}
-static void nanohubDelayStartApps(void *cookie)
-{
- uint32_t status = 0;
- status = osExtAppStartAppsDelayed();
- osLog(LOG_DEBUG, "Started delayed apps; EXT status: %08" PRIX32 "\n", status);
-}
-
static void addDelta(struct ApHubSync *sync, uint64_t apTime, uint64_t hubTime)
{
- static bool delayStart = false;
-
#if DEBUG_APHUB_TIME_SYNC
syncDebugAdd(apTime, hubTime);
#endif
apHubSyncAddDelta(sync, apTime, hubTime);
-
- if (!delayStart) {
- delayStart = true;
- osDefer(nanohubDelayStartApps, NULL, false);
- }
}
static int64_t getAvgDelta(struct ApHubSync *sync)
@@ -951,7 +988,7 @@
if (rx_len >= sizeof(struct HostMsgHdrChre) &&
rx_len == sizeof(struct HostMsgHdrChre) + hostPacket->len &&
osTidById(&hostPacket->appId, &tid)) {
- if (osAppChreVersion(tid) == CHRE_API_VERSION_1_1) {
+ if (osAppChreVersion(tid) >= CHRE_API_VERSION_1_1) {
struct NanohubMsgChreHdr hdr = {
.size = hostPacket->len,
.endpoint = hostPacket->endpoint,
@@ -976,7 +1013,7 @@
} else if (rx_len >= sizeof(struct HostMsgHdrChreV10) &&
rx_len == sizeof(struct HostMsgHdrChreV10) + hostPacketV10->len &&
osTidById(&hostPacketV10->appId, &tid)) {
- if (osAppChreVersion(tid) == CHRE_API_VERSION_1_1) {
+ if (osAppChreVersion(tid) >= CHRE_API_VERSION_1_1) {
struct NanohubMsgChreHdr hdr = {
.size = hostPacketV10->len,
.endpoint = CHRE_HOST_ENDPOINT_UNSPECIFIED,
@@ -1044,7 +1081,7 @@
NANOHUB_COMMAND(NANOHUB_REASON_GET_INTERRUPT,
getInterrupt,
getInterrupt,
- 0,
+ struct { },
struct NanohubGetInterruptRequest),
NANOHUB_COMMAND(NANOHUB_REASON_MASK_INTERRUPT,
maskInterrupt,
@@ -1080,13 +1117,15 @@
return NULL;
}
-static void halSendMgmtResponse(uint32_t cmd, uint32_t status)
+#ifdef LEGACY_HAL_ENABLED
+
+static void halSendLegacyMgmtResponse(uint32_t cmd, uint32_t status)
{
- struct NanohubHalMgmtTx *resp;
+ struct NanohubHalLegacyMgmtTx *resp;
resp = heapAlloc(sizeof(*resp));
if (resp) {
- resp->hdr = (struct NanohubHalHdr) {
+ resp->hdr = (struct NanohubHalLegacyHdr) {
.appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0),
.len = sizeof(*resp) - sizeof(resp->hdr) + sizeof(resp->hdr.msg),
.msg = cmd,
@@ -1096,36 +1135,36 @@
}
}
-static void halExtAppsOn(void *rx, uint8_t rx_len)
+static void halLegacyExtAppsOn(void *rx, uint8_t rx_len)
{
- struct NanohubHalMgmtRx *req = rx;
+ struct NanohubHalLegacyMgmtRx *req = rx;
- halSendMgmtResponse(NANOHUB_HAL_EXT_APPS_ON, osExtAppStartAppsByAppId(le64toh(unaligned_u64(&req->appId))));
+ halSendLegacyMgmtResponse(NANOHUB_HAL_LEGACY_EXT_APPS_ON, osExtAppStartAppsByAppId(le64toh(unaligned_u64(&req->appId))));
}
-static void halExtAppsOff(void *rx, uint8_t rx_len)
+static void halLegacyExtAppsOff(void *rx, uint8_t rx_len)
{
- struct NanohubHalMgmtRx *req = rx;
+ struct NanohubHalLegacyMgmtRx *req = rx;
- halSendMgmtResponse(NANOHUB_HAL_EXT_APPS_OFF, osExtAppStopAppsByAppId(le64toh(unaligned_u64(&req->appId))));
+ halSendLegacyMgmtResponse(NANOHUB_HAL_LEGACY_EXT_APPS_OFF, osExtAppStopAppsByAppId(le64toh(unaligned_u64(&req->appId))));
}
-static void halExtAppDelete(void *rx, uint8_t rx_len)
+static void halLegacyExtAppDelete(void *rx, uint8_t rx_len)
{
- struct NanohubHalMgmtRx *req = rx;
+ struct NanohubHalLegacyMgmtRx *req = rx;
- halSendMgmtResponse(NANOHUB_HAL_EXT_APP_DELETE, osExtAppEraseAppsByAppId(le64toh(unaligned_u64(&req->appId))));
+ halSendLegacyMgmtResponse(NANOHUB_HAL_LEGACY_EXT_APP_DELETE, osExtAppEraseAppsByAppId(le64toh(unaligned_u64(&req->appId))));
}
-static void halQueryMemInfo(void *rx, uint8_t rx_len)
+static void halLegacyQueryMemInfo(void *rx, uint8_t rx_len)
{
}
-static void halQueryApps(void *rx, uint8_t rx_len)
+static void halLegacyQueryApps(void *rx, uint8_t rx_len)
{
- struct NanohubHalQueryAppsRx *req = rx;
- struct NanohubHalQueryAppsTx *resp;
- struct NanohubHalHdr *hdr;
+ struct NanohubHalLegacyQueryAppsRx *req = rx;
+ struct NanohubHalLegacyQueryAppsTx *resp;
+ struct NanohubHalLegacyHdr *hdr;
uint64_t appId;
uint32_t appVer, appSize;
@@ -1133,8 +1172,8 @@
resp = heapAlloc(sizeof(*resp));
if (resp) {
resp->hdr.appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0);
- resp->hdr.len = sizeof(*resp) - sizeof(struct NanohubHalHdr) + 1;
- resp->hdr.msg = NANOHUB_HAL_QUERY_APPS;
+ resp->hdr.len = sizeof(*resp) - sizeof(struct NanohubHalLegacyHdr) + 1;
+ resp->hdr.msg = NANOHUB_HAL_LEGACY_QUERY_APPS;
resp->appId = appId;
resp->version = appVer;
resp->flashUse = appSize;
@@ -1146,16 +1185,16 @@
if (hdr) {
hdr->appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0);
hdr->len = 1;
- hdr->msg = NANOHUB_HAL_QUERY_APPS;
+ hdr->msg = NANOHUB_HAL_LEGACY_QUERY_APPS;
osEnqueueEvtOrFree(EVT_APP_TO_HOST, hdr, heapFree);
}
}
}
-static void halQueryRsaKeys(void *rx, uint8_t rx_len)
+static void halLegacyQueryRsaKeys(void *rx, uint8_t rx_len)
{
- struct NanohubHalQueryRsaKeysRx *req = rx;
- struct NanohubHalQueryRsaKeysTx *resp;
+ struct NanohubHalLegacyQueryRsaKeysRx *req = rx;
+ struct NanohubHalLegacyQueryRsaKeysTx *resp;
int len = 0;
const uint32_t *ptr;
uint32_t numKeys;
@@ -1172,75 +1211,77 @@
}
resp->hdr.appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0);
- resp->hdr.len = sizeof(*resp) - sizeof(struct NanohubHalHdr) + 1 + len;
- resp->hdr.msg = NANOHUB_HAL_QUERY_RSA_KEYS;
+ resp->hdr.len = sizeof(*resp) - sizeof(struct NanohubHalLegacyHdr) + 1 + len;
+ resp->hdr.msg = NANOHUB_HAL_LEGACY_QUERY_RSA_KEYS;
osEnqueueEvtOrFree(EVT_APP_TO_HOST, resp, heapFree);
}
-static void halStartUpload(void *rx, uint8_t rx_len)
+static void halLegacyStartUpload(void *rx, uint8_t rx_len)
{
- struct NanohubHalStartUploadRx *req = rx;
+ struct NanohubHalLegacyStartUploadRx *req = rx;
struct NanohubStartFirmwareUploadRequest hwReq = {
- .size= req->length
+ .size = req->length
};
- struct NanohubHalStartUploadTx *resp;
+ struct NanohubHalLegacyStartUploadTx *resp;
if (!(resp = heapAlloc(sizeof(*resp))))
return;
resp->hdr.appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0);
- resp->hdr.len = sizeof(*resp) - sizeof(struct NanohubHalHdr) + 1;
- resp->hdr.msg = NANOHUB_HAL_START_UPLOAD;
- resp->success = doStartFirmwareUpload(&hwReq);
+ resp->hdr.len = sizeof(*resp) - sizeof(struct NanohubHalLegacyHdr) + 1;
+ resp->hdr.msg = NANOHUB_HAL_LEGACY_START_UPLOAD;
+ resp->success = doStartFirmwareUpload(&hwReq, true);
osEnqueueEvtOrFree(EVT_APP_TO_HOST, resp, heapFree);
}
-static void halContUpload(void *rx, uint8_t rx_len)
+static void halLegacyContUpload(void *rx, uint8_t rx_len)
{
uint32_t offset;
uint32_t reply;
uint8_t len;
- struct NanohubHalContUploadRx *req = rx;
- struct NanohubHalContUploadTx *resp;
+ struct NanohubHalLegacyContUploadRx *req = rx;
+ struct FirmwareWriteCookie *cookie;
- if (!(resp = heapAlloc(sizeof(*resp))))
+ if (!(cookie = heapAlloc(sizeof(*cookie))))
return;
- resp->hdr.appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0);
- resp->hdr.len = sizeof(*resp) - sizeof(struct NanohubHalHdr) + 1;
- resp->hdr.msg = NANOHUB_HAL_CONT_UPLOAD;
+ cookie->evtType = EVT_APP_TO_HOST;
+ cookie->respLegacy.hdr = (struct NanohubHalLegacyHdr) {
+ .appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0),
+ .len = sizeof(cookie->respLegacy) - sizeof(struct NanohubHalLegacyHdr) + 1,
+ .msg = NANOHUB_HAL_LEGACY_CONT_UPLOAD,
+ };
+ cookie->respLegacy.success = false;
if (!mDownloadState) {
reply = NANOHUB_FIRMWARE_CHUNK_REPLY_CANCEL_NO_RETRY;
} else {
offset = le32toh(req->offset);
len = rx_len - sizeof(req->offset);
- reply = doFirmwareChunk(req->data, offset, len, resp);
+ reply = doFirmwareChunk(req->data, offset, len, cookie);
}
if (reply != NANOHUB_FIRMWARE_CHUNK_REPLY_ACCEPTED) {
osLog(LOG_ERROR, "%s: reply=%" PRIu32 "\n", __func__, reply);
- resp->success = false;
-
- osEnqueueEvtOrFree(EVT_APP_TO_HOST, resp, heapFree);
+ osEnqueueEvtOrFree(EVT_APP_TO_HOST, &cookie->respLegacy, writeCookieFree);
}
}
-static void halFinishUpload(void *rx, uint8_t rx_len)
+static void halLegacyFinishUpload(void *rx, uint8_t rx_len)
{
- struct NanohubHalFinishUploadTx *resp;
+ struct NanohubHalLegacyFinishUploadTx *resp;
uint32_t reply;
if (!(resp = heapAlloc(sizeof(*resp))))
return;
resp->hdr.appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0);
- resp->hdr.len = sizeof(*resp) - sizeof(struct NanohubHalHdr) + 1;
- resp->hdr.msg = NANOHUB_HAL_FINISH_UPLOAD;
+ resp->hdr.len = sizeof(*resp) - sizeof(struct NanohubHalLegacyHdr) + 1;
+ resp->hdr.msg = NANOHUB_HAL_LEGACY_FINISH_UPLOAD;
- reply = doFinishFirmwareUpload();
+ reply = doFinishFirmwareUpload(NULL, NULL);
osLog(LOG_INFO, "%s: reply=%" PRIu32 "\n", __func__, reply);
@@ -1249,32 +1290,598 @@
osEnqueueEvtOrFree(EVT_APP_TO_HOST, resp, heapFree);
}
-static void halReboot(void *rx, uint8_t rx_len)
+static void halLegacyReboot(void *rx, uint8_t rx_len)
{
BL.blReboot();
}
+const static struct NanohubHalLegacyCommand mBuiltinHalLegacyCommands[] = {
+ NANOHUB_HAL_LEGACY_COMMAND(NANOHUB_HAL_LEGACY_EXT_APPS_ON,
+ halLegacyExtAppsOn),
+ NANOHUB_HAL_LEGACY_COMMAND(NANOHUB_HAL_LEGACY_EXT_APPS_OFF,
+ halLegacyExtAppsOff),
+ NANOHUB_HAL_LEGACY_COMMAND(NANOHUB_HAL_LEGACY_EXT_APP_DELETE,
+ halLegacyExtAppDelete),
+ NANOHUB_HAL_LEGACY_COMMAND(NANOHUB_HAL_LEGACY_QUERY_MEMINFO,
+ halLegacyQueryMemInfo),
+ NANOHUB_HAL_LEGACY_COMMAND(NANOHUB_HAL_LEGACY_QUERY_APPS,
+ halLegacyQueryApps),
+ NANOHUB_HAL_LEGACY_COMMAND(NANOHUB_HAL_LEGACY_QUERY_RSA_KEYS,
+ halLegacyQueryRsaKeys),
+ NANOHUB_HAL_LEGACY_COMMAND(NANOHUB_HAL_LEGACY_START_UPLOAD,
+ halLegacyStartUpload),
+ NANOHUB_HAL_LEGACY_COMMAND(NANOHUB_HAL_LEGACY_CONT_UPLOAD,
+ halLegacyContUpload),
+ NANOHUB_HAL_LEGACY_COMMAND(NANOHUB_HAL_LEGACY_FINISH_UPLOAD,
+ halLegacyFinishUpload),
+ NANOHUB_HAL_LEGACY_COMMAND(NANOHUB_HAL_LEGACY_REBOOT,
+ halLegacyReboot),
+};
+
+const struct NanohubHalLegacyCommand *nanohubHalLegacyFindCommand(uint8_t msg)
+{
+ uint32_t i;
+
+ for (i = 0; i < ARRAY_SIZE(mBuiltinHalLegacyCommands); i++) {
+ const struct NanohubHalLegacyCommand *cmd = &mBuiltinHalLegacyCommands[i];
+ if (cmd->msg == msg)
+ return cmd;
+ }
+ return NULL;
+}
+
+#endif /* LEGACY_HAL_ENABLED */
+
+static void halSendAppMgmtResponse(struct NanohubHalAppMgmtRx *req, uint32_t status, struct MgmtStatus stat, uint32_t transactionId)
+{
+ struct NanohubHalAppMgmtTx *resp;
+
+ resp = heapAlloc(sizeof(*resp));
+ if (resp) {
+ resp->hdr = (struct NanohubHalHdr) {
+ .appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0),
+ .len = sizeof(*resp) - sizeof(resp->hdr),
+ .transactionId = transactionId,
+ };
+ resp->ret = (struct NanohubHalRet) {
+ .msg = NANOHUB_HAL_APP_MGMT,
+ .status = htole32(status),
+ };
+ resp->cmd = req->cmd;
+ resp->stat = stat;
+ osEnqueueEvtOrFree(EVT_APP_TO_HOST_CHRE, resp, heapFree);
+ }
+}
+
+static void halAppMgmt(void *rx, uint8_t rx_len, uint32_t transactionId)
+{
+ struct NanohubHalAppMgmtRx *req = rx;
+ struct MgmtStatus stat;
+ uint32_t ret;
+
+ switch (req->cmd) {
+ case NANOHUB_HAL_APP_MGMT_START:
+ stat.value= osExtAppStartAppsByAppId(le64toh(unaligned_u64(&req->appId)));
+ ret = stat.op > 0 ? 0 : -1;
+ break;
+ case NANOHUB_HAL_APP_MGMT_STOP:
+ stat.value = osExtAppStopAppsByAppId(le64toh(unaligned_u64(&req->appId)));
+ ret = stat.op > 0 ? 0 : -1;
+ break;
+ case NANOHUB_HAL_APP_MGMT_UNLOAD:
+ stat.value = osExtAppStopAppsByAppId(le64toh(unaligned_u64(&req->appId)));
+ ret = stat.op > 0 ? 0 : -1;
+ break;
+ case NANOHUB_HAL_APP_MGMT_DELETE:
+ stat.value = osExtAppEraseAppsByAppId(le64toh(unaligned_u64(&req->appId)));
+ ret = stat.erase > 0 ? 0 : -1;
+ break;
+ default:
+ return;
+ }
+
+ halSendAppMgmtResponse(req, ret, stat, transactionId);
+}
+
+static void deferHalSysMgmtErase(void *cookie)
+{
+ struct NanohubHalSysMgmtTx *resp = cookie;
+
+ bool success = osEraseShared();
+
+ if (success)
+ resp->ret.status = htole32(0);
+ else
+ resp->ret.status = htole32(-1);
+
+ osEnqueueEvtOrFree(EVT_APP_TO_HOST_CHRE, resp, heapFree);
+}
+
+static void halSysMgmt(void *rx, uint8_t rx_len, uint32_t transactionId)
+{
+ struct NanohubHalSysMgmtRx *req = rx;
+ struct NanohubHalSysMgmtTx *resp;
+ uint32_t ret = 0;
+
+ if (!(resp = heapAlloc(sizeof(*resp))))
+ return;
+
+ resp->hdr = (struct NanohubHalHdr) {
+ .appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0),
+ .len = sizeof(*resp) - sizeof(resp->hdr),
+ .transactionId = transactionId,
+ };
+ resp->ret = (struct NanohubHalRet) {
+ .msg = NANOHUB_HAL_SYS_MGMT,
+ };
+ resp->cmd = req->cmd;
+
+ switch (req->cmd) {
+ case NANOHUB_HAL_SYS_MGMT_ERASE:
+ ret = osExtAppStopAppsByAppId(APP_ID_ANY);
+ osLog(LOG_INFO, "%s: unloaded apps, ret=%08lx\n", __func__, ret);
+ // delay to make sure all apps are unloaded before erasing
+ if (osDefer(deferHalSysMgmtErase, resp, false) == false) {
+ resp->ret.status = htole32(-1);
+ osEnqueueEvtOrFree(EVT_APP_TO_HOST_CHRE, resp, heapFree);
+ }
+ break;
+ case NANOHUB_HAL_SYS_MGMT_REBOOT:
+ BL.blReboot();
+ break;
+ default:
+ resp->ret.status = htole32(-1);
+ osEnqueueEvtOrFree(EVT_APP_TO_HOST_CHRE, resp, heapFree);
+ }
+}
+
+static bool copyTLV64(uint8_t *buf, size_t *offset, size_t max_len, uint8_t tag, uint64_t val)
+{
+ if (*offset + sizeof(uint8_t) + sizeof(uint8_t) + sizeof(uint64_t) > max_len)
+ return false;
+ buf[(*offset)++] = tag;
+ buf[(*offset)++] = sizeof(uint64_t);
+ memcpy(&buf[*offset], &val, sizeof(uint64_t));
+ *offset += sizeof(uint64_t);
+ return true;
+}
+
+static bool copyTLV32(uint8_t *buf, size_t *offset, size_t max_len, uint8_t tag, uint32_t val)
+{
+ if (*offset + sizeof(uint8_t) + sizeof(uint8_t) + sizeof(uint32_t) > max_len)
+ return false;
+ buf[(*offset)++] = tag;
+ buf[(*offset)++] = sizeof(uint32_t);
+ memcpy(&buf[*offset], &val, sizeof(uint32_t));
+ *offset += sizeof(uint32_t);
+ return true;
+}
+
+static bool copyTLV8(uint8_t *buf, size_t *offset, size_t max_len, uint8_t tag, uint8_t val)
+{
+ if (*offset + sizeof(uint8_t) + sizeof(uint8_t) + sizeof(uint8_t) > max_len)
+ return false;
+ buf[(*offset)++] = tag;
+ buf[(*offset)++] = sizeof(uint8_t);
+ memcpy(&buf[*offset], &val, sizeof(uint8_t));
+ *offset += sizeof(uint8_t);
+ return true;
+}
+
+static bool copyTLVEmpty(uint8_t *buf, size_t *offset, size_t max_len, uint8_t tag)
+{
+ if (*offset + sizeof(uint8_t) + sizeof(uint8_t) > max_len)
+ return false;
+ buf[(*offset)++] = tag;
+ buf[(*offset)++] = 0;
+ return true;
+}
+
+static int processAppTags(const struct AppHdr *app, uint32_t crc, uint32_t size, uint8_t *data, uint8_t *tags, int cnt, bool req_tid)
+{
+ int i;
+ size_t offset = 0;
+ const size_t max_len = HOST_HUB_CHRE_PACKET_MAX_LEN - sizeof(struct NanohubHalRet);
+ bool success = true;
+ uint32_t tid;
+ bool tid_valid = false;
+ struct Task *task;
+
+ if (app->hdr.magic != APP_HDR_MAGIC ||
+ app->hdr.fwVer != APP_HDR_VER_CUR ||
+ (app->hdr.fwFlags & FL_APP_HDR_APPLICATION) == 0 ||
+ app->hdr.payInfoType != LAYOUT_APP) {
+ return 0;
+ }
+
+ if (osTidById(&app->hdr.appId, &tid)) {
+ tid_valid = true;
+ task = osTaskFindByTid(tid);
+ if (task) {
+ if (task->app != app)
+ tid_valid = false;
+ } else
+ tid_valid = false;
+ }
+
+ if (!tid_valid && req_tid)
+ return 0;
+
+ for (i=0; i<cnt && success; i++) {
+ switch(tags[i]) {
+ case NANOHUB_HAL_APP_INFO_APPID:
+ success = copyTLV64(data, &offset, max_len, tags[i], app->hdr.appId);
+ break;
+ case NANOHUB_HAL_APP_INFO_CRC:
+ if (size)
+ success = copyTLV32(data, &offset, max_len, tags[i], crc);
+ else
+ success = copyTLVEmpty(data, &offset, max_len, tags[i]);
+ break;
+ case NANOHUB_HAL_APP_INFO_TID:
+ if (tid_valid)
+ success = copyTLV32(data, &offset, max_len, tags[i], tid);
+ else
+ success = copyTLVEmpty(data, &offset, max_len, tags[i]);
+ break;
+ case NANOHUB_HAL_APP_INFO_VERSION:
+ success = copyTLV32(data, &offset, max_len, tags[i], app->hdr.appVer);
+ break;
+ case NANOHUB_HAL_APP_INFO_ADDR:
+ success = copyTLV32(data, &offset, max_len, tags[i], (uint32_t)app);
+ break;
+ case NANOHUB_HAL_APP_INFO_SIZE:
+ if (size)
+ success = copyTLV32(data, &offset, max_len, tags[i], size);
+ else
+ success = copyTLVEmpty(data, &offset, max_len, tags[i]);
+ break;
+ case NANOHUB_HAL_APP_INFO_HEAP:
+ if (tid_valid)
+ success = copyTLV32(data, &offset, max_len, tags[i], heapGetTaskSize(tid));
+ else
+ success = copyTLVEmpty(data, &offset, max_len, tags[i]);
+ break;
+ case NANOHUB_HAL_APP_INFO_DATA:
+ success = copyTLV32(data, &offset, max_len, tags[i], app->sect.got_end - app->sect.data_start);
+ break;
+ case NANOHUB_HAL_APP_INFO_BSS:
+ success = copyTLV32(data, &offset, max_len, tags[i], app->sect.bss_end - app->sect.bss_start);
+ break;
+ case NANOHUB_HAL_APP_INFO_CHRE_MAJOR:
+ if (app->hdr.fwFlags & FL_APP_HDR_CHRE)
+ success = copyTLV8(data, &offset, max_len, tags[i],
+ (app->hdr.chreApiMajor == 0xFF && app->hdr.chreApiMinor == 0xFF) ? 0x01 :
+ app->hdr.chreApiMajor);
+ else
+ success = copyTLVEmpty(data, &offset, max_len, tags[i]);
+ break;
+ case NANOHUB_HAL_APP_INFO_CHRE_MINOR:
+ if (app->hdr.fwFlags & FL_APP_HDR_CHRE)
+ success = copyTLV8(data, &offset, max_len, tags[i],
+ (app->hdr.chreApiMajor == 0xFF && app->hdr.chreApiMinor == 0xFF) ? 0x00 :
+ app->hdr.chreApiMinor);
+ else
+ success = copyTLVEmpty(data, &offset, max_len, tags[i]);
+ break;
+ case NANOHUB_HAL_APP_INFO_END:
+ default:
+ success = false;
+ copyTLVEmpty(data, &offset, max_len, NANOHUB_HAL_APP_INFO_END);
+ break;
+ }
+ }
+
+ return offset;
+}
+
+static void halAppInfo(void *rx, uint8_t rx_len, uint32_t transactionId)
+{
+ struct NanohubHalAppInfoRx *req = rx;
+ struct NanohubHalAppInfoTx *resp;
+ struct SegmentIterator it;
+ uint32_t state;
+ int ret, i;
+ uint32_t sharedSize, numApps;
+ const struct AppHdr *internal;
+ const uint8_t *shared;
+
+ if (!(resp = heapAlloc(sizeof(*resp))))
+ return;
+
+ resp->hdr = (struct NanohubHalHdr) {
+ .appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0),
+ .len = sizeof(*resp) - sizeof(resp->hdr) - sizeof(resp->data),
+ .transactionId = transactionId,
+ };
+ resp->ret = (struct NanohubHalRet) {
+ .msg = NANOHUB_HAL_APP_INFO,
+ };
+
+ shared = platGetSharedAreaInfo(&sharedSize);
+ internal = platGetInternalAppList(&numApps);
+
+ if ((le32toh(req->addr) >= (uint32_t)shared && le32toh(req->addr) < (uint32_t)shared + sharedSize) ||
+ (le32toh(req->addr) < (uint32_t)shared &&
+ ((uint32_t)shared < (uint32_t)internal ||
+ (numApps > 0 && le32toh(req->addr) > (uint32_t)(internal+numApps-1))))) {
+ osSegmentIteratorInit(&it);
+ while (osSegmentIteratorNext(&it)) {
+ state = osSegmentGetState(it.seg);
+ switch (state) {
+ case SEG_ST_EMPTY:
+ case SEG_ST_RESERVED:
+ osEnqueueEvtOrFree(EVT_APP_TO_HOST_CHRE, resp, heapFree);
+ return;
+ case SEG_ST_ERASED:
+ case SEG_ST_VALID:
+ if (le32toh(req->addr) <= (uint32_t)osSegmentGetData(it.seg)) {
+ ret = processAppTags(osSegmentGetData(it.seg), osSegmentGetCrc(it.seg), osSegmentGetSize(it.seg), resp->data, req->tags, rx_len - 4, state == SEG_ST_ERASED);
+ if (ret > 0) {
+ resp->hdr.len += ret;
+ osEnqueueEvtOrFree(EVT_APP_TO_HOST_CHRE, resp, heapFree);
+ return;
+ }
+ }
+ break;
+ }
+ }
+ } else {
+ for (i = 0; i < numApps; i++, internal++) {
+ if (le32toh(req->addr) <= (uint32_t)internal) {
+ ret = processAppTags(internal, 0, 0, resp->data, req->tags, rx_len - 4, false);
+ if (ret > 0) {
+ resp->hdr.len += ret;
+ osEnqueueEvtOrFree(EVT_APP_TO_HOST_CHRE, resp, heapFree);
+ return;
+ }
+ }
+ }
+ }
+
+ osEnqueueEvtOrFree(EVT_APP_TO_HOST_CHRE, resp, heapFree);
+}
+
+static void halSysInfo(void *rx, uint8_t rx_len, uint32_t transactionId)
+{
+ extern uint8_t __code_start[];
+ extern uint8_t __code_end[];
+ extern uint8_t __text_end[];
+ extern uint8_t __ram_start[];
+ extern uint8_t __ram_end[];
+
+ struct NanohubHalSysInfoRx *req = rx;
+ struct NanohubHalSysInfoTx *resp;
+ int i;
+ size_t offset = 0;
+ const size_t max_len = HOST_HUB_CHRE_PACKET_MAX_LEN - sizeof(struct NanohubHalRet);
+ bool success = true;
+ int free, chunks, largest;
+ uint32_t shared_size;
+
+ free = heapGetFreeSize(&chunks, &largest);
+
+ if (!(resp = heapAlloc(sizeof(*resp))))
+ return;
+
+ resp->hdr = (struct NanohubHalHdr) {
+ .appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0),
+ .len = sizeof(*resp) - sizeof(resp->hdr) - sizeof(resp->data),
+ .transactionId = transactionId,
+ };
+ resp->ret = (struct NanohubHalRet) {
+ .msg = NANOHUB_HAL_SYS_INFO,
+ };
+
+ for (i=0; i<rx_len && success; i++) {
+ switch(req->tags[i]) {
+ case NANOHUB_HAL_SYS_INFO_HEAP_FREE:
+ if (free >= 0)
+ success = copyTLV32(resp->data, &offset, max_len, req->tags[i], free);
+ else
+ success = copyTLVEmpty(resp->data, &offset, max_len, req->tags[i]);
+ break;
+ case NANOHUB_HAL_SYS_INFO_RAM_SIZE:
+ success = copyTLV32(resp->data, &offset, max_len, req->tags[i], __ram_end - __ram_start);
+ break;
+ case NANOHUB_HAL_SYS_INFO_EEDATA_SIZE:
+ success = copyTLV32(resp->data, &offset, max_len, req->tags[i], eeDataGetSize());
+ break;
+ case NANOHUB_HAL_SYS_INFO_EEDATA_FREE:
+ success = copyTLV32(resp->data, &offset, max_len, req->tags[i], eeDataGetFree());
+ break;
+ case NANOHUB_HAL_SYS_INFO_CODE_SIZE:
+ success = copyTLV32(resp->data, &offset, max_len, req->tags[i], __code_end - __code_start);
+ break;
+ case NANOHUB_HAL_SYS_INFO_CODE_FREE:
+ success = copyTLV32(resp->data, &offset, max_len, req->tags[i], __code_end - __text_end);
+ break;
+ case NANOHUB_HAL_SYS_INFO_SHARED_SIZE:
+ platGetSharedAreaInfo(&shared_size);
+ success = copyTLV32(resp->data, &offset, max_len, req->tags[i], shared_size);
+ break;
+ case NANOHUB_HAL_SYS_INFO_SHARED_FREE:
+ success = copyTLV32(resp->data, &offset, max_len, req->tags[i], osSegmentGetFree());
+ break;
+ case NANOHUB_HAL_SYS_INFO_END:
+ default:
+ success = false;
+ copyTLVEmpty(resp->data, &offset, max_len, NANOHUB_HAL_APP_INFO_END);
+ break;
+ }
+ }
+
+ resp->hdr.len += offset;
+
+ osEnqueueEvtOrFree(EVT_APP_TO_HOST_CHRE, resp, heapFree);
+}
+
+static void halKeyInfo(void *rx, uint8_t rx_len, uint32_t transactionId)
+{
+ struct NanohubHalKeyInfoRx *req = rx;
+ struct NanohubHalKeyInfoTx *resp;
+ const uint32_t *ptr;
+ uint32_t numKeys;
+ uint32_t dataLength;
+
+ if (!(resp = heapAlloc(sizeof(*resp))))
+ return;
+
+ ptr = BL.blGetPubKeysInfo(&numKeys);
+
+ resp->hdr = (struct NanohubHalHdr) {
+ .appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0),
+ .len = sizeof(*resp) - sizeof(resp->hdr) - sizeof(resp->data),
+ .transactionId = transactionId,
+ };
+ resp->ret = (struct NanohubHalRet) {
+ .msg = NANOHUB_HAL_KEY_INFO,
+ };
+
+ resp->keyLength = 0;
+
+ if (ptr && req->keyNum < numKeys) {
+ if (req->dataOffset < RSA_BYTES) {
+ resp->keyLength = RSA_BYTES;
+ if (RSA_BYTES - req->dataOffset > NANOHUB_RSA_KEY_CHUNK_LEN)
+ dataLength = NANOHUB_RSA_KEY_CHUNK_LEN;
+ else
+ dataLength = RSA_BYTES - req->dataOffset;
+ memcpy(resp->data, (const uint8_t *)ptr + (req->keyNum * RSA_BYTES) + req->dataOffset, dataLength);
+ resp->hdr.len += dataLength;
+ }
+ }
+
+ osEnqueueEvtOrFree(EVT_APP_TO_HOST_CHRE, resp, heapFree);
+}
+
+static void halStartUpload(void *rx, uint8_t rx_len, uint32_t transactionId)
+{
+ struct NanohubHalStartUploadRx *req = rx;
+ struct NanohubStartFirmwareUploadRequest hwReq = {
+ .size = req->length
+ };
+ struct NanohubHalStartUploadTx *resp;
+
+ if (!(resp = heapAlloc(sizeof(*resp))))
+ return;
+
+ resp->hdr = (struct NanohubHalHdr) {
+ .appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0),
+ .len = sizeof(*resp) - sizeof(resp->hdr),
+ .transactionId = transactionId,
+ };
+
+ resp->ret.msg = NANOHUB_HAL_START_UPLOAD;
+ if (doStartFirmwareUpload(&hwReq, false))
+ resp->ret.status = NANOHUB_FIRMWARE_CHUNK_REPLY_ACCEPTED;
+ else
+ resp->ret.status = NANOHUB_FIRMWARE_CHUNK_REPLY_NO_SPACE;
+
+ osEnqueueEvtOrFree(EVT_APP_TO_HOST_CHRE, resp, heapFree);
+}
+
+static void halContUpload(void *rx, uint8_t rx_len, uint32_t transactionId)
+{
+ uint32_t offset;
+ uint32_t reply;
+ uint8_t len;
+ struct NanohubHalContUploadRx *req = rx;
+ struct FirmwareWriteCookie *cookie;
+
+ if (!(cookie = heapAlloc(sizeof(*cookie))))
+ return;
+
+ cookie->evtType = EVT_APP_TO_HOST_CHRE;
+ cookie->resp.hdr = (struct NanohubHalHdr) {
+ .appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0),
+ .len = sizeof(cookie->resp) - sizeof(cookie->resp.hdr),
+ .transactionId = transactionId,
+ };
+ cookie->resp.ret = (struct NanohubHalRet) {
+ .msg = NANOHUB_HAL_CONT_UPLOAD,
+ };
+
+ if (!mDownloadState) {
+ reply = NANOHUB_FIRMWARE_CHUNK_REPLY_CANCEL_NO_RETRY;
+ } else {
+ offset = le32toh(req->offset);
+ len = rx_len - sizeof(req->offset);
+ reply = doFirmwareChunk(req->data, offset, len, cookie);
+ }
+ if (reply != NANOHUB_FIRMWARE_CHUNK_REPLY_ACCEPTED) {
+ osLog(LOG_ERROR, "%s: reply=%" PRIu32 "\n", __func__, reply);
+
+ cookie->resp.ret.status = reply;
+
+ osEnqueueEvtOrFree(EVT_APP_TO_HOST_CHRE, &cookie->resp, writeCookieFree);
+ }
+}
+
+static void halFinishUpload(void *rx, uint8_t rx_len, uint32_t transactionId)
+{
+ struct NanohubHalFinishUploadTx *resp;
+ uint32_t reply;
+ uint32_t addr = 0xFFFFFFFF;
+ uint32_t crc = 0xFFFFFFFF;
+
+ if (!(resp = heapAlloc(sizeof(*resp))))
+ return;
+
+ resp->hdr = (struct NanohubHalHdr) {
+ .appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0),
+ .len = sizeof(*resp) - sizeof(resp->hdr),
+ .transactionId = transactionId,
+ };
+
+ reply = doFinishFirmwareUpload(&addr, &crc);
+
+ osLog(LOG_INFO, "%s: reply=%" PRIu32 "\n", __func__, reply);
+
+ resp->ret = (struct NanohubHalRet) {
+ .msg = NANOHUB_HAL_FINISH_UPLOAD,
+ .status = reply,
+ };
+
+ resp->addr = addr;
+ resp->crc = crc;
+
+ osEnqueueEvtOrFree(EVT_APP_TO_HOST_CHRE, resp, heapFree);
+}
+
const static struct NanohubHalCommand mBuiltinHalCommands[] = {
- NANOHUB_HAL_COMMAND(NANOHUB_HAL_EXT_APPS_ON,
- halExtAppsOn),
- NANOHUB_HAL_COMMAND(NANOHUB_HAL_EXT_APPS_OFF,
- halExtAppsOff),
- NANOHUB_HAL_COMMAND(NANOHUB_HAL_EXT_APP_DELETE,
- halExtAppDelete),
- NANOHUB_HAL_COMMAND(NANOHUB_HAL_QUERY_MEMINFO,
- halQueryMemInfo),
- NANOHUB_HAL_COMMAND(NANOHUB_HAL_QUERY_APPS,
- halQueryApps),
- NANOHUB_HAL_COMMAND(NANOHUB_HAL_QUERY_RSA_KEYS,
- halQueryRsaKeys),
+ NANOHUB_HAL_COMMAND(NANOHUB_HAL_APP_MGMT,
+ halAppMgmt,
+ struct NanohubHalAppMgmtRx,
+ struct NanohubHalAppMgmtRx),
+ NANOHUB_HAL_COMMAND(NANOHUB_HAL_SYS_MGMT,
+ halSysMgmt,
+ struct NanohubHalSysMgmtRx,
+ struct NanohubHalSysMgmtRx),
+ NANOHUB_HAL_COMMAND(NANOHUB_HAL_APP_INFO,
+ halAppInfo,
+ __le32,
+ struct NanohubHalAppInfoRx),
+ NANOHUB_HAL_COMMAND(NANOHUB_HAL_SYS_INFO,
+ halSysInfo,
+ struct { },
+ struct NanohubHalSysInfoRx),
+ NANOHUB_HAL_COMMAND(NANOHUB_HAL_KEY_INFO,
+ halKeyInfo,
+ struct NanohubHalKeyInfoRx,
+ struct NanohubHalKeyInfoRx),
NANOHUB_HAL_COMMAND(NANOHUB_HAL_START_UPLOAD,
- halStartUpload),
+ halStartUpload,
+ struct NanohubHalStartUploadRx,
+ struct NanohubHalStartUploadRx),
NANOHUB_HAL_COMMAND(NANOHUB_HAL_CONT_UPLOAD,
- halContUpload),
+ halContUpload,
+ __le32,
+ struct NanohubHalContUploadRx),
NANOHUB_HAL_COMMAND(NANOHUB_HAL_FINISH_UPLOAD,
- halFinishUpload),
- NANOHUB_HAL_COMMAND(NANOHUB_HAL_REBOOT,
- halReboot),
+ halFinishUpload,
+ struct { },
+ struct { }),
};
const struct NanohubHalCommand *nanohubHalFindCommand(uint8_t msg)
@@ -1289,6 +1896,7 @@
return NULL;
}
+
int64_t hostGetTimeDelta(void)
{
int64_t delta = getAvgDelta(&mTimeSync);
diff --git a/firmware/os/core/nanohub_chre.c b/firmware/os/core/nanohub_chre.c
index 5ed419b..e6fc951 100644
--- a/firmware/os/core/nanohub_chre.c
+++ b/firmware/os/core/nanohub_chre.c
@@ -207,7 +207,7 @@
chreMessageFreeFunction *freeCallback)
{
bool result = false;
- struct HostHubRawPacket *hostMsg = NULL;
+ struct HostHubChrePacket *hostMsg = NULL;
if (messageSize > CHRE_MESSAGE_TO_HOST_MAX_SIZE || (messageSize && !message))
goto out;
@@ -220,8 +220,10 @@
memcpy(hostMsg+1, message, messageSize);
hostMsg->appId = osChreGetAppId();
- hostMsg->dataLen = messageSize;
- result = osEnqueueEvtOrFree(EVT_APP_TO_HOST, hostMsg, heapFree);
+ hostMsg->messageSize = messageSize;
+ hostMsg->messageType = messageType;
+ hostMsg->hostEndpoint = hostEndpoint;
+ result = osEnqueueEvtOrFree(EVT_APP_TO_HOST_CHRE, hostMsg, heapFree);
out:
if (freeCallback)
@@ -538,6 +540,16 @@
osEventsUnsubscribe(2, EVT_APP_STARTED, EVT_APP_STOPPED);
}
+static void osChreEventHostSleep(uintptr_t *retValP, va_list args)
+{
+ // bool enable = va_arg(args, int();
+}
+
+static void osChreEventIsHostAwake(uintptr_t *retValP, va_list args)
+{
+ *retValP = true;
+}
+
static void osChreDrvGnssGetCap(uintptr_t *retValP, va_list args)
{
*retValP = CHRE_GNSS_CAPABILITIES_NONE;
@@ -570,6 +582,12 @@
*retValP = false;
}
+static void osChreDrvGnssConfLocMon(uintptr_t *retValP, va_list args)
+{
+ // bool enable = va_args(args, bool);
+ *retValP = false;
+}
+
static void osChreDrvWifiGetCap(uintptr_t *retValP, va_list args)
{
*retValP = CHRE_WIFI_CAPABILITIES_NONE;
@@ -600,6 +618,33 @@
*retValP = false;
}
+static void osChreDrvAudioGetSrc(uintptr_t *retValP, va_list args)
+{
+ // uint32_t handle = va_args(args, uint32_t);
+ // struct chreAudioSource *audioSource = va_args(args, struct chreAudioSource *);
+ *retValP = false;
+}
+
+static void osChreDrvAudioConfSrc(uintptr_t *retValP, va_list args)
+{
+ // uint32_t handle = va_args(args, uint32_t);
+ // bool enable = va_args(args, int);
+ // uint32_t duration_lo = va_arg(args, uint32_t);
+ // uint32_t duration_hi = va_arg(args, uint32_t);
+ // uint64_t bufferDuration = (((uint64_t)dur_hi) << 32) | dur_lo;
+ // uint32_t interval_lo = va_args(args, uint32_t);
+ // uint32_t interval_hi = va_args(args, uint32_t);
+ // uint64_t deliveryInterval = (((uint64_t)del_hi) << 32) | del_lo;
+ *retValP = false;
+}
+
+static void osChreDrvAudioGetStatus(uintptr_t *retValP, va_list args)
+{
+ // uint32_t handle = va_args(args, uint32_t);
+ // struct chreAudioSourceStatus *status = va_args(args, struct chreAudioSourceStatus *);
+ *retValP = false;
+}
+
static const struct SyscallTable chreMainApiTable = {
.numEntries = SYSCALL_CHRE_MAIN_API_LAST,
.entry = {
@@ -618,7 +663,7 @@
[SYSCALL_CHRE_MAIN_API_SEND_MSG] = { .func = osChreApiSendMessageToHost },
[SYSCALL_CHRE_MAIN_API_SENSOR_FIND_DEFAULT] = { .func = osChreApiSensorFindDefault },
[SYSCALL_CHRE_MAIN_API_SENSOR_GET_INFO_OLD] = { .func = osChreApiSensorGetInfoOld },
- [SYSCALL_CHRE_MAIN_API_SENSOR_GET_INFO] = { .func = osChreApiSensorGetInfo },
+ [SYSCALL_CHRE_MAIN_API_SENSOR_GET_INFO] = { .func = osChreApiSensorGetInfo },
[SYSCALL_CHRE_MAIN_API_SENSOR_GET_STATUS] = { .func = osChreApiSensorGetStatus },
[SYSCALL_CHRE_MAIN_API_SENSOR_CONFIG] = { .func = osChreApiSensorConfig },
[SYSCALL_CHRE_MAIN_API_GET_OS_API_VERSION] = { .func = osChreApiChreApiVersion },
@@ -635,6 +680,8 @@
[SYSCALL_CHRE_MAIN_EVENT_INFO_BY_APP_ID] = { .func = osChreEventInfoByAppId },
[SYSCALL_CHRE_MAIN_EVENT_INFO_BY_INST_ID] = { .func = osChreEeventInfoByInstId },
[SYSCALL_CHRE_MAIN_EVENT_CFG_INFO] = { .func = osChreEventCfgInfo },
+ [SYSCALL_CHRE_MAIN_EVENT_HOST_SLEEP] = { .func = osChreEventHostSleep },
+ [SYSCALL_CHRE_MAIN_EVENT_IS_HOST_AWAKE] = { .func = osChreEventIsHostAwake },
},
};
@@ -654,6 +701,7 @@
[SYSCALL_CHRE_DRV_GNSS_LOC_STOP_ASYNC] = { .func = osChreDrvGnssLocStopAsync },
[SYSCALL_CHRE_DRV_GNSS_MEAS_START_ASYNC] = { .func = osChreDrvGnssMeasStartAsync },
[SYSCALL_CHRE_DRV_GNSS_MEAS_STOP_ASYNC] = { .func = osChreDrvGnssMeasStopAsync },
+ [SYSCALL_CHRE_DRV_GNSS_CONF_PASV_LOC_LIS] = { .func = osChreDrvGnssConfLocMon },
},
};
@@ -674,12 +722,22 @@
},
};
+static const struct SyscallTable chreDrvAudioTable = {
+ .numEntries = SYSCALL_CHRE_DRV_AUDIO_LAST,
+ .entry = {
+ [SYSCALL_CHRE_DRV_AUDIO_GET_SRC] = { .func = osChreDrvAudioGetSrc },
+ [SYSCALL_CHRE_DRV_AUDIO_CONF_SRC] = { .func = osChreDrvAudioConfSrc },
+ [SYSCALL_CHRE_DRV_AUDIO_GET_STATUS] = { .func = osChreDrvAudioGetStatus },
+ },
+};
+
static const struct SyscallTable chreDriversTable = {
.numEntries = SYSCALL_CHRE_DRV_LAST,
.entry = {
[SYSCALL_CHRE_DRV_GNSS] = { .subtable = (struct SyscallTable*)&chreDrvGnssTable, },
[SYSCALL_CHRE_DRV_WIFI] = { .subtable = (struct SyscallTable*)&chreDrvWifiTable, },
[SYSCALL_CHRE_DRV_WWAN] = { .subtable = (struct SyscallTable*)&chreDrvWwanTable, },
+ [SYSCALL_CHRE_DRV_AUDIO] = { .subtable = (struct SyscallTable*)&chreDrvAudioTable },
},
};
diff --git a/firmware/os/core/seos.c b/firmware/os/core/seos.c
index ff8aa7d..9756964 100644
--- a/firmware/os/core/seos.c
+++ b/firmware/os/core/seos.c
@@ -300,12 +300,25 @@
static void osTaskRelease(struct Task *task)
{
- uint32_t task_tid = task->tid;
+ uint32_t taskTid = task->tid;
+ uint32_t platErr, sensorErr;
+ int timErr, heapErr;
+ uint64_t appId;
- platFreeResources(task_tid); // HW resources cleanup (IRQ, DMA etc)
- sensorFreeAll(task_tid);
- timTimerCancelAll(task_tid);
- heapFreeAll(task_tid);
+ if (task->app)
+ appId = task->app->hdr.appId;
+ else
+ appId = 0;
+
+ platErr = platFreeResources(taskTid); // HW resources cleanup (IRQ, DMA etc)
+ sensorErr = sensorFreeAll(taskTid);
+ timErr = timTimerCancelAll(taskTid);
+ heapErr = heapFreeAll(taskTid);
+
+ if (platErr || sensorErr || timErr || heapErr)
+ osLog(LOG_WARN, "released app ID 0x%" PRIx64 "; plat:%08" PRIx32 " sensor:%08" PRIx32 " tim:%d heap:%d; TID %04" PRIX32 "\n", appId, platErr, sensorErr, timErr, heapErr, taskTid);
+ else
+ osLog(LOG_INFO, "released app ID 0x%" PRIx64 "; TID %04" PRIX32 "\n", appId, taskTid);
}
static inline void osTaskEnd(struct Task *task)
@@ -500,6 +513,24 @@
return (struct Segment *)(start + size);
}
+uint32_t osSegmentGetFree()
+{
+ struct SegmentIterator it;
+ const struct Segment *storageSeg = NULL;
+
+ osSegmentIteratorInit(&it);
+ while (osSegmentIteratorNext(&it)) {
+ if (osSegmentGetState(it.seg) == SEG_ST_EMPTY) {
+ storageSeg = it.seg;
+ break;
+ }
+ }
+ if (!storageSeg || storageSeg > it.sharedEnd)
+ return 0;
+
+ return (uint8_t *)it.sharedEnd - (uint8_t *)storageSeg;
+}
+
struct Segment *osGetSegment(const struct AppHdr *app)
{
uint32_t size;
@@ -601,13 +632,13 @@
footerLen = (-fullSize) & 3;
memset(footer, 0x00, footerLen);
-#ifdef SEGMENT_CRC_SUPPORT
- struct SegmentFooter segFooter {
- .crc = ~crc32(storageSeg, fullSize, ~0),
+ wdtDisableClk();
+ struct SegmentFooter segFooter = {
+ .crc = ~soft_crc32(storageSeg, fullSize, ~0),
};
+ wdtEnableClk();
memcpy(&footer[footerLen], &segFooter, sizeof(segFooter));
footerLen += sizeof(segFooter);
-#endif
if (ret && footerLen)
ret = osWriteShared((uint8_t*)storageSeg + fullSize, footer, footerLen);
@@ -660,10 +691,10 @@
static bool osExtAppIsValid(const struct AppHdr *app, uint32_t len)
{
- //TODO: when CRC support is ready, add CRC check here
return osAppIsValid(app) &&
len >= sizeof(*app) &&
osAppSegmentGetState(app) == SEG_ST_VALID &&
+ osAppSegmentCalcCrcResidue(app) == CRC_RESIDUE &&
!(app->hdr.fwFlags & FL_APP_HDR_INTERNAL);
}
@@ -721,7 +752,7 @@
// print external NanoApp info to facilitate NanoApp debugging
if (!(task->app->hdr.fwFlags & FL_APP_HDR_INTERNAL))
osLog(LOG_INFO,
- "loaded app ID 0x%" PRIx64 " at flash base 0x%" PRIxPTR " ram base 0x%" PRIxPTR "; TID %04X\n",
+ "loaded app ID 0x%" PRIx64 " at flash base 0x%" PRIxPTR " ram base 0x%" PRIxPTR "; TID %04" PRIX16 "\n",
task->app->hdr.appId, (uintptr_t) task->app, (uintptr_t) task->platInfo.data, task->tid);
done = osTaskInit(task);
@@ -768,20 +799,20 @@
osStopTask(task, true);
}
-static bool matchDelayStart(const void *cookie, const struct AppHdr *app)
+static bool matchAutoStart(const void *cookie, const struct AppHdr *app)
{
bool match = (bool)cookie;
if (app->hdr.fwFlags & FL_APP_HDR_CHRE) {
if (app->hdr.chreApiMajor == 0xFF && app->hdr.chreApiMinor == 0xFF)
- return !match;
+ return match;
else if ((app->hdr.chreApiMajor < 0x01) ||
(app->hdr.chreApiMajor == 0x01 && app->hdr.chreApiMinor < 0x01))
- return !match;
- else
return match;
+ else
+ return !match;
} else {
- return !match;
+ return match;
}
}
@@ -951,11 +982,6 @@
return osExtAppStartApps(matchAppId, &appId);
}
-uint32_t osExtAppStartAppsDelayed()
-{
- return osExtAppStartApps(matchDelayStart, (void *)true);
-}
-
static void osStartTasks(void)
{
const struct AppHdr *app;
@@ -1006,7 +1032,7 @@
}
osLog(LOG_DEBUG, "Starting external apps...\n");
- status = osExtAppStartApps(matchDelayStart, (void *)false);
+ status = osExtAppStartApps(matchAutoStart, (void *)true);
osLog(LOG_DEBUG, "Started %" PRIu32 " internal apps; EXT status: %08" PRIX32 "\n", taskCnt, status);
}
@@ -1203,13 +1229,22 @@
static bool osEventsSubscribeUnsubscribeV(bool sub, uint32_t numEvts, va_list ap)
{
- union SeosInternalSlabData *act = slabAllocatorAlloc(mMiscInternalThingsSlab);
+ struct Task *task = osGetCurrentTask();
+ union SeosInternalSlabData *act;
int i;
- if (!act || numEvts > MAX_EVT_SUB_CNT)
+ if (!sub && osTaskTestFlags(task, FL_TASK_STOPPED)) // stopping, so this is a no-op
+ return true;
+
+ if (numEvts > MAX_EVT_SUB_CNT)
return false;
- act->evtSub.tid = osGetCurrentTid();
+ act = slabAllocatorAlloc(mMiscInternalThingsSlab);
+
+ if (!act)
+ return false;
+
+ act->evtSub.tid = task->tid;
act->evtSub.numEvts = numEvts;
for (i = 0; i < numEvts; i++)
act->evtSub.evts[i] = va_arg(ap, uint32_t);
@@ -1272,12 +1307,8 @@
osTaskAddIoCount(task, 1);
- if (osTaskTestFlags(task, FL_TASK_STOPPED)) {
- handleEventFreeing(evtType, evtData, evtFreeInfo);
- return true;
- }
-
- if (!evtQueueEnqueue(mEvtsInternal, evtType, evtData, evtFreeInfo, urgent)) {
+ if (osTaskTestFlags(task, FL_TASK_STOPPED) ||
+ !evtQueueEnqueue(mEvtsInternal, evtType, evtData, evtFreeInfo, urgent)) {
osTaskAddIoCount(task, -1);
return false;
}
@@ -1377,7 +1408,7 @@
return osEnqueuePrivateEvtEx(evtType & EVT_MASK, evtData, taggedPtrMakeFromUint(osGetCurrentTid()), toTid);
}
-bool osTidById(uint64_t *appId, uint32_t *tid)
+bool osTidById(const uint64_t *appId, uint32_t *tid)
{
struct Task *task;
diff --git a/firmware/os/drivers/bosch_bmi160/bosch_bmi160.c b/firmware/os/drivers/bosch_bmi160/bosch_bmi160.c
index 513a9c8..3f272d6 100644
--- a/firmware/os/drivers/bosch_bmi160/bosch_bmi160.c
+++ b/firmware/os/drivers/bosch_bmi160/bosch_bmi160.c
@@ -16,6 +16,7 @@
#include <algos/time_sync.h>
#include <atomic.h>
+#include <common/math/macros.h>
#include <cpu/cpuMath.h>
#include <errno.h>
#include <gpio.h>
@@ -40,7 +41,7 @@
#include <variant/variant.h>
#ifdef MAG_SLAVE_PRESENT
-#include <calibration/magnetometer/mag_cal.h>
+#include <calibration/magnetometer/mag_cal/mag_cal.h>
#endif
#ifdef ACCEL_CAL_ENABLED
@@ -61,13 +62,8 @@
#ifdef GYRO_CAL_ENABLED
#include <calibration/gyroscope/gyro_cal.h>
-#include <common/math/macros.h>
#endif // GYRO_CAL_ENABLED
-#if defined(GYRO_CAL_DBG_ENABLED) || defined(OVERTEMPCAL_DBG_ENABLED)
-#include <calibration/util/cal_log.h>
-#endif // GYRO_CAL_DBG_ENABLED || OVERTEMPCAL_DBG_ENABLED
-
#ifdef OVERTEMPCAL_ENABLED
#include <calibration/over_temp/over_temp_cal.h>
#endif // OVERTEMPCAL_ENABLED
@@ -108,7 +104,7 @@
#define DBG_WM_CALC 0
#define TIMESTAMP_DBG 0
-#define BMI160_APP_VERSION 17
+#define BMI160_APP_VERSION 20
// fixme: to list required definitions for a slave mag
#ifdef USE_BMM150
@@ -259,6 +255,18 @@
#define MAX_NUM_COMMS_EVENT_SAMPLES 15
+#ifndef BMI160_ACC_SAMPLES
+#define BMI160_ACC_SAMPLES 3000
+#endif
+
+#ifndef BMI160_GYRO_SAMPLES
+#define BMI160_GYRO_SAMPLES 20
+#endif
+
+#ifndef BMI160_MAG_SAMPLES
+#define BMI160_MAG_SAMPLES 600
+#endif
+
// Default accel range is 8g
#ifndef BMI160_ACC_RANGE_G
#define BMI160_ACC_RANGE_G 8
@@ -749,18 +757,19 @@
{
#ifdef ACCEL_CAL_ENABLED
{ DEC_INFO_RATE_RAW_BIAS("Accelerometer", AccRates, SENS_TYPE_ACCEL, NUM_AXIS_THREE,
- NANOHUB_INT_NONWAKEUP, 3000, SENS_TYPE_ACCEL_RAW, 1.0/kScale_acc,
- SENS_TYPE_ACCEL_BIAS) },
+ NANOHUB_INT_NONWAKEUP, BMI160_ACC_SAMPLES, SENS_TYPE_ACCEL_RAW,
+ 1.0/kScale_acc, SENS_TYPE_ACCEL_BIAS) },
#else
{ DEC_INFO_RATE_RAW("Accelerometer", AccRates, SENS_TYPE_ACCEL, NUM_AXIS_THREE,
- NANOHUB_INT_NONWAKEUP, 3000, SENS_TYPE_ACCEL_RAW, 1.0/kScale_acc) },
+ NANOHUB_INT_NONWAKEUP, BMI160_ACC_SAMPLES, SENS_TYPE_ACCEL_RAW,
+ 1.0/kScale_acc) },
#endif
{ DEC_INFO_RATE_BIAS("Gyroscope", GyrRates, SENS_TYPE_GYRO, NUM_AXIS_THREE,
- NANOHUB_INT_NONWAKEUP, 20, SENS_TYPE_GYRO_BIAS) },
+ NANOHUB_INT_NONWAKEUP, BMI160_GYRO_SAMPLES, SENS_TYPE_GYRO_BIAS) },
#ifdef MAG_SLAVE_PRESENT
{ DEC_INFO_RATE_RAW_BIAS("Magnetometer", MagRates, SENS_TYPE_MAG, NUM_AXIS_THREE,
- NANOHUB_INT_NONWAKEUP, 600, SENS_TYPE_MAG_RAW, 1.0/kScale_mag,
- SENS_TYPE_MAG_BIAS) },
+ NANOHUB_INT_NONWAKEUP, BMI160_MAG_SAMPLES, SENS_TYPE_MAG_RAW,
+ 1.0/kScale_mag, SENS_TYPE_MAG_BIAS) },
#endif
{ DEC_INFO("Step Detector", SENS_TYPE_STEP_DETECT, NUM_AXIS_EMBEDDED,
NANOHUB_INT_NONWAKEUP, 100) },
@@ -1201,13 +1210,6 @@
int i;
uint8_t val = 0x12;
bool any_fifo_enabled_prev = anyFifoEnabled();
-#ifdef ACCEL_CAL_ENABLED
- struct BMI160Sensor *mSensorAcc;
- bool accelCalNewBiasAvailable;
- struct TripleAxisDataPoint *sample;
- float accelCalBiasX, accelCalBiasY, accelCalBiasZ;
- bool fallThrough;
-#endif
// if ACC is configed, enable ACC bit in fifo_config reg.
if (mTask.sensors[ACC].configed && mTask.sensors[ACC].latency != SENSOR_LATENCY_NODATA) {
@@ -1215,42 +1217,6 @@
mTask.fifo_enabled[ACC] = true;
} else {
mTask.fifo_enabled[ACC] = false;
-#ifdef ACCEL_CAL_ENABLED
- // https://source.android.com/devices/sensors/sensor-types.html
- // "The bias and scale calibration must only be updated while the sensor is deactivated,
- // so as to avoid causing jumps in values during streaming."
- accelCalNewBiasAvailable = accelCalUpdateBias(&mTask.acc, &accelCalBiasX, &accelCalBiasY, &accelCalBiasZ);
-
- mSensorAcc = &mTask.sensors[ACC];
- // notify HAL about new accel bias calibration
- if (accelCalNewBiasAvailable) {
- fallThrough = true;
- if (mSensorAcc->data_evt->samples[0].firstSample.numSamples > 0) {
- // flush existing samples so the bias appears after them
- flushData(mSensorAcc,
- EVENT_TYPE_BIT_DISCARDABLE | sensorGetMyEventType(mSensorInfo[ACC].sensorType));
-
- // try to allocate another data event and break if unsuccessful
- if (!allocateDataEvt(mSensorAcc, sensorGetTime())) {
- fallThrough = false;
- }
- }
-
- if (fallThrough) {
- mSensorAcc->data_evt->samples[0].firstSample.biasCurrent = true;
- mSensorAcc->data_evt->samples[0].firstSample.biasPresent = 1;
- mSensorAcc->data_evt->samples[0].firstSample.biasSample =
- mSensorAcc->data_evt->samples[0].firstSample.numSamples;
- sample = &mSensorAcc->data_evt->samples[mSensorAcc->data_evt->samples[0].firstSample.numSamples++];
- sample->x = accelCalBiasX;
- sample->y = accelCalBiasY;
- sample->z = accelCalBiasZ;
- flushData(mSensorAcc, sensorGetMyEventType(mSensorInfo[ACC].biasType));
-
- allocateDataEvt(mSensorAcc, sensorGetTime());
- }
- }
-#endif
}
// if GYR is configed, enable GYR bit in fifo_config reg.
@@ -2092,7 +2058,12 @@
x, y, z, mTask.tempCelsius);
accelCalBiasRemove(&mTask.acc, &x, &y, &z);
-#endif
+
+#ifdef ACCEL_CAL_DBG_ENABLED
+ // Prints debug data report.
+ accelCalDebPrint(&mTask.acc, mTask.tempCelsius);
+#endif // ACCEL_CAL_DBG_ENABLED
+#endif // ACCEL_CAL_ENABLED
#ifdef GYRO_CAL_ENABLED
// Gyro Cal -- Add accelerometer sample.
@@ -2159,12 +2130,54 @@
return;
}
+#ifdef ACCEL_CAL_ENABLED
+ // https://source.android.com/devices/sensors/sensor-types.html
+ // "The bias and scale calibration must only be updated while the sensor is deactivated,
+ // so as to avoid causing jumps in values during streaming." Note, this is now regulated
+ // by the SensorHAL.
+ if (mSensor->idx == ACC) {
+ float accel_offset[3] = {0.0f, 0.0f, 0.0f};
+ bool accelCalNewBiasAvailable = accelCalUpdateBias(
+ &mTask.acc, &accel_offset[0], &accel_offset[1], &accel_offset[2]);
+ if (accelCalNewBiasAvailable) {
+ if (mSensor->data_evt->samples[0].firstSample.numSamples > 0) {
+ // Flushes existing samples so the bias appears after them.
+ flushData(mSensor,
+ EVENT_TYPE_BIT_DISCARDABLE |
+ sensorGetMyEventType(mSensorInfo[ACC].sensorType));
+
+ // Tries to allocate another data event and breaks if unsuccessful.
+ if (!allocateDataEvt(mSensor, rtc_time)) {
+ return;
+ }
+ }
+ mSensor->data_evt->samples[0].firstSample.biasCurrent = true;
+ mSensor->data_evt->samples[0].firstSample.biasPresent = 1;
+ mSensor->data_evt->samples[0].firstSample.biasSample =
+ mSensor->data_evt->samples[0].firstSample.numSamples;
+ sample = &mSensor->data_evt->
+ samples[mSensor->data_evt->samples[0].firstSample.numSamples++];
+
+ // Updates the accel offset in HAL.
+ sample->x = accel_offset[0];
+ sample->y = accel_offset[1];
+ sample->z = accel_offset[2];
+
+ flushData(mSensor, sensorGetMyEventType(mSensorInfo[ACC].biasType));
+ if (!allocateDataEvt(mSensor, rtc_time)) {
+ return;
+ }
+ }
+ }
+#endif // ACCEL_CAL_ENABLED
+
#ifdef MAG_SLAVE_PRESENT
if (mSensor->idx == MAG && (newMagBias || !mTask.magBiasPosted)) {
if (mSensor->data_evt->samples[0].firstSample.numSamples > 0) {
// flush existing samples so the bias appears after them
flushData(mSensor,
- EVENT_TYPE_BIT_DISCARDABLE | sensorGetMyEventType(mSensorInfo[MAG].sensorType));
+ EVENT_TYPE_BIT_DISCARDABLE |
+ sensorGetMyEventType(mSensorInfo[MAG].sensorType));
if (!allocateDataEvt(mSensor, rtc_time)) {
return;
}
@@ -2176,9 +2189,13 @@
mSensor->data_evt->samples[0].firstSample.biasPresent = 1;
mSensor->data_evt->samples[0].firstSample.biasSample =
mSensor->data_evt->samples[0].firstSample.numSamples;
- sample = &mSensor->data_evt->samples[mSensor->data_evt->samples[0].firstSample.numSamples++];
+ sample = &mSensor->data_evt->
+ samples[mSensor->data_evt->samples[0].firstSample.numSamples++];
+
+ // Updates the mag offset in HAL.
magCalGetBias(&mTask.moc, &sample->x, &sample->y, &sample->z);
- // bias is non-discardable, if we fail to enqueue, don't clear new_mag_bias
+
+ // Bias is non-discardable, if we fail to enqueue, don't clear magBiasPosted.
if (flushData(mSensor, sensorGetMyEventType(mSensorInfo[MAG].biasType))) {
mTask.magBiasPosted = true;
}
@@ -2187,17 +2204,20 @@
return;
}
}
-#endif
+#endif // MAG_SLAVE_PRESENT
+
#ifdef GYRO_CAL_ENABLED
if (mSensor->idx == GYR) {
// GyroCal -- Checks for a new offset estimate update.
float gyro_offset[3] = {0.0f, 0.0f, 0.0f};
float gyro_offset_temperature_celsius = 0.0f;
+ uint64_t calibration_time_nanos = 0;
bool new_gyrocal_offset_update = gyroCalNewBiasAvailable(&mTask.gyro_cal);
if (new_gyrocal_offset_update) {
// GyroCal -- Gets the GyroCal offset estimate.
gyroCalGetBias(&mTask.gyro_cal, &gyro_offset[0], &gyro_offset[1],
- &gyro_offset[2], &gyro_offset_temperature_celsius);
+ &gyro_offset[2], &gyro_offset_temperature_celsius,
+ &calibration_time_nanos);
#ifdef OVERTEMPCAL_ENABLED
// OTC-Gyro Cal -- Sends a new GyroCal estimate to the OTC-Gyro.
@@ -2243,17 +2263,6 @@
sample->y = gyro_offset[1];
sample->z = gyro_offset[2];
-#if defined(GYRO_CAL_DBG_ENABLED) || defined(OVERTEMPCAL_DBG_ENABLED)
- CAL_DEBUG_LOG("[GYRO_OFFSET:STORED]",
- "Offset|Temp|Time: %s%d.%06d, %s%d.%06d, %s%d.%06d | "
- "%s%d.%06d | %llu",
- CAL_ENCODE_FLOAT(sample->x, 6),
- CAL_ENCODE_FLOAT(sample->y, 6),
- CAL_ENCODE_FLOAT(sample->z, 6),
- CAL_ENCODE_FLOAT(gyro_offset_temperature_celsius, 6),
- (unsigned long long int)rtc_time);
-#endif // GYRO_CAL_DBG_ENABLED || OVERTEMPCAL_DBG_ENABLED
-
flushData(mSensor, sensorGetMyEventType(mSensorInfo[GYR].biasType));
if (!allocateDataEvt(mSensor, rtc_time)) {
return;
@@ -2284,7 +2293,7 @@
//DEBUG_PRINT("bmi160: x: %d, y: %d, z: %d\n", (int)(1000*x), (int)(1000*y), (int)(1000*z));
- //TODO: This was added to prevent to much data of the same type accumulate in internal buffer.
+ //TODO: This was added to prevent too much data of the same type accumulate in internal buffer.
// It might no longer be necessary and can be removed.
if (mSensor->data_evt->samples[0].firstSample.numSamples == MAX_NUM_COMMS_EVENT_SAMPLES) {
flushAllData();
@@ -3116,8 +3125,11 @@
bias->hardwareBias[2] & 0xFF);
#ifdef GYRO_CAL_ENABLED
- gyroCalSetBias(&T(gyro_cal), bias->softwareBias[0], bias->softwareBias[1],
- bias->softwareBias[2], sensorGetTime());
+ const float dummy_temperature_celsius = 25.0f;
+ gyroCalSetBias(&T(gyro_cal), bias->softwareBias[0],
+ bias->softwareBias[1], bias->softwareBias[2],
+ dummy_temperature_celsius,
+ sensorGetTime());
#endif // GYRO_CAL_ENABLED
if (!saveCalibration()) {
T(pending_calibration_save) = true;
@@ -3217,9 +3229,8 @@
(int)(d->inclination * 180 / M_PI + 0.5f));
// Passing local field information to mag calibration routine
-#ifdef DIVERSITY_CHECK_ENABLED
diversityCheckerLocalFieldUpdate(&mTask.moc.diversity_checker, d->strength);
-#endif
+
// TODO: pass local field information to rotation vector sensor.
} else {
ERROR_PRINT("magCfgData: unknown type 0x%04x, size %d", p->type, p->size);
@@ -3333,7 +3344,7 @@
}
}
if (mTask.sensors[STEPCNT].flush > 0 || T(pending_step_cnt)) {
- T(pending_step_cnt) = T(pending_step_cnt) && !stepCntFlushGetData();
+ T(pending_step_cnt) = !stepCntFlushGetData() && T(pending_step_cnt);
return;
}
if (mTask.pending_calibration_save) {
@@ -3833,83 +3844,98 @@
osEventSubscribe(mTask.tid, EVT_APP_START);
#ifdef ACCEL_CAL_ENABLED
- // Init Accel Cal
- accelCalInit(&mTask.acc,
- 800000000, /* Stillness Time in ns (0.8s) */
- 5, /* Minimum Sample Number */
- 0.00025, /* Threshold */
- 15, /* nx bucket count */
- 15, /* nxb bucket count */
- 15, /* ny bucket count */
- 15, /* nyb bucket count */
- 15, /* nz bucket count */
- 15, /* nzb bucket count */
- 15); /* nle bucket count */
-#endif
+ // Initializes the accelerometer offset calibration algorithm.
+ const struct AccelCalParameters accel_cal_parameters = {
+ MSEC_TO_NANOS(800), // t0
+ 5, // n_s
+ 15, // fx
+ 15, // fxb
+ 15, // fy
+ 15, // fyb
+ 15, // fz
+ 15, // fzb
+ 15, // fle
+ 0.00025f // th
+ };
+ accelCalInit(&mTask.acc, &accel_cal_parameters);
+#endif // ACCEL_CAL_ENABLED
#ifdef GYRO_CAL_ENABLED
- // Gyro Cal -- Initialization.
- gyroCalInit(&mTask.gyro_cal,
- SEC_TO_NANOS(5.0f), // Min stillness period = 5.0 seconds
- SEC_TO_NANOS(5.9f), // Max stillness period = 6.0 seconds (NOTE 1)
- 0, 0, 0, // Initial bias offset calibration
- 0, // Time stamp of initial bias calibration
- SEC_TO_NANOS(1.5f), // Analysis window length = 1.5 seconds
- 7.5e-5f, // Gyroscope variance threshold [rad/sec]^2
- 1.5e-5f, // Gyroscope confidence delta [rad/sec]^2
- 4.5e-3f, // Accelerometer variance threshold [m/sec^2]^2
- 9.0e-4f, // Accelerometer confidence delta [m/sec^2]^2
- 5.0f, // Magnetometer variance threshold [uT]^2
- 1.0f, // Magnetometer confidence delta [uT]^2
- 0.95f, // Stillness threshold [0,1]
- 40.0f * MDEG_TO_RAD, // Stillness mean variation limit [rad/sec]
- 1.5f, // Max temperature delta during stillness [C]
- true); // Gyro calibration enable
- // NOTE 1: This parameter is set to 5.9 seconds to achieve a max stillness
- // period of 6.0 seconds and avoid buffer boundary conditions that could push
- // the max stillness to the next multiple of the analysis window length
- // (i.e., 7.5 seconds).
+ // Initializes the gyroscope offset calibration algorithm.
+ const struct GyroCalParameters gyro_cal_parameters = {
+ SEC_TO_NANOS(5), // min_still_duration_nanos
+ SEC_TO_NANOS(5.9f), // max_still_duration_nanos [see, NOTE 1]
+ 0, // calibration_time_nanos
+ SEC_TO_NANOS(1.5f), // window_time_duration_nanos
+ 0, // bias_x
+ 0, // bias_y
+ 0, // bias_z
+ 0.95f, // stillness_threshold
+ MDEG_TO_RAD * 40.0f, // stillness_mean_delta_limit [rad/sec]
+ 7.5e-5f, // gyro_var_threshold [rad/sec]^2
+ 1.5e-5f, // gyro_confidence_delta [rad/sec]^2
+ 4.5e-3f, // accel_var_threshold [m/sec^2]^2
+ 9.0e-4f, // accel_confidence_delta [m/sec^2]^2
+ 5.0f, // mag_var_threshold [uTesla]^2
+ 1.0f, // mag_confidence_delta [uTesla]^2
+ 1.5f, // temperature_delta_limit_celsius
+ true // gyro_calibration_enable
+ };
+ // [NOTE 1]: 'max_still_duration_nanos' is set to 5.9 seconds to achieve a
+ // max stillness period of 6.0 seconds and avoid buffer boundary conditions
+ // that could push the max stillness to the next multiple of the analysis
+ // window length (i.e., 7.5 seconds).
+ gyroCalInit(&mTask.gyro_cal, &gyro_cal_parameters);
#ifdef OVERTEMPCAL_ENABLED
- // Initialize over-temp calibration.
- overTempCalInit(&mTask.over_temp_gyro_cal,
- 5, // Min num of points to enable model update
- SEC_TO_NANOS(0.5f), // Min temperature update interval [nsec]
- 0.75f, // Temperature span of bin method [C]
- 40.0f * MDEG_TO_RAD, // Jump tolerance [rad/sec]
- 50.0f * MDEG_TO_RAD, // Outlier rejection tolerance [rad/sec]
- DAYS_TO_NANOS(2), // Model data point age limit [nsec]
- 80.0f * MDEG_TO_RAD, // Limit for temp. sensitivity [rad/sec/C]
- 3.0e3f * MDEG_TO_RAD, // Limit for model intercept parameter [rad/sec]
- 0.1f * MDEG_TO_RAD, // Significant offset change [rad/sec]
- true); // Over-temp compensation enable
+ // Initializes the gyroscope over-temperature offset compensation algorithm.
+ const struct OverTempCalParameters gyro_otc_parameters = {
+ MSEC_TO_NANOS(500), // min_temp_update_period_nanos
+ DAYS_TO_NANOS(2), // age_limit_nanos
+ 0.75f, // delta_temp_per_bin
+ 40.0f * MDEG_TO_RAD, // jump_tolerance
+ 50.0f * MDEG_TO_RAD, // outlier_limit
+ 80.0f * MDEG_TO_RAD, // temp_sensitivity_limit
+ 3.0e3f * MDEG_TO_RAD, // sensor_intercept_limit
+ 0.1f * MDEG_TO_RAD, // significant_offset_change
+ 5, // min_num_model_pts
+ true // over_temp_enable
+ };
+ overTempCalInit(&mTask.over_temp_gyro_cal, &gyro_otc_parameters);
+
#endif // OVERTEMPCAL_ENABLED
#endif // GYRO_CAL_ENABLED
#ifdef MAG_SLAVE_PRESENT
-#ifdef DIVERSITY_CHECK_ENABLED
- initMagCal(&mTask.moc,
- 0.0f, 0.0f, 0.0f, // bias x, y, z
- 1.0f, 0.0f, 0.0f, // c00, c01, c02
- 0.0f, 1.0f, 0.0f, // c10, c11, c12
- 0.0f, 0.0f, 1.0f, // c20, c21, c22
- 3000000, // min_batch_window_in_micros
- 8, // min_num_diverse_vectors
- 1, // max_num_max_distance
- 6.0f, // var_threshold
- 10.0f, // max_min_threshold
- 48.f, // local_field
- 0.5f, // threshold_tuning_param
- 2.552f); // max_distance_tuning_param
-#else
- initMagCal(&mTask.moc,
- 0.0f, 0.0f, 0.0f, // bias x, y, z
- 1.0f, 0.0f, 0.0f, // c00, c01, c02
- 0.0f, 1.0f, 0.0f, // c10, c11, c12
- 0.0f, 0.0f, 1.0f, // c20, c21, c22
- 3000000); // min_batch_window_in_micros
-#endif
-#endif
+ const struct MagCalParameters mag_cal_parameters = {
+ 3000000, // min_batch_window_in_micros
+ 0.0f, // x_bias
+ 0.0f, // y_bias
+ 0.0f, // z_bias
+ 1.0f, // c00
+ 0.0f, // c01
+ 0.0f, // c02
+ 0.0f, // c10
+ 1.0f, // c11
+ 0.0f, // c12
+ 0.0f, // c20
+ 0.0f, // c21
+ 1.0f // c22
+ };
+
+ // Initializes the magnetometer offset calibration algorithm with diversity
+ // checker.
+ const struct DiversityCheckerParameters mag_diversity_parameters = {
+ 6.0f, // var_threshold
+ 10.0f, // max_min_threshold
+ 48.0f, // local_field
+ 0.5f, // threshold_tuning_param
+ 2.552f, // max_distance_tuning_param
+ 8, // min_num_diverse_vectors
+ 1 // max_num_max_distance
+ };
+ initMagCal(&mTask.moc, &mag_cal_parameters, &mag_diversity_parameters);
+#endif // MAG_SLAVE_PRESENT
slabSize = sizeof(struct TripleAxisDataEvent) +
MAX_NUM_COMMS_EVENT_SAMPLES * sizeof(struct TripleAxisDataPoint);
diff --git a/firmware/os/drivers/bosch_bmp280/bosch_bmp280.c b/firmware/os/drivers/bosch_bmp280/bosch_bmp280.c
index 39b5206..4950973 100644
--- a/firmware/os/drivers/bosch_bmp280/bosch_bmp280.c
+++ b/firmware/os/drivers/bosch_bmp280/bosch_bmp280.c
@@ -32,7 +32,7 @@
#define BMP280_APP_ID APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 5)
-#define BMP280_APP_VERSION 3
+#define BMP280_APP_VERSION 4
#ifndef BMP280_I2C_BUS_ID
#define BMP280_I2C_BUS_ID 0
@@ -44,13 +44,15 @@
#define BOSCH_BMP280_ID 0x58
-#define BOSCH_BMP280_REG_RESET 0x60
+#define BOSCH_BMP280_REG_RESET 0xE0
#define BOSCH_BMP280_REG_DIG_T1 0x88
#define BOSCH_BMP280_REG_ID 0xd0
#define BOSCH_BMP280_REG_CTRL_MEAS 0xf4
#define BOSCH_BMP280_REG_CONFIG 0xf5
#define BOSCH_BMP280_REG_PRES_MSB 0xf7
+#define BOSCH_BMP280_SOFT_RESET_CMD 0xB6
+
#define BOSCH_BMP280_MAX_PENDING_I2C_REQUESTS 4
#define BOSCH_BMP280_MAX_I2C_TRANSFER_SIZE 6
@@ -62,22 +64,28 @@
#define CTRL_ON ((2 << 5) | (5 << 2) | 3)
// temp: 2x oversampling, baro: 16x oversampling, power: sleep
#define CTRL_SLEEP ((2 << 5) | (5 << 2))
+// config: standby time: 62.5ms, IIR filter coefficient: 4
+#define CTRL_CFG ((1 << 5) | (2 << 2))
enum BMP280SensorEvents
{
EVT_SENSOR_I2C = EVT_APP_START + 1,
EVT_SENSOR_BARO_TIMER,
EVT_SENSOR_TEMP_TIMER,
+ EVT_SENSOR_SOFTRESET_TIMER,
};
enum BMP280TaskState
{
STATE_RESET,
+ STATE_SOFTRESET,
+ STATE_SOFTRESET_MODE,
STATE_VERIFY_ID,
STATE_AWAITING_COMP_PARAMS,
STATE_CONFIG,
STATE_FINISH_INIT,
STATE_IDLE,
+ STATE_ENABLING_BARO_TEMP,
STATE_ENABLING_BARO,
STATE_ENABLING_TEMP,
STATE_DISABLING_BARO,
@@ -114,11 +122,14 @@
uint32_t tempHandle;
uint32_t baroTimerHandle;
uint32_t tempTimerHandle;
+ uint32_t resetHandle;
float offset;
struct I2cTransfer transfers[BOSCH_BMP280_MAX_PENDING_I2C_REQUESTS];
+ bool tmpbaroOn;
+ bool tmptempOn;
bool baroOn;
bool tempOn;
bool baroReading;
@@ -262,6 +273,16 @@
osEnqueuePrivateEvt(EVT_SENSOR_TEMP_TIMER, cookie, NULL, mTask.id);
}
+static void softresetCallback(uint32_t timerId, void *cookie)
+{
+ osEnqueuePrivateEvt(EVT_SENSOR_SOFTRESET_TIMER, cookie, NULL, mTask.id);
+}
+
+static void softreset()
+{
+ writeRegister(BOSCH_BMP280_REG_RESET, BOSCH_BMP280_SOFT_RESET_CMD, STATE_SOFTRESET);
+}
+
static void setMode(bool on, uint8_t state)
{
writeRegister(BOSCH_BMP280_REG_CTRL_MEAS, (on) ? CTRL_ON : CTRL_SLEEP, state);
@@ -298,12 +319,37 @@
mTask.baroReading = false;
}
- if (oldMode != newMode)
- setMode(newMode, (on ? STATE_ENABLING_BARO : STATE_DISABLING_BARO));
- else
- sensorSignalInternalEvt(mTask.baroHandle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, on, 0);
+ if (!on && mTask.tmpbaroOn && mTask.resetHandle)
+ {
+ if (!mTask.tmptempOn) {
+ timTimerCancel(mTask.resetHandle);
+ mTask.resetHandle = 0;
+ }
+ mTask.tmpbaroOn = 0;
+ sensorSignalInternalEvt(mTask.baroHandle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, true, 0);
+ }
- mTask.baroOn = on;
+ if (oldMode != newMode)
+ {
+ if (newMode == 0)
+ {
+ setMode(newMode, STATE_DISABLING_BARO);
+ mTask.baroOn = false;
+ }
+ else
+ {
+ mTask.tmpbaroOn = true;
+ if (!mTask.tmptempOn) {
+ // do soft reset first when newMode is on
+ softreset();
+ }
+ }
+ }
+ else
+ {
+ sensorSignalInternalEvt(mTask.baroHandle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, on, 0);
+ mTask.baroOn = on;
+ }
return true;
}
@@ -362,12 +408,38 @@
mTask.tempReading = false;
}
- if (oldMode != newMode)
- setMode(newMode, (on ? STATE_ENABLING_TEMP : STATE_DISABLING_TEMP));
- else
- sensorSignalInternalEvt(mTask.tempHandle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, on, 0);
+ if (!on && mTask.tmptempOn && mTask.resetHandle)
+ {
+ if(!mTask.tmpbaroOn) {
+ timTimerCancel(mTask.resetHandle);
+ mTask.resetHandle = 0;
+ }
+ mTask.tmptempOn = 0;
+ sensorSignalInternalEvt(mTask.tempHandle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, true, 0);
+ }
- mTask.tempOn = on;
+ if (oldMode != newMode)
+ {
+ if (newMode == 0)
+ {
+ setMode(newMode, STATE_DISABLING_TEMP);
+ mTask.tempOn = false;
+ }
+ else
+ {
+ mTask.tmptempOn = true;
+ if (!mTask.tmpbaroOn) {
+ // do soft reset first when newMode is on
+ softreset();
+ }
+ }
+ }
+ else
+ {
+ sensorSignalInternalEvt(mTask.tempHandle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, on, 0);
+ mTask.tempOn = on;
+ }
+
return true;
}
@@ -495,6 +567,31 @@
break;
}
+ case STATE_SOFTRESET: {
+ //create timer for 2ms delay
+ mTask.resetHandle = timTimerSet(2000000ull, 0, 50, softresetCallback, NULL, true);
+ break;
+ }
+
+ case STATE_SOFTRESET_MODE: {
+ if (mTask.tmpbaroOn && mTask.tmptempOn) {
+ setMode(true,STATE_ENABLING_BARO_TEMP);
+ mTask.tmpbaroOn = false;
+ mTask.baroOn = true;
+ mTask.tmptempOn = false;
+ mTask.tempOn = true;
+ } else if (mTask.tmpbaroOn) {
+ setMode(true,STATE_ENABLING_BARO);
+ mTask.tmpbaroOn = false;
+ mTask.baroOn = true;
+ } else if (mTask.tmptempOn) {
+ setMode(true,STATE_ENABLING_TEMP);
+ mTask.tmptempOn = false;
+ mTask.tempOn = true;
+ }
+ break;
+ }
+
case STATE_VERIFY_ID: {
/* Check the sensor ID */
if (xfer->err != 0 || xfer->txrxBuf[0] != BOSCH_BMP280_ID) {
@@ -521,7 +618,13 @@
case STATE_CONFIG: {
// standby time: 62.5ms, IIR filter coefficient: 4
- writeRegister(BOSCH_BMP280_REG_CONFIG, (1 << 5) | (2 << 2), STATE_FINISH_INIT);
+ writeRegister(BOSCH_BMP280_REG_CONFIG, CTRL_CFG, STATE_FINISH_INIT);
+ break;
+ }
+
+ case STATE_ENABLING_BARO_TEMP: {
+ sensorSignalInternalEvt(mTask.baroHandle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, true, 0);
+ sensorSignalInternalEvt(mTask.tempHandle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, true, 0);
break;
}
@@ -606,7 +709,7 @@
i2cMasterRequest(I2C_BUS_ID, I2C_SPEED);
/* Reset chip */
- writeRegister(BOSCH_BMP280_REG_RESET, 0xB6, STATE_RESET);
+ writeRegister(BOSCH_BMP280_REG_RESET, BOSCH_BMP280_SOFT_RESET_CMD, STATE_RESET);
break;
}
@@ -649,6 +752,12 @@
mTask.tempReading = true;
break;
}
+
+ case EVT_SENSOR_SOFTRESET_TIMER:
+ {
+ writeRegister(BOSCH_BMP280_REG_CONFIG, CTRL_CFG, STATE_SOFTRESET_MODE);
+ break;
+ }
}
}
diff --git a/firmware/os/inc/chreApi.h b/firmware/os/inc/chreApi.h
index e355ed5..2c15701 100644
--- a/firmware/os/inc/chreApi.h
+++ b/firmware/os/inc/chreApi.h
@@ -74,13 +74,16 @@
#define SYSCALL_CHRE_MAIN_EVENT_INFO_BY_APP_ID 2 // (uint64_t, struct chreNanoappInfo *) -> bool
#define SYSCALL_CHRE_MAIN_EVENT_INFO_BY_INST_ID 3 // (uint32_t, struct chreNanoappInfo *) -> bool
#define SYSCALL_CHRE_MAIN_EVENT_CFG_INFO 4 // (bool) -> void
-#define SYSCALL_CHRE_MAIN_EVENT_LAST 5 // always last. holes are allowed, but not immediately before this
+#define SYSCALL_CHRE_MAIN_EVENT_HOST_SLEEP 5 // (bool) -> void
+#define SYSCALL_CHRE_MAIN_EVENT_IS_HOST_AWAKE 6 // (void) -> bool
+#define SYSCALL_CHRE_MAIN_EVENT_LAST 7 // always last. holes are allowed, but not immediately before this
//level 2 indices in the CHRE.drivers table
#define SYSCALL_CHRE_DRV_GNSS 0
#define SYSCALL_CHRE_DRV_WIFI 1
#define SYSCALL_CHRE_DRV_WWAN 2
-#define SYSCALL_CHRE_DRV_LAST 3 // always last. holes are allowed, but not immediately before this
+#define SYSCALL_CHRE_DRV_AUDIO 3
+#define SYSCALL_CHRE_DRV_LAST 4 // always last. holes are allowed, but not immediately before this
//level 3 indices in the CHRE.drivers.gnss table
#define SYSCALL_CHRE_DRV_GNSS_GET_CAP 0 // (void) -> uint32_t
@@ -88,7 +91,8 @@
#define SYSCALL_CHRE_DRV_GNSS_LOC_STOP_ASYNC 2 // (const void *) -> bool
#define SYSCALL_CHRE_DRV_GNSS_MEAS_START_ASYNC 3 // (uint32_t, const void *) -> bool
#define SYSCALL_CHRE_DRV_GNSS_MEAS_STOP_ASYNC 4 // (const void *) -> bool
-#define SYSCALL_CHRE_DRV_GNSS_LAST 5 // always last. holes are allowed, but not immediately before this
+#define SYSCALL_CHRE_DRV_GNSS_CONF_PASV_LOC_LIS 5 // (bool) -> bool
+#define SYSCALL_CHRE_DRV_GNSS_LAST 6 // always last. holes are allowed, but not immediately before this
//level 3 indices in the CHRE.drivers.wifi table
#define SYSCALL_CHRE_DRV_WIFI_GET_CAP 0 // (void) -> uint32_t
@@ -98,9 +102,15 @@
//level 3 indices in the CHRE.drivers.wwan table
#define SYSCALL_CHRE_DRV_WWAN_GET_CAP 0 // (void) -> uint32_t
-#define SYSCALL_CHRE_DRV_WWAN_GET_CELL_INFO_ASYNC 1 // (const void *cookie) -> bool
+#define SYSCALL_CHRE_DRV_WWAN_GET_CELL_INFO_ASYNC 1 // (const void *) -> bool
#define SYSCALL_CHRE_DRV_WWAN_LAST 2 // always last. holes are allowed, but not immediately before this
+//level 3 indicies in the CHRE.drivers.audio table
+#define SYSCALL_CHRE_DRV_AUDIO_GET_SRC 0 // (uint32_t, struct chreAudioSource *) -> bool
+#define SYSCALL_CHRE_DRV_AUDIO_CONF_SRC 1 // (uint32_t, bool, uint64_t, uint64_t) -> bool
+#define SYSCALL_CHRE_DRV_AUDIO_GET_STATUS 2 // (uint32_t, struct chreAudioSourceStatus *) -> bool
+#define SYSCALL_CHRE_DRV_AUDIO_LAST 3 // always last. holes are allowed, but not immediately before this
+
//called by os entry point to export the api
void osChreApiExport(void);
// release CHRE event and optionally call completion callback
diff --git a/firmware/os/inc/eeData.h b/firmware/os/inc/eeData.h
index 9b313ee..0a6eca6 100644
--- a/firmware/os/inc/eeData.h
+++ b/firmware/os/inc/eeData.h
@@ -45,6 +45,9 @@
bool eeDataGet(uint32_t name, void *buf, uint32_t *szP);
bool eeDataSet(uint32_t name, const void *buf, uint32_t len);
+uint32_t eeDataGetSize();
+uint32_t eeDataGetFree();
+
//allow getting old "versions". Set state to NULL initially, call till you get NULL as return value
void *eeDataGetAllVersions(uint32_t name, void *buf, uint32_t *szP, void **stateP);
bool eeDataEraseOldVersion(uint32_t name, void *addr); // addr is non-NULL address returned by call to eeDataGetAllVersions
diff --git a/firmware/os/inc/eventnums.h b/firmware/os/inc/eventnums.h
index 5746433..cda24f3 100644
--- a/firmware/os/inc/eventnums.h
+++ b/firmware/os/inc/eventnums.h
@@ -32,6 +32,7 @@
#define EVT_APP_TO_SENSOR_HAL_DATA 0x00000404 //sensor driver out of band data update to sensor hal
#define EVT_APP_STARTED 0x00000405 //sent when a app has successfully started
#define EVT_APP_STOPPED 0x00000406 //sent when a app has stopped
+#define EVT_APP_TO_HOST_CHRE 0x00000407 //app data to host. Type is struct HostHubChrePacket
#define EVT_DEBUG_LOG 0x00007F01 //send message payload to Linux kernel log
#define EVT_MASK 0x0000FFFF
@@ -55,6 +56,18 @@
}ATTRIBUTE_PACKED;
SET_PACKED_STRUCT_MODE_OFF
+#define HOST_HUB_CHRE_PACKET_MAX_LEN 128
+
+SET_PACKED_STRUCT_MODE_ON
+struct HostHubChrePacket {
+ uint64_t appId;
+ uint8_t messageSize; //not incl this header, 128 bytes max
+ uint32_t messageType;
+ uint16_t hostEndpoint;
+ //raw data in unspecified format here
+}ATTRIBUTE_PACKED;
+SET_PACKED_STRUCT_MODE_OFF
+
SET_PACKED_STRUCT_MODE_ON
struct NanohubMsgChreHdrV10 {
uint8_t size;
diff --git a/firmware/os/inc/heap.h b/firmware/os/inc/heap.h
index c5bea59..cb1ccb2 100644
--- a/firmware/os/inc/heap.h
+++ b/firmware/os/inc/heap.h
@@ -24,19 +24,16 @@
#include <stdint.h>
#include <stdbool.h>
-
-
-
bool heapInit(void);
void* heapAlloc(uint32_t sz);
void heapFree(void* ptr);
int heapFreeAll(uint32_t tid);
-
+int heapGetFreeSize(int *numChunks, int *largestChunk);
+int heapGetTaskSize(uint32_t tid);
#ifdef __cplusplus
}
#endif
-
#endif
diff --git a/firmware/os/inc/nanohubCommand.h b/firmware/os/inc/nanohubCommand.h
index 408cffd..e100980 100644
--- a/firmware/os/inc/nanohubCommand.h
+++ b/firmware/os/inc/nanohubCommand.h
@@ -34,11 +34,20 @@
void nanohubPrefetchTx(uint32_t interrupt, uint32_t wakeup, uint32_t nonwakeup);
const struct NanohubCommand *nanohubFindCommand(uint32_t packetReason);
-struct NanohubHalCommand {
+struct NanohubHalLegacyCommand {
uint8_t msg;
void (*handler)(void *, uint8_t);
};
+const struct NanohubHalLegacyCommand *nanohubHalLegacyFindCommand(uint8_t msg);
+
+struct NanohubHalCommand {
+ uint8_t msg;
+ void (*handler)(void *, uint8_t, uint32_t);
+ uint8_t minDataLen;
+ uint8_t maxDataLen;
+};
+
const struct NanohubHalCommand *nanohubHalFindCommand(uint8_t msg);
uint64_t hostGetTime(void);
int64_t hostGetTimeDelta(void);
diff --git a/firmware/os/inc/nanohubPacket.h b/firmware/os/inc/nanohubPacket.h
index ad962ea..df4af66 100644
--- a/firmware/os/inc/nanohubPacket.h
+++ b/firmware/os/inc/nanohubPacket.h
@@ -166,6 +166,7 @@
NANOHUB_FIRMWARE_CHUNK_REPLY_RESTART,
NANOHUB_FIRMWARE_CHUNK_REPLY_CANCEL,
NANOHUB_FIRMWARE_CHUNK_REPLY_CANCEL_NO_RETRY,
+ NANOHUB_FIRMWARE_CHUNK_REPLY_NO_SPACE,
};
SET_PACKED_STRUCT_MODE_ON
@@ -278,18 +279,6 @@
} ATTRIBUTE_PACKED;
SET_PACKED_STRUCT_MODE_OFF
-SET_PACKED_STRUCT_MODE_ON
-struct NanohubHalHdr {
- uint64_t appId;
- uint8_t len;
- uint8_t msg;
-} ATTRIBUTE_PACKED;
-SET_PACKED_STRUCT_MODE_OFF
-
-#define NANOHUB_HAL_EXT_APPS_ON 0
-#define NANOHUB_HAL_EXT_APPS_OFF 1
-#define NANOHUB_HAL_EXT_APP_DELETE 2
-
// this behaves more stable w.r.t. endianness than bit field
// this is setting byte fields in MgmtStatus response
// the high-order bit, if set, is indication of counter overflow
@@ -310,32 +299,46 @@
} ATTRIBUTE_PACKED;
SET_PACKED_STRUCT_MODE_OFF
+#ifdef LEGACY_HAL_ENABLED
+
SET_PACKED_STRUCT_MODE_ON
-struct NanohubHalMgmtRx {
+struct NanohubHalLegacyHdr {
+ uint64_t appId;
+ uint8_t len;
+ uint8_t msg;
+} ATTRIBUTE_PACKED;
+SET_PACKED_STRUCT_MODE_OFF
+
+#define NANOHUB_HAL_LEGACY_EXT_APPS_ON 0
+#define NANOHUB_HAL_LEGACY_EXT_APPS_OFF 1
+#define NANOHUB_HAL_LEGACY_EXT_APP_DELETE 2
+
+SET_PACKED_STRUCT_MODE_ON
+struct NanohubHalLegacyMgmtRx {
__le64 appId;
struct MgmtStatus stat;
} ATTRIBUTE_PACKED;
SET_PACKED_STRUCT_MODE_OFF
SET_PACKED_STRUCT_MODE_ON
-struct NanohubHalMgmtTx {
- struct NanohubHalHdr hdr;
+struct NanohubHalLegacyMgmtTx {
+ struct NanohubHalLegacyHdr hdr;
__le32 status;
} ATTRIBUTE_PACKED;
SET_PACKED_STRUCT_MODE_OFF
-#define NANOHUB_HAL_QUERY_MEMINFO 3
-#define NANOHUB_HAL_QUERY_APPS 4
+#define NANOHUB_HAL_LEGACY_QUERY_MEMINFO 3
+#define NANOHUB_HAL_LEGACY_QUERY_APPS 4
SET_PACKED_STRUCT_MODE_ON
-struct NanohubHalQueryAppsRx {
+struct NanohubHalLegacyQueryAppsRx {
__le32 idx;
} ATTRIBUTE_PACKED;
SET_PACKED_STRUCT_MODE_OFF
SET_PACKED_STRUCT_MODE_ON
-struct NanohubHalQueryAppsTx {
- struct NanohubHalHdr hdr;
+struct NanohubHalLegacyQueryAppsTx {
+ struct NanohubHalLegacyHdr hdr;
__le64 appId;
__le32 version;
__le32 flashUse;
@@ -343,22 +346,205 @@
} ATTRIBUTE_PACKED;
SET_PACKED_STRUCT_MODE_OFF
-#define NANOHUB_HAL_QUERY_RSA_KEYS 5
+#define NANOHUB_HAL_LEGACY_QUERY_RSA_KEYS 5
SET_PACKED_STRUCT_MODE_ON
-struct NanohubHalQueryRsaKeysRx {
+struct NanohubHalLegacyQueryRsaKeysRx {
__le32 offset;
} ATTRIBUTE_PACKED;
SET_PACKED_STRUCT_MODE_OFF
SET_PACKED_STRUCT_MODE_ON
-struct NanohubHalQueryRsaKeysTx {
- struct NanohubHalHdr hdr;
+struct NanohubHalLegacyQueryRsaKeysTx {
+ struct NanohubHalLegacyHdr hdr;
uint8_t data[];
} ATTRIBUTE_PACKED;
SET_PACKED_STRUCT_MODE_OFF
-#define NANOHUB_HAL_START_UPLOAD 6
+#define NANOHUB_HAL_LEGACY_START_UPLOAD 6
+
+SET_PACKED_STRUCT_MODE_ON
+struct NanohubHalLegacyStartUploadRx {
+ uint8_t isOs;
+ __le32 length;
+} ATTRIBUTE_PACKED;
+SET_PACKED_STRUCT_MODE_OFF
+
+SET_PACKED_STRUCT_MODE_ON
+struct NanohubHalLegacyStartUploadTx {
+ struct NanohubHalLegacyHdr hdr;
+ uint8_t success;
+} ATTRIBUTE_PACKED;
+SET_PACKED_STRUCT_MODE_OFF
+
+#define NANOHUB_HAL_LEGACY_CONT_UPLOAD 7
+
+SET_PACKED_STRUCT_MODE_ON
+struct NanohubHalLegacyContUploadRx {
+ __le32 offset;
+ uint8_t data[];
+} ATTRIBUTE_PACKED;
+SET_PACKED_STRUCT_MODE_OFF
+
+SET_PACKED_STRUCT_MODE_ON
+struct NanohubHalLegacyContUploadTx {
+ struct NanohubHalLegacyHdr hdr;
+ uint8_t success;
+} ATTRIBUTE_PACKED;
+SET_PACKED_STRUCT_MODE_OFF
+
+#define NANOHUB_HAL_LEGACY_FINISH_UPLOAD 8
+
+SET_PACKED_STRUCT_MODE_ON
+struct NanohubHalLegacyFinishUploadTx {
+ struct NanohubHalLegacyHdr hdr;
+ uint8_t success;
+} ATTRIBUTE_PACKED;
+SET_PACKED_STRUCT_MODE_OFF
+
+#define NANOHUB_HAL_LEGACY_REBOOT 9
+
+SET_PACKED_STRUCT_MODE_ON
+struct NanohubHalLegacyRebootTx {
+ struct NanohubHalLegacyHdr hdr;
+ __le32 reason;
+} ATTRIBUTE_PACKED;
+SET_PACKED_STRUCT_MODE_OFF
+
+#endif /* LEGACY_HAL_ENABLED */
+
+SET_PACKED_STRUCT_MODE_ON
+struct NanohubHalHdr {
+ __le64 appId;
+ uint8_t len;
+ __le32 transactionId;
+ __le16 unused;
+} ATTRIBUTE_PACKED;
+SET_PACKED_STRUCT_MODE_OFF
+
+SET_PACKED_STRUCT_MODE_ON
+struct NanohubHalRet {
+ uint8_t msg;
+ __le32 status;
+} ATTRIBUTE_PACKED;
+
+#define NANOHUB_HAL_APP_MGMT 0x10
+
+SET_PACKED_STRUCT_MODE_ON
+struct NanohubHalAppMgmtRx {
+ __le64 appId;
+ uint8_t cmd;
+} ATTRIBUTE_PACKED;
+SET_PACKED_STRUCT_MODE_OFF
+
+#define NANOHUB_HAL_APP_MGMT_START 0
+#define NANOHUB_HAL_APP_MGMT_STOP 1
+#define NANOHUB_HAL_APP_MGMT_UNLOAD 2
+#define NANOHUB_HAL_APP_MGMT_DELETE 3
+
+SET_PACKED_STRUCT_MODE_ON
+struct NanohubHalAppMgmtTx {
+ struct NanohubHalHdr hdr;
+ struct NanohubHalRet ret;
+ uint8_t cmd;
+ struct MgmtStatus stat;
+} ATTRIBUTE_PACKED;
+SET_PACKED_STRUCT_MODE_OFF
+
+#define NANOHUB_HAL_SYS_MGMT 0x11
+
+SET_PACKED_STRUCT_MODE_ON
+struct NanohubHalSysMgmtRx {
+ uint8_t cmd;
+} ATTRIBUTE_PACKED;
+SET_PACKED_STRUCT_MODE_OFF
+
+#define NANOHUB_HAL_SYS_MGMT_ERASE 0
+#define NANOHUB_HAL_SYS_MGMT_REBOOT 1
+
+SET_PACKED_STRUCT_MODE_ON
+struct NanohubHalSysMgmtTx {
+ struct NanohubHalHdr hdr;
+ struct NanohubHalRet ret;
+ uint8_t cmd;
+} ATTRIBUTE_PACKED;
+SET_PACKED_STRUCT_MODE_OFF
+
+#define NANOHUB_HAL_APP_INFO 0x12
+
+SET_PACKED_STRUCT_MODE_ON
+struct NanohubHalAppInfoRx {
+ __le32 addr;
+ uint8_t tags[HOST_HUB_CHRE_PACKET_MAX_LEN - sizeof(__le32)];
+} ATTRIBUTE_PACKED;
+SET_PACKED_STRUCT_MODE_OFF
+
+#define NANOHUB_HAL_APP_INFO_APPID 0x00
+#define NANOHUB_HAL_APP_INFO_CRC 0x01
+#define NANOHUB_HAL_APP_INFO_TID 0x02
+#define NANOHUB_HAL_APP_INFO_VERSION 0x03
+#define NANOHUB_HAL_APP_INFO_ADDR 0x04
+#define NANOHUB_HAL_APP_INFO_SIZE 0x05
+#define NANOHUB_HAL_APP_INFO_HEAP 0x06
+#define NANOHUB_HAL_APP_INFO_DATA 0x07
+#define NANOHUB_HAL_APP_INFO_BSS 0x08
+#define NANOHUB_HAL_APP_INFO_CHRE_MAJOR 0x09
+#define NANOHUB_HAL_APP_INFO_CHRE_MINOR 0x0A
+#define NANOHUB_HAL_APP_INFO_END 0xFF
+
+SET_PACKED_STRUCT_MODE_ON
+struct NanohubHalAppInfoTx {
+ struct NanohubHalHdr hdr;
+ struct NanohubHalRet ret;
+ uint8_t data[HOST_HUB_CHRE_PACKET_MAX_LEN - sizeof(struct NanohubHalRet)];
+} ATTRIBUTE_PACKED;
+SET_PACKED_STRUCT_MODE_OFF
+
+#define NANOHUB_HAL_SYS_INFO 0x13
+
+SET_PACKED_STRUCT_MODE_ON
+struct NanohubHalSysInfoRx {
+ uint8_t tags[HOST_HUB_CHRE_PACKET_MAX_LEN];
+} ATTRIBUTE_PACKED;
+SET_PACKED_STRUCT_MODE_OFF
+
+#define NANOHUB_HAL_SYS_INFO_HEAP_FREE 0x0F
+#define NANOHUB_HAL_SYS_INFO_RAM_SIZE 0x12
+#define NANOHUB_HAL_SYS_INFO_EEDATA_SIZE 0x13
+#define NANOHUB_HAL_SYS_INFO_EEDATA_FREE 0x14
+#define NANOHUB_HAL_SYS_INFO_CODE_SIZE 0x15
+#define NANOHUB_HAL_SYS_INFO_CODE_FREE 0x16
+#define NANOHUB_HAL_SYS_INFO_SHARED_SIZE 0x17
+#define NANOHUB_HAL_SYS_INFO_SHARED_FREE 0x18
+#define NANOHUB_HAL_SYS_INFO_END 0xFF
+
+SET_PACKED_STRUCT_MODE_ON
+struct NanohubHalSysInfoTx {
+ struct NanohubHalHdr hdr;
+ struct NanohubHalRet ret;
+ uint8_t data[HOST_HUB_CHRE_PACKET_MAX_LEN - sizeof(struct NanohubHalRet)];
+} ATTRIBUTE_PACKED;
+SET_PACKED_STRUCT_MODE_OFF
+
+#define NANOHUB_HAL_KEY_INFO 0x14
+
+SET_PACKED_STRUCT_MODE_ON
+struct NanohubHalKeyInfoRx {
+ uint32_t keyNum;
+ uint32_t dataOffset;
+} ATTRIBUTE_PACKED;
+SET_PACKED_STRUCT_MODE_OFF
+
+SET_PACKED_STRUCT_MODE_ON
+struct NanohubHalKeyInfoTx {
+ struct NanohubHalHdr hdr;
+ struct NanohubHalRet ret;
+ uint32_t keyLength;
+ uint8_t data[NANOHUB_RSA_KEY_CHUNK_LEN];
+} ATTRIBUTE_PACKED;
+SET_PACKED_STRUCT_MODE_OFF
+
+#define NANOHUB_HAL_START_UPLOAD 0x16
SET_PACKED_STRUCT_MODE_ON
struct NanohubHalStartUploadRx {
@@ -370,41 +556,34 @@
SET_PACKED_STRUCT_MODE_ON
struct NanohubHalStartUploadTx {
struct NanohubHalHdr hdr;
- uint8_t success;
+ struct NanohubHalRet ret;
} ATTRIBUTE_PACKED;
SET_PACKED_STRUCT_MODE_OFF
-#define NANOHUB_HAL_CONT_UPLOAD 7
+#define NANOHUB_HAL_CONT_UPLOAD 0x17
SET_PACKED_STRUCT_MODE_ON
struct NanohubHalContUploadRx {
__le32 offset;
- uint8_t data[];
+ uint8_t data[HOST_HUB_CHRE_PACKET_MAX_LEN-sizeof(__le32)];
} ATTRIBUTE_PACKED;
-SET_PACKED_STRUCT_MODE_OFF
+SET_PACKED_STRUCT_MODE_ON
SET_PACKED_STRUCT_MODE_ON
struct NanohubHalContUploadTx {
struct NanohubHalHdr hdr;
- uint8_t success;
+ struct NanohubHalRet ret;
} ATTRIBUTE_PACKED;
SET_PACKED_STRUCT_MODE_OFF
-#define NANOHUB_HAL_FINISH_UPLOAD 8
+#define NANOHUB_HAL_FINISH_UPLOAD 0x18
SET_PACKED_STRUCT_MODE_ON
struct NanohubHalFinishUploadTx {
struct NanohubHalHdr hdr;
- uint8_t success;
-} ATTRIBUTE_PACKED;
-SET_PACKED_STRUCT_MODE_OFF
-
-#define NANOHUB_HAL_REBOOT 9
-
-SET_PACKED_STRUCT_MODE_ON
-struct NanohubHalRebootTx {
- struct NanohubHalHdr hdr;
- __le32 reason;
+ struct NanohubHalRet ret;
+ __le32 addr;
+ __le32 crc;
} ATTRIBUTE_PACKED;
SET_PACKED_STRUCT_MODE_OFF
diff --git a/firmware/os/inc/seos.h b/firmware/os/inc/seos.h
index c66d897..c8a3d48 100644
--- a/firmware/os/inc/seos.h
+++ b/firmware/os/inc/seos.h
@@ -22,6 +22,7 @@
#endif
#include <plat/taggedPtr.h>
+#include <plat/wdt.h>
#include <stdbool.h>
#include <stdint.h>
#include <stdarg.h>
@@ -30,12 +31,11 @@
#include <plat/app.h>
#include <eventnums.h>
#include <variant/variant.h>
+#include <crc.h>
#include "toolchain.h"
#include <nanohub/nanohub.h>
-//#define SEGMENT_CRC_SUPPORT
-
#ifndef MAX_TASKS
/* Default to 16 tasks, override may come from variant.h */
#define MAX_TASKS 16
@@ -177,7 +177,7 @@
bool osDefer(OsDeferCbkF callback, void *cookie, bool urgent);
-bool osTidById(uint64_t *appId, uint32_t *tid);
+bool osTidById(const uint64_t *appId, uint32_t *tid);
bool osAppInfoById(uint64_t appId, uint32_t *appIdx, uint32_t *appVer, uint32_t *appSize);
bool osAppInfoByIndex(uint32_t appIdx, uint64_t *appId, uint32_t *appVer, uint32_t *appSize);
bool osExtAppInfoByIndex(uint32_t appIdx, uint64_t *appId, uint32_t *appVer, uint32_t *appSize);
@@ -189,8 +189,9 @@
bool osAppSegmentSetState(const struct AppHdr *app, uint32_t segState);
bool osSegmentSetSize(struct Segment *seg, uint32_t size);
bool osAppWipeData(struct AppHdr *app);
-struct Segment *osGetSegment(const struct AppHdr *app);
struct Segment *osSegmentGetEnd();
+uint32_t osSegmentGetFree();
+struct Segment *osGetSegment(const struct AppHdr *app);
static inline int32_t osSegmentGetSize(const struct Segment *seg)
{
@@ -207,17 +208,19 @@
return (struct AppHdr*)(&seg[1]);
}
-#ifdef SEGMENT_CRC_SUPPORT
-
struct SegmentFooter
{
uint32_t crc;
};
#define FOOTER_SIZE sizeof(struct SegmentFooter)
-#else
-#define FOOTER_SIZE 0
-#endif
+
+static inline uint32_t osSegmentGetCrc(const struct Segment *seg)
+{
+ struct SegmentFooter *footer = (struct SegmentFooter *)(((uint8_t*)seg) +
+ ((osSegmentGetSize(seg) + 3) & ~3) + sizeof(*seg));
+ return footer ? footer->crc : 0xFFFFFFFF;
+}
static inline uint32_t osSegmentSizeAlignedWithFooter(uint32_t size)
{
@@ -243,6 +246,24 @@
return osSegmentGetState(osGetSegment(app));
}
+static inline uint32_t osAppSegmentGetCrc(const struct AppHdr *app)
+{
+ return osSegmentGetCrc(osGetSegment(app));
+}
+
+static inline uint32_t osAppSegmentCalcCrcResidue(const struct AppHdr *app)
+{
+ struct Segment *seg = osGetSegment(app);
+ uint32_t size = osSegmentSizeAlignedWithFooter(osSegmentGetSize(seg));
+ uint32_t crc;
+
+ wdtDisableClk();
+ crc = soft_crc32((uint8_t*)seg, size + sizeof(*seg), ~0);
+ wdtEnableClk();
+
+ return crc;
+}
+
struct SegmentIterator {
const struct Segment *shared;
const struct Segment *sharedEnd;
@@ -272,7 +293,6 @@
uint32_t osExtAppStopAppsByAppId(uint64_t appId);
uint32_t osExtAppEraseAppsByAppId(uint64_t appId);
uint32_t osExtAppStartAppsByAppId(uint64_t appId);
-uint32_t osExtAppStartAppsDelayed();
bool osAppIsChre(uint16_t tid);
uint32_t osAppChreVersion(uint16_t tid);
@@ -293,15 +313,33 @@
#define INTERNAL_APP_INIT(_id, _ver, _init, _end, _event) \
SET_INTERNAL_LOCATION(location, ".internal_app_init")static const struct AppHdr \
SET_INTERNAL_LOCATION_ATTRIBUTES(used, section (".internal_app_init")) mAppHdr = { \
- .hdr.magic = APP_HDR_MAGIC, \
- .hdr.fwVer = APP_HDR_VER_CUR, \
- .hdr.fwFlags = FL_APP_HDR_INTERNAL | FL_APP_HDR_APPLICATION, \
- .hdr.appId = (_id), \
- .hdr.appVer = (_ver), \
+ .hdr.magic = APP_HDR_MAGIC, \
+ .hdr.fwVer = APP_HDR_VER_CUR, \
+ .hdr.fwFlags = FL_APP_HDR_INTERNAL | FL_APP_HDR_APPLICATION, \
+ .hdr.appId = (_id), \
+ .hdr.appVer = (_ver), \
.hdr.payInfoType = LAYOUT_APP, \
- .vec.init = (uint32_t)(_init), \
- .vec.end = (uint32_t)(_end), \
- .vec.handle = (uint32_t)(_event) \
+ .vec.init = (uint32_t)(_init), \
+ .vec.end = (uint32_t)(_end), \
+ .vec.handle = (uint32_t)(_event) \
+}
+#endif
+
+#ifndef INTERNAL_CHRE_APP_INIT
+#define INTERNAL_CHRE_APP_INIT(_id, _ver, _init, _end, _event) \
+SET_INTERNAL_LOCATION(location, ".internal_app_init")static const struct AppHdr \
+SET_INTERNAL_LOCATION_ATTRIBUTES(used, section (".internal_app_init")) mAppHdr = { \
+ .hdr.magic = APP_HDR_MAGIC, \
+ .hdr.fwVer = APP_HDR_VER_CUR, \
+ .hdr.fwFlags = FL_APP_HDR_INTERNAL | FL_APP_HDR_APPLICATION | FL_APP_HDR_CHRE, \
+ .hdr.chreApiMajor = 0x01, \
+ .hdr.chreApiMinor = 0x02, \
+ .hdr.appId = (_id), \
+ .hdr.appVer = (_ver), \
+ .hdr.payInfoType = LAYOUT_APP, \
+ .vec.init = (uint32_t)(_init), \
+ .vec.end = (uint32_t)(_end), \
+ .vec.handle = (uint32_t)(_event) \
}
#endif
diff --git a/firmware/os/platform/stm32/eeData.c b/firmware/os/platform/stm32/eeData.c
index 20e859e..2710ba0 100644
--- a/firmware/os/platform/stm32/eeData.c
+++ b/firmware/os/platform/stm32/eeData.c
@@ -90,6 +90,34 @@
return (uint32_t*)data - 1;
}
+uint32_t eeDataGetSize()
+{
+ return __eedata_end - __eedata_start;
+}
+
+uint32_t eeDataGetFree()
+{
+ uint32_t *p = __eedata_start;
+
+ //find the last incarnation of "name" in flash area
+ while (p < __eedata_end) {
+ uint32_t info = *p;
+ uint32_t name = info & EE_DATA_NAME_MAX;
+ uint32_t sz = info / (EE_DATA_NAME_MAX + 1);
+
+ //check for ending condition (name == max)
+ if (name == EE_DATA_NAME_MAX)
+ break;
+
+ p++;
+
+ //skip over to next data chunk header
+ p += (sz + 3) / 4;
+ }
+
+ return __eedata_end - p;
+}
+
bool eeDataGet(uint32_t name, void *buf, uint32_t *szP)
{
uint32_t offset = 0;
diff --git a/lib/Android.mk b/lib/Android.mk
index 0caa123..88707e1 100644
--- a/lib/Android.mk
+++ b/lib/Android.mk
@@ -20,7 +20,6 @@
nanohub/aes.c \
nanohub/rsa.c \
nanohub/sha2.c \
- nanohub/softcrc.c \
src_includes := \
$(LOCAL_PATH)/include \
@@ -36,12 +35,24 @@
include $(BUILD_NANOHUB_BL_STATIC_LIBRARY)
+include $(CLEAR_NANO_VARS)
+
+LOCAL_MODULE := libnanohub_common_os
+LOCAL_MODULE_TAGS := optional
+
+LOCAL_SRC_FILES := nanohub/softcrc.c
+LOCAL_C_INCLUDES := $(src_includes)
+LOCAL_EXPORT_C_INCLUDE_DIRS := $(src_includes)
+
+include $(BUILD_NANOHUB_OS_STATIC_LIBRARY)
+
include $(CLEAR_VARS)
LOCAL_MODULE := libnanohub_common
LOCAL_MODULE_TAGS := optional
LOCAL_SRC_FILES := \
$(src_files) \
+ nanohub/softcrc.c \
nanohub/nanoapp.c \
LOCAL_CFLAGS := \
diff --git a/sensorhal/Android.mk b/sensorhal/Android.mk
index 385a762..04bc80e 100644
--- a/sensorhal/Android.mk
+++ b/sensorhal/Android.mk
@@ -72,6 +72,9 @@
sensors.cpp \
../../../../$(NANOHUB_SENSORHAL_SENSORLIST)
+LOCAL_HEADER_LIBRARIES := \
+ libhardware_headers
+
LOCAL_SHARED_LIBRARIES := \
liblog \
libcutils \
@@ -114,6 +117,9 @@
LOCAL_SRC_FILES := \
activity.cpp
+LOCAL_HEADER_LIBRARIES := \
+ libhardware_headers
+
LOCAL_SHARED_LIBRARIES := \
libcutils \
libhubconnection \
diff --git a/sensorhal/hubconnection.cpp b/sensorhal/hubconnection.cpp
index 9629703..6bf3512 100644
--- a/sensorhal/hubconnection.cpp
+++ b/sensorhal/hubconnection.cpp
@@ -132,6 +132,8 @@
mMagAccuracyRestore = SENSOR_STATUS_UNRELIABLE;
mGyroBias[0] = mGyroBias[1] = mGyroBias[2] = 0.0f;
mAccelBias[0] = mAccelBias[1] = mAccelBias[2] = 0.0f;
+ mAccelEnabledBias[0] = mAccelEnabledBias[1] = mAccelEnabledBias[2] = 0.0f;
+ mAccelEnabledBiasStored = true;
memset(&mGyroOtcData, 0, sizeof(mGyroOtcData));
mLefty.accel = false;
@@ -238,7 +240,6 @@
mSensorState[COMMS_SENSOR_DOUBLE_TAP].sensorType = SENS_TYPE_DOUBLE_TAP;
mSensorState[COMMS_SENSOR_DOUBLE_TAP].rate = SENSOR_RATE_ONCHANGE;
mSensorState[COMMS_SENSOR_WRIST_TILT].sensorType = SENS_TYPE_WRIST_TILT;
- mSensorState[COMMS_SENSOR_WRIST_TILT].rate = SENSOR_RATE_ONCHANGE;
mSensorState[COMMS_SENSOR_DOUBLE_TOUCH].sensorType = SENS_TYPE_DOUBLE_TOUCH;
mSensorState[COMMS_SENSOR_DOUBLE_TOUCH].rate = SENSOR_RATE_ONESHOT;
mSensorState[COMMS_SENSOR_ACTIVITY_IN_VEHICLE_START].sensorType = SENS_TYPE_ACTIVITY_IN_VEHICLE_START;
@@ -733,6 +734,22 @@
sendDirectReportEvent(&nev[cnt], 1);
if (mSensorState[sensor].enable && isSampleIntervalSatisfied(sensor, timestamp)) {
+ if (!mAccelEnabledBiasStored) {
+ // accel is enabled, but no enabled bias. Store latest bias and use
+ // for accel and uncalibrated accel due to:
+ // https://source.android.com/devices/sensors/sensor-types.html
+ // "The bias and scale calibration must only be updated while the sensor is deactivated,
+ // so as to avoid causing jumps in values during streaming."
+ mAccelEnabledBiasStored = true;
+ mAccelEnabledBias[0] = mAccelBias[0];
+ mAccelEnabledBias[1] = mAccelBias[1];
+ mAccelEnabledBias[2] = mAccelBias[2];
+ }
+ // samples arrive using latest bias
+ // adjust for enabled bias being different from lastest bias
+ sv->x += mAccelBias[0] - mAccelEnabledBias[0];
+ sv->y += mAccelBias[1] - mAccelEnabledBias[1];
+ sv->z += mAccelBias[2] - mAccelEnabledBias[2];
++cnt;
}
@@ -742,9 +759,17 @@
ue->x_uncalib = sample->ix * mScaleAccel + mAccelBias[0];
ue->y_uncalib = sample->iy * mScaleAccel + mAccelBias[1];
ue->z_uncalib = sample->iz * mScaleAccel + mAccelBias[2];
- ue->x_bias = mAccelBias[0];
- ue->y_bias = mAccelBias[1];
- ue->z_bias = mAccelBias[2];
+ if (!mAccelEnabledBiasStored) {
+ // No enabled bias (which means accel is disabled). Use latest bias.
+ ue->x_bias = mAccelBias[0];
+ ue->y_bias = mAccelBias[1];
+ ue->z_bias = mAccelBias[2];
+ } else {
+ // enabled bias is valid, so use it
+ ue->x_bias = mAccelEnabledBias[0];
+ ue->y_bias = mAccelEnabledBias[1];
+ ue->z_bias = mAccelEnabledBias[2];
+ }
sendDirectReportEvent(&nev[cnt], 1);
if (mSensorState[COMMS_SENSOR_ACCEL_UNCALIBRATED].enable
@@ -1674,6 +1699,11 @@
Mutex::Autolock autoLock(mLock);
if (isValidHandle(handle)) {
+ // disabling accel, so no longer need to use the bias from when
+ // accel was first enabled
+ if (handle == COMMS_SENSOR_ACCEL && !enable)
+ mAccelEnabledBiasStored = false;
+
mSensorState[handle].enable = enable;
initConfigCmd(&cmd, handle);
diff --git a/sensorhal/hubconnection.h b/sensorhal/hubconnection.h
index 558ece3..63bd22d 100644
--- a/sensorhal/hubconnection.h
+++ b/sensorhal/hubconnection.h
@@ -247,7 +247,8 @@
uint8_t mMagAccuracy;
uint8_t mMagAccuracyRestore;
- float mGyroBias[3], mAccelBias[3];
+ float mGyroBias[3], mAccelBias[3], mAccelEnabledBias[3];
+ bool mAccelEnabledBiasStored;
GyroOtcData mGyroOtcData;
float mScaleAccel, mScaleMag;
diff --git a/util/common/Android.bp b/util/common/Android.bp
new file mode 100644
index 0000000..ac6d65d
--- /dev/null
+++ b/util/common/Android.bp
@@ -0,0 +1,37 @@
+//
+// Copyright (C) 2018 The Android Open Source Project
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+cc_library_static {
+ name: "libhubutilcommon",
+ srcs: [
+ "file.cpp",
+ "JSONObject.cpp",
+ "ring.cpp",
+ ],
+ cflags: ["-Wall", "-Werror", "-Wextra"],
+ header_libs: [
+ "libhardware_headers",
+ "libstagefright_foundation_headers",
+ "libstagefright_headers",
+ "libutils_headers",
+ ],
+ export_header_lib_headers: [
+ "libhardware_headers",
+ "libutils_headers",
+ ],
+ export_include_dirs: [
+ ".",
+ ],
+ proprietary: true,
+}
diff --git a/util/common/Android.mk b/util/common/Android.mk
deleted file mode 100644
index bd40f5a..0000000
--- a/util/common/Android.mk
+++ /dev/null
@@ -1,36 +0,0 @@
-# Copyright (C) 2015 The Android Open Source Project
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-# http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-LOCAL_PATH := $(call my-dir)
-
-COMMON_CFLAGS := -Wall -Werror -Wextra
-
-include $(CLEAR_VARS)
-
-LOCAL_MODULE := libhubutilcommon
-LOCAL_MODULE_TAGS := optional
-
-LOCAL_CFLAGS += $(COMMON_CFLAGS)
-
-LOCAL_C_INCLUDES += \
- $(LOCAL_PATH)
-
-LOCAL_SRC_FILES := \
- file.cpp \
- JSONObject.cpp \
- ring.cpp
-
-LOCAL_EXPORT_C_INCLUDE_DIRS := $(LOCAL_C_INCLUDES)
-
-include $(BUILD_STATIC_LIBRARY)
diff --git a/util/common/JSONObject.cpp b/util/common/JSONObject.cpp
index 83ed14e..8679c71 100644
--- a/util/common/JSONObject.cpp
+++ b/util/common/JSONObject.cpp
@@ -14,10 +14,6 @@
* limitations under the License.
*/
-//#define LOG_NDEBUG 0
-#define LOG_TAG "JSONObject"
-#include <utils/Log.h>
-
#include "JSONObject.h"
#include <ctype.h>
diff --git a/util/common/file.cpp b/util/common/file.cpp
index 1757601..daf9e40 100644
--- a/util/common/file.cpp
+++ b/util/common/file.cpp
@@ -14,9 +14,6 @@
* limitations under the License.
*/
-//#define LOG_NDEBUG 0
-#define LOG_TAG "file"
-#include <utils/Log.h>
#include "file.h"
#include <fcntl.h>
diff --git a/util/common/file.h b/util/common/file.h
index 97d74b0..4bbe613 100644
--- a/util/common/file.h
+++ b/util/common/file.h
@@ -18,6 +18,8 @@
#define FILE_H_
+#include <stdio.h> // for SEEK_SET
+
#include <media/stagefright/foundation/ABase.h>
#include <utils/Errors.h>
diff --git a/util/common/ring.cpp b/util/common/ring.cpp
index 245f3d0..c5ac53d 100644
--- a/util/common/ring.cpp
+++ b/util/common/ring.cpp
@@ -14,10 +14,6 @@
* limitations under the License.
*/
-//#define LOG_DEBUG 1
-#define LOG_TAG "ring"
-#include <utils/Log.h>
-
#include "ring.h"
#include <stdlib.h>
diff --git a/util/nanoapp_cmd/nanoapp_cmd.c b/util/nanoapp_cmd/nanoapp_cmd.c
index 5692181..af19386 100644
--- a/util/nanoapp_cmd/nanoapp_cmd.c
+++ b/util/nanoapp_cmd/nanoapp_cmd.c
@@ -45,7 +45,9 @@
#define MAX_DOWNLOAD_RETRIES 4
#define UNINSTALL_CMD "uninstall"
-#define NANOHUB_EXT_APP_DELETE 2
+#define NANOHUB_HAL_EXT_APPS_ON 0
+#define NANOHUB_HAL_EXT_APPS_OFF 1
+#define NANOHUB_HAL_EXT_APP_DELETE 2
#define LOGE(fmt, ...) do { \
__android_log_print(ANDROID_LOG_ERROR, LOG_TAG, fmt, ##__VA_ARGS__); \
@@ -72,6 +74,13 @@
uint8_t data[];
} __attribute__((packed));
+struct HalCmd
+{
+ struct HostMsgHdr hdr;
+ uint8_t cmd;
+ uint64_t appId;
+} __attribute__((packed));
+
struct App
{
uint32_t num;
@@ -238,6 +247,38 @@
return NULL;
}
+int findAppIdByName(char *name, uint64_t *appId)
+{
+ FILE *fp;
+ char *line = NULL;
+ size_t len;
+ ssize_t numRead;
+ int ret = 0;
+
+ fp = openFile("/vendor/firmware/napp_list.cfg", "r");
+ if (!fp)
+ return -1;
+
+ while ((numRead = getline(&line, &len, fp)) != -1) {
+ char entry[MAX_APP_NAME_LEN+1];
+ uint32_t appVersion;
+
+ sscanf(line, "%" STRINGIFY(MAX_APP_NAME_LEN) "s %" PRIx64 " %" PRIx32 "\n", entry, appId, &appVersion);
+
+ if (strncmp(entry, name, MAX_APP_NAME_LEN) == 0) {
+ ret = 1;
+ break;
+ }
+ }
+
+ fclose(fp);
+
+ if (line)
+ free(line);
+
+ return ret;
+}
+
int parseConfigAppInfo(int *installCnt, int *uninstallCnt)
{
FILE *fp;
@@ -309,23 +350,62 @@
printf("done\n");
}
+bool sendCmd(uint8_t cmd, uint64_t appId)
+{
+ struct HalCmd msg;
+
+ msg.hdr.eventId = EVT_APP_FROM_HOST;
+ msg.hdr.appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0);
+ msg.hdr.len = sizeof(msg) - sizeof(msg.hdr); // payload length
+ msg.cmd = cmd;
+ memcpy(&msg.appId, &appId, sizeof(uint64_t));
+
+ return fileWriteData("/dev/nanohub", &msg, sizeof(msg));
+}
+
+int halCmd(uint8_t cmd, char *arg)
+{
+ uint64_t appId;
+ char *endptr = arg + strlen(arg);
+
+ if (strcmp(arg, UNINSTALL_CMD) == 0) {
+ printf("%s is not a valid app name\n", arg);
+ return 1;
+ }
+
+ if ((findAppIdByName(arg, &appId) == 1) || (appId = strtoull(arg, &endptr, 16)) > 0) {
+ if (*endptr != '\0') {
+ printf("Couldn't find nanoapp '%s' in napp_list.cfg\n", arg);
+ return 1;
+ } else if (cmd == NANOHUB_HAL_EXT_APPS_ON)
+ printf("Loading ");
+ else if (cmd == NANOHUB_HAL_EXT_APPS_OFF)
+ printf("Unloading ");
+ else if (cmd == NANOHUB_HAL_EXT_APP_DELETE)
+ printf("Deleting ");
+ else {
+ printf("Unrecognized cmd: %d\n", cmd);
+ return 1;
+ }
+ printf("\"0x%016" PRIx64 "\"...", appId);
+ fflush(stdout);
+ if (sendCmd(cmd, appId))
+ printf("done\n");
+ return 0;
+ } else {
+ printf("Couldn't find nanoapp '%s' in napp_list.cfg\n", arg);
+ return 1;
+ }
+}
+
void removeApps(int updateCnt)
{
- uint8_t buffer[sizeof(struct HostMsgHdr) + 1 + sizeof(uint64_t)];
- struct HostMsgHdr *mHostMsgHdr = (struct HostMsgHdr *)(&buffer[0]);
- uint8_t *cmd = (uint8_t *)(&buffer[sizeof(struct HostMsgHdr)]);
- uint64_t *appId = (uint64_t *)(&buffer[sizeof(struct HostMsgHdr) + 1]);
int i;
for (i = 0; i < updateCnt; i++) {
- mHostMsgHdr->eventId = EVT_APP_FROM_HOST;
- mHostMsgHdr->appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0);
- mHostMsgHdr->len = 1 + sizeof(uint64_t);
- *cmd = NANOHUB_EXT_APP_DELETE;
- memcpy(appId, &appsToUninstall[i], sizeof(uint64_t));
- printf("Deleting \"%016" PRIx64 "\"...", appsToUninstall[i]);
+ printf("Deleting \"0x%016" PRIx64 "\"...", appsToUninstall[i]);
fflush(stdout);
- if (fileWriteData("/dev/nanohub", buffer, sizeof(buffer)))
+ if (sendCmd(NANOHUB_HAL_EXT_APP_DELETE, appsToUninstall[i]))
printf("done\n");
}
}
@@ -372,7 +452,7 @@
if (argc < 3 && (argc < 2 || strcmp(argv[1], "download") != 0)) {
printf("usage: %s <action> <sensor> <data> -d\n", argv[0]);
- printf(" action: config|cfgdata|calibrate|flush|download\n");
+ printf(" action: config|cfgdata|calibrate|flush\n");
printf(" sensor: (uncal_)accel|(uncal_)gyro|(uncal_)mag|als|prox|baro|temp|orien\n");
printf(" gravity|geomag|linear_acc|rotation|game\n");
printf(" win_orien|tilt|step_det|step_cnt|double_tap\n");
@@ -383,6 +463,9 @@
printf(" calibrate: [N.A.]\n");
printf(" flush: [N.A.]\n");
printf(" -d: if specified, %s will keep draining /dev/nanohub until cancelled.\n", argv[0]);
+ printf("usage: %s <cmd> [app]\n", argv[0]);
+ printf(" cmd: download|load|unload|delete\n");
+ printf(" app: appId or name from napp_list.cfg\n");
return 1;
}
@@ -469,6 +552,27 @@
printf("Unsupported sensor: %s For action: %s\n", argv[2], argv[1]);
return 1;
}
+ } else if (strcmp(argv[1], "load") == 0) {
+ if (argc != 3) {
+ printf("Wrong arg number\n");
+ return 1;
+ }
+
+ return halCmd(NANOHUB_HAL_EXT_APPS_ON, argv[2]);
+ } else if (strcmp(argv[1], "unload") == 0) {
+ if (argc != 3) {
+ printf("Wrong arg number\n");
+ return 1;
+ }
+
+ return halCmd(NANOHUB_HAL_EXT_APPS_OFF, argv[2]);
+ } else if (strcmp(argv[1], "delete") == 0) {
+ if (argc != 3) {
+ printf("Wrong arg number\n");
+ return 1;
+ }
+
+ return halCmd(NANOHUB_HAL_EXT_APP_DELETE, argv[2]);
} else if (strcmp(argv[1], "download") == 0) {
int installCnt, uninstallCnt;
@@ -496,8 +600,9 @@
if (parseConfigAppInfo(&installCnt, &uninstallCnt) != 0) {
LOGE("Failed to download all apps!");
+ return 1;
}
- return 1;
+ return 0;
} else {
printf("Unsupported action: %s\n", argv[1]);
return 1;
