diff options
author | Android Build Coastguard Worker <android-build-coastguard-worker@google.com> | 2022-05-11 05:02:30 +0000 |
---|---|---|
committer | Android Build Coastguard Worker <android-build-coastguard-worker@google.com> | 2022-05-11 05:02:30 +0000 |
commit | 14f2d57c44af0dda5ec9f1b0d7aa19bbc25fa387 (patch) | |
tree | 75cf19c94f1b68eb875ab7db62526dacdb04d0ca | |
parent | 2457a6d194efd426f2f5d4186efcece5c4057e27 (diff) | |
parent | 23569ea3ef5706cf2df403d9722ea7fd3a95db82 (diff) | |
download | contexthub-android13-mainline-mediaprovider-release.tar.gz |
Snap for 8570526 from 23569ea3ef5706cf2df403d9722ea7fd3a95db82 to mainline-mediaprovider-releaseaml_mpr_331918000aml_mpr_331812020aml_mpr_331711020aml_mpr_331613010aml_mpr_331512020aml_mpr_331412040aml_mpr_331311080aml_mpr_331112050aml_mpr_331112030aml_mpr_331011070aml_mpr_330911040aml_mpr_330811020android13-mainline-mediaprovider-release
Change-Id: I2892246cab234a4fd3cb25ab342b1c98b5f2ca03
71 files changed, 774 insertions, 333 deletions
diff --git a/Android.bp b/Android.bp new file mode 100644 index 00000000..2fa08975 --- /dev/null +++ b/Android.bp @@ -0,0 +1,45 @@ +// +// Copyright (C) 2021 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. + +package { + default_applicable_licenses: ["device_google_contexthub_license"], +} + +// Added automatically by a large-scale-change that took the approach of +// 'apply every license found to every target'. While this makes sure we respect +// every license restriction, it may not be entirely correct. +// +// e.g. GPL in an MIT project might only apply to the contrib/ directory. +// +// Please consider splitting the single license below into multiple licenses, +// taking care not to lose any license_kind information, and overriding the +// default license using the 'licenses: [...]' property on targets as needed. +// +// For unused files, consider creating a 'fileGroup' with "//visibility:private" +// to attach the license to, and including a comment whether the files may be +// used in the current project. +// See: http://go/android-license-faq +license { + name: "device_google_contexthub_license", + visibility: [":__subpackages__"], + license_kinds: [ + "SPDX-license-identifier-Apache-2.0", + "SPDX-license-identifier-BSD", + "SPDX-license-identifier-MIT", + "SPDX-license-identifier-NCSA", + "legacy_notice", + ], + // large-scale-change unable to identify any license_text files +} diff --git a/METADATA b/METADATA new file mode 100644 index 00000000..d97975ca --- /dev/null +++ b/METADATA @@ -0,0 +1,3 @@ +third_party { + license_type: NOTICE +} diff --git a/contexthubhal/Android.bp b/contexthubhal/Android.bp index c55c526f..a6fc724a 100644 --- a/contexthubhal/Android.bp +++ b/contexthubhal/Android.bp @@ -12,6 +12,15 @@ // 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. +package { + // See: http://go/android-license-faq + // A large-scale-change added 'default_applicable_licenses' to import + // all of the 'license_kinds' from "device_google_contexthub_license" + // to get the below license kinds: + // SPDX-license-identifier-Apache-2.0 + default_applicable_licenses: ["device_google_contexthub_license"], +} + cc_defaults { name: "contexthub_libs_default", relative_install_path: "hw", diff --git a/contexthubhal/system_comms.cpp b/contexthubhal/system_comms.cpp index 87e5feef..30106995 100644 --- a/contexthubhal/system_comms.cpp +++ b/contexthubhal/system_comms.cpp @@ -1015,8 +1015,8 @@ bool SystemComm::AppManager::saveApps() } // Write the JSON string to disk. - Json::StyledWriter writer; - std::string serializedSettings(writer.write(*appsObject)); + Json::StreamWriterBuilder factory; + std::string serializedSettings(Json::writeString(factory, *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)", @@ -1050,8 +1050,10 @@ bool SystemComm::AppManager::restoreApps() std::string str(buf); std::shared_ptr<Json::Value> in(new Json::Value); - Json::Reader reader; - bool valid = reader.parse(str, *in); + Json::CharReaderBuilder builder; + std::unique_ptr<Json::CharReader> reader(builder.newCharReader()); + bool valid = reader->parse( + str.data(), str.data() + str.size(), in.get(), /* errorMessage = */ nullptr); free(buf); if (valid && in->isObject()) { diff --git a/contexthubhal/test/Android.bp b/contexthubhal/test/Android.bp index 9ab543c5..fb352a16 100644 --- a/contexthubhal/test/Android.bp +++ b/contexthubhal/test/Android.bp @@ -14,6 +14,15 @@ // limitations under the License. // +package { + // See: http://go/android-license-faq + // A large-scale-change added 'default_applicable_licenses' to import + // all of the 'license_kinds' from "device_google_contexthub_license" + // to get the below license kinds: + // legacy_notice + default_applicable_licenses: ["device_google_contexthub_license"], +} + cc_test { name: "nanohub_hal_test", gtest: false, diff --git a/firmware/Android.mk b/firmware/Android.mk index 3f4b9095..87897bc4 100644 --- a/firmware/Android.mk +++ b/firmware/Android.mk @@ -21,6 +21,9 @@ ifneq ($(NANOHUB_OS_PATH),) include $(CLEAR_NANO_VARS) LOCAL_MODULE := libnanohub_os +LOCAL_LICENSE_KINDS := SPDX-license-identifier-Apache-2.0 SPDX-license-identifier-BSD SPDX-license-identifier-MIT SPDX-license-identifier-NCSA +LOCAL_LICENSE_CONDITIONS := notice +LOCAL_NOTICE_FILE := $(LOCAL_PATH)/NOTICE LOCAL_MODULE_TAGS := optional LOCAL_SRC_FILES := \ @@ -60,6 +63,9 @@ include $(BUILD_NANOHUB_OS_STATIC_LIBRARY) include $(CLEAR_NANO_VARS) LOCAL_MODULE := nanohub_bl +LOCAL_LICENSE_KINDS := SPDX-license-identifier-Apache-2.0 SPDX-license-identifier-BSD SPDX-license-identifier-MIT SPDX-license-identifier-NCSA +LOCAL_LICENSE_CONDITIONS := notice +LOCAL_NOTICE_FILE := $(LOCAL_PATH)/NOTICE LOCAL_SRC_FILES := \ os/core/bl.c \ @@ -80,6 +86,9 @@ include $(BUILD_NANOHUB_BL_EXECUTABLE) include $(CLEAR_NANO_VARS) LOCAL_MODULE := nanohub_os +LOCAL_LICENSE_KINDS := SPDX-license-identifier-Apache-2.0 SPDX-license-identifier-BSD SPDX-license-identifier-MIT SPDX-license-identifier-NCSA +LOCAL_LICENSE_CONDITIONS := notice +LOCAL_NOTICE_FILE := $(LOCAL_PATH)/NOTICE LOCAL_CFLAGS := \ -DPLATFORM_HW_VER=0 \ diff --git a/firmware/app/chre/chre_test0.app/Android.mk b/firmware/app/chre/chre_test0.app/Android.mk index 4e265c20..4eb25cd4 100644 --- a/firmware/app/chre/chre_test0.app/Android.mk +++ b/firmware/app/chre/chre_test0.app/Android.mk @@ -19,6 +19,9 @@ LOCAL_PATH := $(call my-dir) include $(CLEAR_NANO_VARS) LOCAL_MODULE := chre_test0 +LOCAL_LICENSE_KINDS := SPDX-license-identifier-Apache-2.0 +LOCAL_LICENSE_CONDITIONS := notice +LOCAL_NOTICE_FILE := $(LOCAL_PATH)/../../../NOTICE LOCAL_MODULE_TAGS := optional # Googl + T + 0x9000 diff --git a/firmware/app/chre/chre_test0.app/main.c b/firmware/app/chre/chre_test0.app/main.c index 03e6d8d8..e1874494 100644 --- a/firmware/app/chre/chre_test0.app/main.c +++ b/firmware/app/chre/chre_test0.app/main.c @@ -89,7 +89,7 @@ void nanoappHandleEvent(uint32_t srcTid, uint16_t evtType, const void* evtData) ") cnt: %d\n", t->timerId, chreGetTime(), mCnt); extMsg->msg = 0x01; extMsg->val = mCnt; - chreSendMessageToHost(extMsg, sizeof(*extMsg), 0, nanoappFreeMessage); + chreSendMessageToHostEndpoint(extMsg, sizeof(*extMsg), 0, CHRE_HOST_ENDPOINT_BROADCAST, nanoappFreeMessage); if (mCnt-- <= 0) chreTimerCancel(t->timerId); break; diff --git a/firmware/app/chre/chre_test1.app/Android.mk b/firmware/app/chre/chre_test1.app/Android.mk index f5a303c8..59fd063e 100644 --- a/firmware/app/chre/chre_test1.app/Android.mk +++ b/firmware/app/chre/chre_test1.app/Android.mk @@ -19,6 +19,9 @@ LOCAL_PATH := $(call my-dir) include $(CLEAR_NANO_VARS) LOCAL_MODULE := chre_test1 +LOCAL_LICENSE_KINDS := SPDX-license-identifier-Apache-2.0 +LOCAL_LICENSE_CONDITIONS := notice +LOCAL_NOTICE_FILE := $(LOCAL_PATH)/../../../NOTICE LOCAL_MODULE_TAGS := optional # Googl + T + 0x9001 diff --git a/firmware/app/chre/chre_test1.app/main.cpp b/firmware/app/chre/chre_test1.app/main.cpp index 201448a9..d00415e0 100644 --- a/firmware/app/chre/chre_test1.app/main.cpp +++ b/firmware/app/chre/chre_test1.app/main.cpp @@ -102,7 +102,7 @@ void nanoappHandleEvent(uint32_t srcTid, uint16_t evtType, const void* evtData) ") cnt: %d\n", t->timerId, chreGetTime(), mCnt); extMsg->msg = 0x01; extMsg->val = mCnt; - chreSendMessageToHost(extMsg, sizeof(*extMsg), 0, nanoappFreeMessage); + chreSendMessageToHostEndpoint(extMsg, sizeof(*extMsg), 0, CHRE_HOST_ENDPOINT_BROADCAST, nanoappFreeMessage); if (mCnt-- <= 0) chreTimerCancel(t->timerId); break; diff --git a/firmware/app/chre/chre_test2.app/Android.mk b/firmware/app/chre/chre_test2.app/Android.mk index 0a471f19..5631b000 100644 --- a/firmware/app/chre/chre_test2.app/Android.mk +++ b/firmware/app/chre/chre_test2.app/Android.mk @@ -19,6 +19,9 @@ LOCAL_PATH := $(call my-dir) include $(CLEAR_NANO_VARS) LOCAL_MODULE := chre_test2 +LOCAL_LICENSE_KINDS := SPDX-license-identifier-Apache-2.0 +LOCAL_LICENSE_CONDITIONS := notice +LOCAL_NOTICE_FILE := $(LOCAL_PATH)/../../../NOTICE LOCAL_MODULE_TAGS := optional # Googl + T + 0x9002 diff --git a/firmware/app/chre/common/Android.mk b/firmware/app/chre/common/Android.mk index 6a996db5..9e230a0d 100644 --- a/firmware/app/chre/common/Android.mk +++ b/firmware/app/chre/common/Android.mk @@ -23,6 +23,9 @@ LOCAL_PATH := $(call my-dir) include $(CLEAR_NANO_VARS) LOCAL_MODULE := libnanochre10 +LOCAL_LICENSE_KINDS := SPDX-license-identifier-Apache-2.0 +LOCAL_LICENSE_CONDITIONS := notice +LOCAL_NOTICE_FILE := $(LOCAL_PATH)/../../../NOTICE LOCAL_MODULE_TAGS := optional LOCAL_SRC_FILES := \ @@ -41,6 +44,9 @@ include $(BUILD_NANOHUB_APP_STATIC_LIBRARY) include $(CLEAR_NANO_VARS) LOCAL_MODULE := libnanochre11 +LOCAL_LICENSE_KINDS := SPDX-license-identifier-Apache-2.0 +LOCAL_LICENSE_CONDITIONS := notice +LOCAL_NOTICE_FILE := $(LOCAL_PATH)/../../../NOTICE LOCAL_MODULE_TAGS := optional LOCAL_SRC_FILES := \ @@ -59,6 +65,9 @@ include $(BUILD_NANOHUB_APP_STATIC_LIBRARY) include $(CLEAR_NANO_VARS) LOCAL_MODULE := libnanochre +LOCAL_LICENSE_KINDS := SPDX-license-identifier-Apache-2.0 +LOCAL_LICENSE_CONDITIONS := notice +LOCAL_NOTICE_FILE := $(LOCAL_PATH)/../../../NOTICE LOCAL_MODULE_TAGS := optional LOCAL_SRC_FILES := \ diff --git a/firmware/app/chre/common/chre_app.c b/firmware/app/chre/common/chre_app.c index 04829b61..ed5b05ee 100644 --- a/firmware/app/chre/common/chre_app.c +++ b/firmware/app/chre/common/chre_app.c @@ -48,7 +48,7 @@ static void initDataHeader(struct chreSensorDataHeader *header, uint64_t timesta header->baseTimestamp = timestamp; header->sensorHandle = sensorHandle; header->readingCount = 1; - header->reserved[0] = header->reserved[1] = 0; + header->reserved = 0; } static void processTripleAxisData(const struct TripleAxisDataEvent *src, uint32_t sensorHandle, uint8_t sensorType) diff --git a/firmware/app/test0.app/Android.mk b/firmware/app/test0.app/Android.mk index 3e7d6493..10beea86 100644 --- a/firmware/app/test0.app/Android.mk +++ b/firmware/app/test0.app/Android.mk @@ -19,6 +19,9 @@ LOCAL_PATH := $(call my-dir) include $(CLEAR_NANO_VARS) LOCAL_MODULE := test0 +LOCAL_LICENSE_KINDS := SPDX-license-identifier-Apache-2.0 +LOCAL_LICENSE_CONDITIONS := notice +LOCAL_NOTICE_FILE := $(LOCAL_PATH)/../../NOTICE LOCAL_MODULE_TAGS := optional # Googl + T + 0x8000 diff --git a/firmware/app/test1.app/Android.mk b/firmware/app/test1.app/Android.mk index 7d04e3c6..f7875fe3 100644 --- a/firmware/app/test1.app/Android.mk +++ b/firmware/app/test1.app/Android.mk @@ -19,6 +19,9 @@ LOCAL_PATH := $(call my-dir) include $(CLEAR_NANO_VARS) LOCAL_MODULE := test1 +LOCAL_LICENSE_KINDS := SPDX-license-identifier-Apache-2.0 +LOCAL_LICENSE_CONDITIONS := notice +LOCAL_NOTICE_FILE := $(LOCAL_PATH)/../../NOTICE LOCAL_MODULE_TAGS := optional # Googl + T + 0x8000 diff --git a/firmware/build/common_config.mk b/firmware/build/common_config.mk index 990db3c4..189d71d3 100644 --- a/firmware/build/common_config.mk +++ b/firmware/build/common_config.mk @@ -71,6 +71,7 @@ LOCAL_CFLAGS_stm32 += \ # CHRE-specific LOCAL_CFLAGS += \ -DCHRE_MESSAGE_TO_HOST_MAX_SIZE=128 \ + -DCHRE_NANOAPP_SUPPORTS_PRE_V1_5 \ -DCHRE_NO_DOUBLE_SUPPORT \ # DEBUG/RELEASE-specific diff --git a/firmware/chre/platform/assert.h b/firmware/chre/platform/assert.h new file mode 100644 index 00000000..a2bba24c --- /dev/null +++ b/firmware/chre/platform/assert.h @@ -0,0 +1,25 @@ +/* + * Copyright (C) 2022 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 PLATFORM_ASSERT_H_ +#define PLATFORM_ASSERT_H_ + +#include <util/nano_assert.h> + +#define CHRE_ASSERT ASSERT +#define CHRE_ASSERT_NOT_NULL ASSERT_NOT_NULL + +#endif diff --git a/firmware/external/Android.mk b/firmware/external/Android.mk index 31baa2bf..d4cb5711 100644 --- a/firmware/external/Android.mk +++ b/firmware/external/Android.mk @@ -19,6 +19,9 @@ LOCAL_PATH := $(call my-dir) include $(CLEAR_NANO_VARS) LOCAL_MODULE := libnanomath_os +LOCAL_LICENSE_KINDS := SPDX-license-identifier-BSD +LOCAL_LICENSE_CONDITIONS := notice +LOCAL_NOTICE_FILE := $(LOCAL_PATH)/../NOTICE LOCAL_MODULE_TAGS := optional LOCAL_SRC_FILES := \ @@ -46,6 +49,9 @@ include $(BUILD_NANOHUB_OS_STATIC_LIBRARY) include $(CLEAR_NANO_VARS) LOCAL_MODULE := libnanomath +LOCAL_LICENSE_KINDS := SPDX-license-identifier-BSD +LOCAL_LICENSE_CONDITIONS := notice +LOCAL_NOTICE_FILE := $(LOCAL_PATH)/../NOTICE LOCAL_MODULE_TAGS := optional LOCAL_SRC_FILES := \ diff --git a/firmware/firmware.mk b/firmware/firmware.mk index 221ee0cb..c35756a4 100644 --- a/firmware/firmware.mk +++ b/firmware/firmware.mk @@ -57,6 +57,7 @@ FLAGS += -I$(VARIANT_PATH)/inc FLAGS += -Iexternal/freebsd/inc FLAGS += -I../lib/include FLAGS += -I../../../../system/chre/chre_api/include/chre_api +FLAGS += -I../../../../system/chre/util/include FLAGS += -Wall -Werror #help avoid commmon embedded C mistakes diff --git a/firmware/firmware_conf.mk b/firmware/firmware_conf.mk index 7ea3f612..f6da08f4 100644 --- a/firmware/firmware_conf.mk +++ b/firmware/firmware_conf.mk @@ -17,5 +17,6 @@ # parameters common for OS and APPs COMMON_FLAGS += \ -DCHRE_MESSAGE_TO_HOST_MAX_SIZE=128 \ + -DCHRE_NANOAPP_SUPPORTS_PRE_V1_5 \ -DCHRE_NO_DOUBLE_SUPPORT \ diff --git a/firmware/lib/builtins/Android.mk b/firmware/lib/builtins/Android.mk index b43bdb2b..de685e12 100644 --- a/firmware/lib/builtins/Android.mk +++ b/firmware/lib/builtins/Android.mk @@ -19,6 +19,9 @@ LOCAL_PATH := $(call my-dir) include $(CLEAR_NANO_VARS) LOCAL_MODULE := libnanobuiltins +LOCAL_LICENSE_KINDS := SPDX-license-identifier-BSD SPDX-license-identifier-MIT SPDX-license-identifier-NCSA +LOCAL_LICENSE_CONDITIONS := notice +LOCAL_NOTICE_FILE := $(LOCAL_PATH)/LICENSE.TXT $(LOCAL_PATH)/NOTICE LOCAL_MODULE_TAGS := optional LOCAL_SRC_FILES_cortexm4 := \ diff --git a/firmware/lib/libc/Android.mk b/firmware/lib/libc/Android.mk index 1558e473..6f4578ee 100644 --- a/firmware/lib/libc/Android.mk +++ b/firmware/lib/libc/Android.mk @@ -27,6 +27,9 @@ src_files := \ strncpy.c \ LOCAL_MODULE := libnanolibc_os +LOCAL_LICENSE_KINDS := SPDX-license-identifier-Apache-2.0 SPDX-license-identifier-BSD +LOCAL_LICENSE_CONDITIONS := notice +LOCAL_NOTICE_FILE := $(LOCAL_PATH)/../../NOTICE LOCAL_MODULE_TAGS := optional LOCAL_SRC_FILES := $(src_files) @@ -39,6 +42,9 @@ include $(BUILD_NANOHUB_OS_STATIC_LIBRARY) include $(CLEAR_NANO_VARS) LOCAL_MODULE := libnanolibc +LOCAL_LICENSE_KINDS := SPDX-license-identifier-Apache-2.0 SPDX-license-identifier-BSD +LOCAL_LICENSE_CONDITIONS := notice +LOCAL_NOTICE_FILE := $(LOCAL_PATH)/../../NOTICE LOCAL_MODULE_TAGS := optional LOCAL_SRC_FILES := $(src_files) diff --git a/firmware/lib/libm/Android.mk b/firmware/lib/libm/Android.mk index b2f872fe..b5f86b31 100644 --- a/firmware/lib/libm/Android.mk +++ b/firmware/lib/libm/Android.mk @@ -19,6 +19,9 @@ LOCAL_PATH := $(call my-dir) include $(CLEAR_NANO_VARS) LOCAL_MODULE := libnanolibm +LOCAL_LICENSE_KINDS := SPDX-license-identifier-Apache-2.0 SPDX-license-identifier-BSD +LOCAL_LICENSE_CONDITIONS := notice +LOCAL_NOTICE_FILE := $(LOCAL_PATH)/../../NOTICE LOCAL_MODULE_TAGS := optional LOCAL_SRC_FILES := \ diff --git a/firmware/os/algos/calibration/accelerometer/accel_cal.c b/firmware/os/algos/calibration/accelerometer/accel_cal.c index d1716386..c955f3ed 100644 --- a/firmware/os/algos/calibration/accelerometer/accel_cal.c +++ b/firmware/os/algos/calibration/accelerometer/accel_cal.c @@ -16,7 +16,6 @@ #include "calibration/accelerometer/accel_cal.h" -#include <errno.h> #include <inttypes.h> #include <math.h> #include <stdio.h> @@ -40,7 +39,10 @@ #define MAX_OFF 0.1f // Will not accept offsets that are larger than 100 mg. #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 TEMP_CUT \ + ((MAX_TEMP - MIN_TEMP) / \ + ACCEL_CAL_NUM_TEMP_WINDOWS) // Separation window size for temperature buckets + // in degrees C. #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. @@ -179,13 +181,14 @@ bool accelCalNewBiasAvailable(struct AccelCal *acc) { // Accel cal init. void accelCalInit(struct AccelCal *acc, const struct AccelCalParameters *parameters) { - // Init core accel data. - 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); + int i; + + for (i = 0; i < ACCEL_CAL_NUM_TEMP_WINDOWS; ++i) { + // Init core accel data. + accelCalAlgoInit(&acc->ac1[i], parameters->fx, parameters->fxb, + parameters->fy, parameters->fyb, parameters->fz, + parameters->fzb, parameters->fle); + } // Stillness Reset. accelStillInit(&acc->asd, parameters->t0, parameters->n_s, parameters->th); @@ -483,12 +486,7 @@ void accelCalRun(struct AccelCal *acc, uint64_t sample_time_nanos, float x, accelTempHisto(&acc->adf, temp); #endif - // Two temp buckets. - if (temp < TEMP_CUT) { - temp_gate = 0; - } else { - temp_gate = 1; - } + temp_gate = (int) ((temp - MIN_TEMP) / TEMP_CUT); #ifdef ACCEL_CAL_DBG_ENABLED accelStatsCounter(&acc->asd, &acc->adf); #endif diff --git a/firmware/os/algos/calibration/accelerometer/accel_cal.h b/firmware/os/algos/calibration/accelerometer/accel_cal.h index 3324875d..ad7de9e7 100644 --- a/firmware/os/algos/calibration/accelerometer/accel_cal.h +++ b/firmware/os/algos/calibration/accelerometer/accel_cal.h @@ -27,6 +27,7 @@ #include <stdint.h> #include <sys/types.h> + #include "common/math/kasa.h" #include "common/math/mat.h" diff --git a/firmware/os/algos/calibration/diversity_checker/diversity_checker.c b/firmware/os/algos/calibration/diversity_checker/diversity_checker.c index 3fab81f8..d9de89ab 100644 --- a/firmware/os/algos/calibration/diversity_checker/diversity_checker.c +++ b/firmware/os/algos/calibration/diversity_checker/diversity_checker.c @@ -16,17 +16,17 @@ #include "calibration/diversity_checker/diversity_checker.h" -#include <errno.h> #include <stdarg.h> #include <stdio.h> #include <string.h> #include "common/math/vec.h" +#include "chre/util/nanoapp/assert.h" // Struct initialization. void diversityCheckerInit(struct DiversityChecker* diverse_data, const struct DiversityCheckerParameters* parameters) { - ASSERT_NOT_NULL(diverse_data); + CHRE_ASSERT_NOT_NULL(diverse_data); // Initialize parameters. diverse_data->threshold_tuning_param_sq = @@ -61,7 +61,7 @@ void diversityCheckerInit(struct DiversityChecker* diverse_data, // Reset void diversityCheckerReset(struct DiversityChecker* diverse_data) { - ASSERT_NOT_NULL(diverse_data); + CHRE_ASSERT_NOT_NULL(diverse_data); // Clear data memory. memset(&diverse_data->diverse_data, 0, sizeof(diverse_data->diverse_data)); @@ -109,7 +109,7 @@ bool diversityCheckerFindNearestPoint(struct DiversityChecker* diverse_data, void diversityCheckerUpdate(struct DiversityChecker* diverse_data, float x, float y, float z) { - ASSERT_NOT_NULL(diverse_data); + CHRE_ASSERT_NOT_NULL(diverse_data); // If memory is full, no need to run through the data. if (!diverse_data->data_full) { @@ -138,7 +138,7 @@ void diversityCheckerUpdate(struct DiversityChecker* diverse_data, float x, bool diversityCheckerNormQuality(struct DiversityChecker* diverse_data, float x_bias, float y_bias, float z_bias) { - ASSERT_NOT_NULL(diverse_data); + CHRE_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 >= diff --git a/firmware/os/algos/calibration/magnetometer/mag_cal/mag_cal.c b/firmware/os/algos/calibration/magnetometer/mag_cal/mag_cal.c index fda5170a..1948df32 100644 --- a/firmware/os/algos/calibration/magnetometer/mag_cal/mag_cal.c +++ b/firmware/os/algos/calibration/magnetometer/mag_cal/mag_cal.c @@ -16,7 +16,6 @@ #include "calibration/magnetometer/mag_cal/mag_cal.h" -#include <errno.h> #include <inttypes.h> #include <string.h> diff --git a/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc b/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc index a8daaeb8..122af489 100644 --- a/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc +++ b/firmware/os/algos/calibration/nano_calibration/nano_calibration.cc @@ -19,7 +19,7 @@ #include <cstdint> #include <cstring> -#include "chre/util/nanoapp/log.h" +#include "common/techeng_log_util.h" namespace nano_calibration { namespace { @@ -35,16 +35,15 @@ constexpr char kMagTag[] = {"[NanoSensorCal:MAG_UT]"}; // messages will be produced at a rate determined by // 'slow_message_interval_min'. struct LogMessageRegimen { - uint8_t rapid_message_interval_sec; // Assists device verification. - uint8_t slow_message_interval_min; // Avoids long-term log spam. + uint8_t rapid_message_interval_sec; // Assists device verification. + uint8_t slow_message_interval_min; // Avoids long-term log spam. uint8_t duration_of_rapid_messages_min; }; constexpr LogMessageRegimen kGyroscopeMessagePlan = { /*rapid_message_interval_sec*/ 20, /*slow_message_interval_min*/ 5, - /*duration_of_rapid_messages_min*/ 3 -}; + /*duration_of_rapid_messages_min*/ 3}; using ::online_calibration::CalibrationDataThreeAxis; using ::online_calibration::CalibrationTypeFlags; @@ -58,18 +57,22 @@ using ::online_calibration::SensorType; #endif #ifdef NANO_SENSOR_CAL_DBG_ENABLED -#define NANO_CAL_LOGD(tag, format, ...) LOGD("%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__) +#define NANO_CAL_LOGD(tag, format, ...) \ + TECHENG_LOGD("%s " format, tag, ##__VA_ARGS__) +#define NANO_CAL_LOGW(tag, format, ...) \ + TECHENG_LOGW("%s " format, tag, ##__VA_ARGS__) +#define NANO_CAL_LOGE(tag, format, ...) \ + TECHENG_LOGE("%s " format, tag, ##__VA_ARGS__) #else -#define NANO_CAL_LOGD(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__) +#define NANO_CAL_LOGD(tag, format, ...) techeng_log_null(format, ##__VA_ARGS__) +#define NANO_CAL_LOGW(tag, format, ...) techeng_log_null(format, ##__VA_ARGS__) +#define NANO_CAL_LOGE(tag, format, ...) techeng_log_null(format, ##__VA_ARGS__) #endif // NANO_SENSOR_CAL_DBG_ENABLED // NOTE: LOGI is defined to ensure calibration updates are always logged for // field diagnosis and verification. -#define NANO_CAL_LOGI(tag, format, ...) LOGI("%s " format, tag, ##__VA_ARGS__) +#define NANO_CAL_LOGI(tag, format, ...) \ + TECHENG_LOGI("%s " format, tag, ##__VA_ARGS__) } // namespace @@ -199,6 +202,12 @@ void NanoSensorCal::ProcessSample(const SensorData &sample) { accel_cal_update_flags_, kAccelTag); PrintCalibration(accel_cal_->GetSensorCalibration(), accel_cal_update_flags_, kAccelTag); + + if (result_callback_ != nullptr) { + result_callback_->SetCalibrationEvent(sample.timestamp_nanos, + SensorType::kAccelerometerMps2, + accel_cal_update_flags_); + } } } @@ -210,7 +219,19 @@ void NanoSensorCal::ProcessSample(const SensorData &sample) { if (NotifyAshCalibration(CHRE_SENSOR_TYPE_GYROSCOPE, gyro_cal_->GetSensorCalibration(), gyro_cal_update_flags_, kGyroTag)) { - HandleGyroLogMessage(sample.timestamp_nanos); + const bool print_gyro_log = + HandleGyroLogMessage(sample.timestamp_nanos); + + if (result_callback_ != nullptr && + (print_gyro_log || + gyro_cal_update_flags_ != CalibrationTypeFlags::BIAS)) { + // Rate-limits OTC gyro telemetry updates since they can happen + // frequently with temperature change. However, all GyroCal stillness + // and OTC model parameter updates will be recorded. + result_callback_->SetCalibrationEvent(sample.timestamp_nanos, + SensorType::kGyroscopeRps, + gyro_cal_update_flags_); + } } } } @@ -224,6 +245,12 @@ void NanoSensorCal::ProcessSample(const SensorData &sample) { mag_cal_update_flags_, kMagTag); PrintCalibration(mag_cal_->GetSensorCalibration(), mag_cal_update_flags_, kMagTag); + + if (result_callback_ != nullptr) { + result_callback_->SetCalibrationEvent(sample.timestamp_nanos, + SensorType::kMagnetometerUt, + mag_cal_update_flags_); + } } } } @@ -294,7 +321,7 @@ bool NanoSensorCal::NotifyAshCalibration( bool NanoSensorCal::LoadAshCalibration(uint8_t chreSensorType, OnlineCalibrationThreeAxis *online_cal, - CalibrationTypeFlags* flags, + CalibrationTypeFlags *flags, const char *sensor_tag) { ashCalParams recalled_ash_cal_parameters; if (ashLoadCalibrationParams(chreSensorType, ASH_CAL_STORAGE_ASH, @@ -443,7 +470,7 @@ void NanoSensorCal::PrintCalibration(const CalibrationDataThreeAxis &cal_data, } } -void NanoSensorCal::HandleGyroLogMessage(uint64_t timestamp_nanos) { +bool NanoSensorCal::HandleGyroLogMessage(uint64_t timestamp_nanos) { // Limits the log messaging update rate for the gyro calibrations since // these can occur frequently with rapid temperature changes. const int64_t next_log_interval_nanos = @@ -454,14 +481,15 @@ void NanoSensorCal::HandleGyroLogMessage(uint64_t timestamp_nanos) { : SEC_TO_NANOS(kGyroscopeMessagePlan.rapid_message_interval_sec); const bool print_gyro_log = NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA( - timestamp_nanos, gyro_notification_time_nanos_, - next_log_interval_nanos); + timestamp_nanos, gyro_notification_time_nanos_, next_log_interval_nanos); if (print_gyro_log) { gyro_notification_time_nanos_ = timestamp_nanos; PrintCalibration(gyro_cal_->GetSensorCalibration(), gyro_cal_update_flags_, kGyroTag); } + + return print_gyro_log; } } // 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 index 5dad0f68..82a8396d 100644 --- a/firmware/os/algos/calibration/nano_calibration/nano_calibration.h +++ b/firmware/os/algos/calibration/nano_calibration/nano_calibration.h @@ -42,16 +42,17 @@ #ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_NANO_CALIBRATION_NANO_CALIBRATION_H_ #define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_NANO_CALIBRATION_NANO_CALIBRATION_H_ +#include <ash.h> +#include <chre.h> #include <stdbool.h> #include <stdint.h> -#include <ash.h> -#include <chre.h> #include <cstdint> #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/result_callback_interface.h" #include "calibration/online_calibration/common_data/sensor_data.h" #include "common/math/macros.h" @@ -88,6 +89,11 @@ class NanoSensorCal { void HandleTemperatureSamples(uint16_t event_type, const chreSensorFloatData *event_data); + void set_result_callback( + online_calibration::ResultCallbackInterface *result_callback) { + result_callback_ = result_callback; + } + private: // Passes sensor data to the runtime calibration algorithms. void ProcessSample(const online_calibration::SensorData &sample); @@ -99,7 +105,7 @@ class NanoSensorCal { // which runtime calibration parameters were recalled. bool LoadAshCalibration(uint8_t chreSensorType, OnlineCalibrationThreeAxis *online_cal, - online_calibration::CalibrationTypeFlags* flags, + online_calibration::CalibrationTypeFlags *flags, const char *sensor_tag); // Provides sensor calibration updates using the ASH API for the specified @@ -126,7 +132,7 @@ class NanoSensorCal { const online_calibration::CalibrationDataThreeAxis &cal_data, online_calibration::CalibrationTypeFlags flags, const char *sensor_tag); - void HandleGyroLogMessage(uint64_t timestamp_nanos); + bool HandleGyroLogMessage(uint64_t timestamp_nanos); // Pointer to the accelerometer runtime calibration object. OnlineCalibrationThreeAxis *accel_cal_ = nullptr; @@ -154,6 +160,9 @@ class NanoSensorCal { online_calibration::CalibrationTypeFlags::NONE; online_calibration::CalibrationTypeFlags mag_cal_update_flags_ = online_calibration::CalibrationTypeFlags::NONE; + + // Pointer to telemetry logger. + online_calibration::ResultCallbackInterface *result_callback_ = nullptr; }; } // namespace nano_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 index 7d66f756..f21108e2 100644 --- a/firmware/os/algos/calibration/online_calibration/common_data/calibration_data.h +++ b/firmware/os/algos/calibration/online_calibration/common_data/calibration_data.h @@ -48,6 +48,12 @@ namespace online_calibration { * behavior with temperature (e.g., linear bias sensitivity * model). * QUALITY_DEGRADED - Indicates a degradation in calibration quality. + * OTC_STILL_BIAS - Indicates that a stillness-induced bias update occurred as + * an input to the over-temperature compensation algorithm + * NOTE: Stillness bias values (e.g., GyroCal) may be + * different from the OTC bias. If these bias value are + * desired, they should be retrieved directly (see related + * calibration wrappers for access [e.g., GyroOffsetOtcCal]). */ enum class CalibrationTypeFlags : uint8_t { NONE = 0x00, @@ -56,6 +62,7 @@ enum class CalibrationTypeFlags : uint8_t { CROSS_AXIS = 0x04, OVER_TEMP = 0x08, QUALITY_DEGRADED = 0x10, + OTC_STILL_BIAS = 0x20, ALL = 0xFF }; diff --git a/firmware/os/algos/calibration/online_calibration/common_data/result_callback_interface.h b/firmware/os/algos/calibration/online_calibration/common_data/result_callback_interface.h new file mode 100644 index 00000000..ca54f2fb --- /dev/null +++ b/firmware/os/algos/calibration/online_calibration/common_data/result_callback_interface.h @@ -0,0 +1,32 @@ +#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_RESULT_CALLBACK_INTERFACE_H_ +#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_RESULT_CALLBACK_INTERFACE_H_ + +#include "calibration/online_calibration/common_data/calibration_data.h" +#include "calibration/online_calibration/common_data/sensor_data.h" + +namespace online_calibration { + +// Interface for a results callback implementation (useful for building +// calibration event loggers). +class ResultCallbackInterface { + protected: + // Protected destructor. The implementation can destroy itself, it can't be + // destroyed through this interface. + virtual ~ResultCallbackInterface() = default; + + public: + // Sets a calibration event, such as a magnetometer calibration event. + // + // event_timestamp_nanos: Timestamp in nanoseconds of when the calibration + // event was produced in the sensor timebase. + // sensor_type: Which sensor the calibration was produced for. + // flags: What kind of update the calibration was, e.g. offset, quality + // degradation (like a magnetization event), over temperature, etc. + virtual void SetCalibrationEvent(uint64_t event_timestamp_nanos, + SensorType sensor_type, + CalibrationTypeFlags flags) = 0; +}; + +} // namespace online_calibration + +#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_ONLINE_CALIBRATION_COMMON_DATA_RESULT_CALLBACK_INTERFACE_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 index d73ed3b4..02c26709 100644 --- 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 @@ -71,6 +71,7 @@ CalibrationTypeFlags GyroOffsetOtcCal::SetMeasurement( } // Checks for a new calibration, and updates the OTC. + CalibrationTypeFlags cal_update_callback_flags = CalibrationTypeFlags::NONE; if (gyroCalNewBiasAvailable(&gyro_cal_)) { float offset[3]; float temperature_celsius = kInvalidTemperatureCelsius; @@ -79,6 +80,7 @@ CalibrationTypeFlags GyroOffsetOtcCal::SetMeasurement( &temperature_celsius, &calibration_time_nanos); overTempCalUpdateSensorEstimate(&over_temp_cal_, calibration_time_nanos, offset, temperature_celsius); + cal_update_callback_flags |= CalibrationTypeFlags::OTC_STILL_BIAS; } // Checks the OTC for a new calibration model update. @@ -89,7 +91,6 @@ CalibrationTypeFlags GyroOffsetOtcCal::SetMeasurement( 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); @@ -111,7 +112,7 @@ CalibrationTypeFlags GyroOffsetOtcCal::SetMeasurement( // 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) { + if (cal_update_callback_flags != CalibrationTypeFlags::NONE) { cal_data_.calibration_quality.level = CalibrationQualityLevel::HIGH_QUALITY; cal_data_.calibration_quality.value = kHighQualityRps; cal_update_polling_flags_ |= cal_update_callback_flags; 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 6f887f78..782dce93 100644 --- a/firmware/os/algos/calibration/over_temp/over_temp_cal.c +++ b/firmware/os/algos/calibration/over_temp/over_temp_cal.c @@ -25,7 +25,7 @@ #include "calibration/util/cal_log.h" #endif // OVERTEMPCAL_DBG_ENABLED || OVERTEMPCAL_DBG_LOG_TEMP -#include "util/nano_assert.h" +#include "chre/util/nanoapp/assert.h" /////// DEFINITIONS AND MACROS //////////////////////////////////////////////// @@ -256,7 +256,7 @@ static void createDebugTag(struct OverTempCal *over_temp_cal, void overTempCalInit(struct OverTempCal *over_temp_cal, const struct OverTempCalParameters *parameters) { - ASSERT_NOT_NULL(over_temp_cal); + CHRE_ASSERT_NOT_NULL(over_temp_cal); // Clears OverTempCal memory. memset(over_temp_cal, 0, sizeof(struct OverTempCal)); @@ -312,10 +312,10 @@ void overTempCalSetModel(struct OverTempCal *over_temp_cal, const float *offset, float offset_temp_celsius, uint64_t timestamp_nanos, const float *temp_sensitivity, const float *sensor_intercept, bool jump_start_model) { - ASSERT_NOT_NULL(over_temp_cal); - ASSERT_NOT_NULL(offset); - ASSERT_NOT_NULL(temp_sensitivity); - ASSERT_NOT_NULL(sensor_intercept); + CHRE_ASSERT_NOT_NULL(over_temp_cal); + CHRE_ASSERT_NOT_NULL(offset); + CHRE_ASSERT_NOT_NULL(temp_sensitivity); + CHRE_ASSERT_NOT_NULL(sensor_intercept); // Initializes the OTC linear model parameters. resetOtcLinearModel(over_temp_cal); @@ -415,12 +415,12 @@ void overTempCalSetModel(struct OverTempCal *over_temp_cal, const float *offset, void overTempCalGetModel(struct OverTempCal *over_temp_cal, float *offset, float *offset_temp_celsius, uint64_t *timestamp_nanos, float *temp_sensitivity, float *sensor_intercept) { - ASSERT_NOT_NULL(over_temp_cal); - ASSERT_NOT_NULL(offset); - ASSERT_NOT_NULL(offset_temp_celsius); - ASSERT_NOT_NULL(timestamp_nanos); - ASSERT_NOT_NULL(temp_sensitivity); - ASSERT_NOT_NULL(sensor_intercept); + CHRE_ASSERT_NOT_NULL(over_temp_cal); + CHRE_ASSERT_NOT_NULL(offset); + CHRE_ASSERT_NOT_NULL(offset_temp_celsius); + CHRE_ASSERT_NOT_NULL(timestamp_nanos); + CHRE_ASSERT_NOT_NULL(temp_sensitivity); + CHRE_ASSERT_NOT_NULL(sensor_intercept); // Gets the latest over-temp calibration model data. memcpy(temp_sensitivity, over_temp_cal->temp_sensitivity, @@ -436,8 +436,8 @@ void overTempCalGetModel(struct OverTempCal *over_temp_cal, float *offset, void overTempCalSetModelData(struct OverTempCal *over_temp_cal, size_t data_length, uint64_t timestamp_nanos, const struct OverTempModelThreeAxis *model_data) { - ASSERT_NOT_NULL(over_temp_cal); - ASSERT_NOT_NULL(model_data); + CHRE_ASSERT_NOT_NULL(over_temp_cal); + CHRE_ASSERT_NOT_NULL(model_data); // Load only "good" data from the input 'model_data'. over_temp_cal->num_model_pts = NANO_MIN(data_length, OTC_MODEL_SIZE); @@ -488,7 +488,7 @@ void overTempCalSetModelData(struct OverTempCal *over_temp_cal, void overTempCalGetModelData(struct OverTempCal *over_temp_cal, size_t *data_length, struct OverTempModelThreeAxis *model_data) { - ASSERT_NOT_NULL(over_temp_cal); + CHRE_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 OverTempModelThreeAxis)); @@ -506,10 +506,10 @@ void overTempCalGetOffset(struct OverTempCal *over_temp_cal, void overTempCalRemoveOffset(struct OverTempCal *over_temp_cal, uint64_t timestamp_nanos, float xi, float yi, float zi, float *xo, float *yo, float *zo) { - ASSERT_NOT_NULL(over_temp_cal); - ASSERT_NOT_NULL(xo); - ASSERT_NOT_NULL(yo); - ASSERT_NOT_NULL(zo); + CHRE_ASSERT_NOT_NULL(over_temp_cal); + CHRE_ASSERT_NOT_NULL(xo); + CHRE_ASSERT_NOT_NULL(yo); + CHRE_ASSERT_NOT_NULL(zo); // Determines whether over-temp compensation will be applied. if (over_temp_cal->over_temp_enable) { @@ -526,7 +526,7 @@ void overTempCalRemoveOffset(struct OverTempCal *over_temp_cal, } bool overTempCalNewModelUpdateAvailable(struct OverTempCal *over_temp_cal) { - ASSERT_NOT_NULL(over_temp_cal); + CHRE_ASSERT_NOT_NULL(over_temp_cal); const bool update_available = over_temp_cal->new_overtemp_model_available && over_temp_cal->over_temp_enable; @@ -537,7 +537,7 @@ bool overTempCalNewModelUpdateAvailable(struct OverTempCal *over_temp_cal) { } bool overTempCalNewOffsetAvailable(struct OverTempCal *over_temp_cal) { - ASSERT_NOT_NULL(over_temp_cal); + CHRE_ASSERT_NOT_NULL(over_temp_cal); const bool update_available = over_temp_cal->new_overtemp_offset_available && over_temp_cal->over_temp_enable; @@ -551,9 +551,9 @@ void overTempCalUpdateSensorEstimate(struct OverTempCal *over_temp_cal, uint64_t timestamp_nanos, const float *offset, float temperature_celsius) { - ASSERT_NOT_NULL(over_temp_cal); - ASSERT_NOT_NULL(offset); - ASSERT(over_temp_cal->delta_temp_per_bin > 0); + CHRE_ASSERT_NOT_NULL(over_temp_cal); + CHRE_ASSERT_NOT_NULL(offset); + CHRE_ASSERT(over_temp_cal->delta_temp_per_bin > 0); // Updates the age of each OTC model data point. modelDataSetAgeUpdate(over_temp_cal, timestamp_nanos); @@ -687,7 +687,7 @@ void overTempCalUpdateSensorEstimate(struct OverTempCal *over_temp_cal, void overTempCalSetTemperature(struct OverTempCal *over_temp_cal, uint64_t timestamp_nanos, float temperature_celsius) { - ASSERT_NOT_NULL(over_temp_cal); + CHRE_ASSERT_NOT_NULL(over_temp_cal); #ifdef OVERTEMPCAL_DBG_ENABLED #ifdef OVERTEMPCAL_DBG_LOG_TEMP @@ -752,8 +752,8 @@ void overTempCalSetTemperature(struct OverTempCal *over_temp_cal, bool overTempValidateAndSetWeight( struct OverTempCal *over_temp_cal, size_t index, const struct OverTempCalWeight *new_otc_weight) { - ASSERT_NOT_NULL(over_temp_cal); - ASSERT_NOT_NULL(new_otc_weight); + CHRE_ASSERT_NOT_NULL(over_temp_cal); + CHRE_ASSERT_NOT_NULL(new_otc_weight); // The input weighting coefficient must be positive. if (new_otc_weight->weight <= 0.0f) { @@ -784,7 +784,7 @@ bool overTempValidateAndSetWeight( void compensateWithLinearModel(struct OverTempCal *over_temp_cal, uint64_t timestamp_nanos, float temperature_celsius) { - ASSERT_NOT_NULL(over_temp_cal); + CHRE_ASSERT_NOT_NULL(over_temp_cal); // Defaults to using the current compensated offset value. float compensated_offset[3]; @@ -811,8 +811,8 @@ void compensateWithLinearModel(struct OverTempCal *over_temp_cal, void addLinearTemperatureExtrapolation(struct OverTempCal *over_temp_cal, float *compensated_offset, float delta_temp_celsius) { - ASSERT_NOT_NULL(over_temp_cal); - ASSERT_NOT_NULL(compensated_offset); + CHRE_ASSERT_NOT_NULL(over_temp_cal); + CHRE_ASSERT_NOT_NULL(compensated_offset); // Adds a delta term to the 'compensated_offset' using the temperature // difference defined by 'delta_temp_celsius'. @@ -830,8 +830,8 @@ 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); + CHRE_ASSERT_NOT_NULL(over_temp_cal); + CHRE_ASSERT_NOT_NULL(estimate); // Uses the most recent offset estimate for offset compensation. float compensated_offset[3]; @@ -857,8 +857,8 @@ void compareAndCompensateWithNearest(struct OverTempCal *over_temp_cal, uint64_t timestamp_nanos, float temperature_celsius, bool compare_to_linear_model) { - ASSERT_NOT_NULL(over_temp_cal); - ASSERT_NOT_NULL(over_temp_cal->nearest_offset); + CHRE_ASSERT_NOT_NULL(over_temp_cal); + CHRE_ASSERT_NOT_NULL(over_temp_cal->nearest_offset); // The default compensated offset is the nearest-temperature offset vector. float compensated_offset[3]; @@ -914,7 +914,7 @@ void compareAndCompensateWithNearest(struct OverTempCal *over_temp_cal, void updateCalOffset(struct OverTempCal *over_temp_cal, uint64_t timestamp_nanos, float temperature_celsius) { - ASSERT_NOT_NULL(over_temp_cal); + CHRE_ASSERT_NOT_NULL(over_temp_cal); // If 'temperature_celsius' is invalid, then no changes to the compensated // offset are computed. @@ -1050,8 +1050,8 @@ void updateCalOffset(struct OverTempCal *over_temp_cal, void setCompensatedOffset(struct OverTempCal *over_temp_cal, const float *compensated_offset, uint64_t timestamp_nanos, float temperature_celsius) { - ASSERT_NOT_NULL(over_temp_cal); - ASSERT_NOT_NULL(compensated_offset); + CHRE_ASSERT_NOT_NULL(over_temp_cal); + CHRE_ASSERT_NOT_NULL(compensated_offset); // If the 'compensated_offset' value has changed significantly, then set // 'new_overtemp_offset_available' true. @@ -1077,8 +1077,8 @@ void setCompensatedOffset(struct OverTempCal *over_temp_cal, void setLatestEstimate(struct OverTempCal *over_temp_cal, const float *offset, float offset_temp_celsius) { - ASSERT_NOT_NULL(over_temp_cal); - ASSERT_NOT_NULL(offset); + CHRE_ASSERT_NOT_NULL(over_temp_cal); + CHRE_ASSERT_NOT_NULL(offset); if (over_temp_cal->latest_offset != NULL) { // Sets the latest over-temp calibration estimate. @@ -1091,7 +1091,7 @@ void setLatestEstimate(struct OverTempCal *over_temp_cal, const float *offset, void refreshOtcModel(struct OverTempCal *over_temp_cal, uint64_t timestamp_nanos) { - ASSERT_NOT_NULL(over_temp_cal); + CHRE_ASSERT_NOT_NULL(over_temp_cal); if (NANO_TIMER_CHECK_T1_GEQUAL_T2_PLUS_DELTA( timestamp_nanos, over_temp_cal->last_model_update_nanos, OTC_REFRESH_MODEL_NANOS)) { @@ -1109,7 +1109,7 @@ void refreshOtcModel(struct OverTempCal *over_temp_cal, void computeModelUpdate(struct OverTempCal *over_temp_cal, uint64_t timestamp_nanos) { - ASSERT_NOT_NULL(over_temp_cal); + CHRE_ASSERT_NOT_NULL(over_temp_cal); // Ensures that the minimum number of points required for a model fit has been // satisfied. @@ -1171,7 +1171,7 @@ void computeModelUpdate(struct OverTempCal *over_temp_cal, void findNearestEstimate(struct OverTempCal *over_temp_cal, float temperature_celsius) { - ASSERT_NOT_NULL(over_temp_cal); + CHRE_ASSERT_NOT_NULL(over_temp_cal); // If 'temperature_celsius' is invalid, then do not search. if (temperature_celsius <= INVALID_TEMPERATURE_CELSIUS) { @@ -1195,7 +1195,7 @@ void findNearestEstimate(struct OverTempCal *over_temp_cal, void removeStaleModelData(struct OverTempCal *over_temp_cal, uint64_t timestamp_nanos) { - ASSERT_NOT_NULL(over_temp_cal); + CHRE_ASSERT_NOT_NULL(over_temp_cal); bool removed_one = false; for (size_t i = 0; i < over_temp_cal->num_model_pts; i++) { @@ -1223,7 +1223,7 @@ void removeStaleModelData(struct OverTempCal *over_temp_cal, bool removeModelDataByIndex(struct OverTempCal *over_temp_cal, size_t model_index) { - ASSERT_NOT_NULL(over_temp_cal); + CHRE_ASSERT_NOT_NULL(over_temp_cal); // This function will not remove all of the model data. At least one model // sample will be left. @@ -1263,8 +1263,8 @@ bool removeModelDataByIndex(struct OverTempCal *over_temp_cal, } bool jumpStartModelData(struct OverTempCal *over_temp_cal) { - ASSERT_NOT_NULL(over_temp_cal); - ASSERT(over_temp_cal->delta_temp_per_bin > 0); + CHRE_ASSERT_NOT_NULL(over_temp_cal); + CHRE_ASSERT(over_temp_cal->delta_temp_per_bin > 0); // Prevent a divide by zero below. if (over_temp_cal->delta_temp_per_bin <= 0) { @@ -1321,10 +1321,10 @@ bool jumpStartModelData(struct OverTempCal *over_temp_cal) { void updateModel(const struct OverTempCal *over_temp_cal, float *temp_sensitivity, float *sensor_intercept) { - ASSERT_NOT_NULL(over_temp_cal); - ASSERT_NOT_NULL(temp_sensitivity); - ASSERT_NOT_NULL(sensor_intercept); - ASSERT(over_temp_cal->num_model_pts > 0); + CHRE_ASSERT_NOT_NULL(over_temp_cal); + CHRE_ASSERT_NOT_NULL(temp_sensitivity); + CHRE_ASSERT_NOT_NULL(sensor_intercept); + CHRE_ASSERT(over_temp_cal->num_model_pts > 0); float sw = 0.0f; float st = 0.0f, stt = 0.0f; @@ -1347,7 +1347,7 @@ void updateModel(const struct OverTempCal *over_temp_cal, } // Second pass computes the mean corrected second moment values. - ASSERT(sw > 0.0f); + CHRE_ASSERT(sw > 0.0f); const float inv_sw = 1.0f / sw; for (size_t i = 0; i < n; ++i) { weight = evaluateWeightingFunction( @@ -1362,7 +1362,7 @@ void updateModel(const struct OverTempCal *over_temp_cal, } // Calculates the linear model fit parameters. - ASSERT(stt > 0.0f); + CHRE_ASSERT(stt > 0.0f); const float inv_stt = 1.0f / stt; temp_sensitivity[0] = stsx * inv_stt; sensor_intercept[0] = (sx - st * temp_sensitivity[0]) * inv_sw; @@ -1374,8 +1374,8 @@ void updateModel(const struct OverTempCal *over_temp_cal, bool outlierCheck(struct OverTempCal *over_temp_cal, const float *offset, size_t axis_index, float temperature_celsius) { - ASSERT_NOT_NULL(over_temp_cal); - ASSERT_NOT_NULL(offset); + CHRE_ASSERT_NOT_NULL(over_temp_cal); + CHRE_ASSERT_NOT_NULL(offset); // If a model has been defined, then check to see if this offset could be a // potential outlier: @@ -1394,7 +1394,7 @@ bool outlierCheck(struct OverTempCal *over_temp_cal, const float *offset, } void resetOtcLinearModel(struct OverTempCal *over_temp_cal) { - ASSERT_NOT_NULL(over_temp_cal); + CHRE_ASSERT_NOT_NULL(over_temp_cal); // Sets the temperature sensitivity model parameters to // OTC_INITIAL_SENSITIVITY to indicate that the model is in an "initial" @@ -1420,7 +1420,7 @@ bool checkAndEnforceTemperatureRange(float *temperature_celsius) { bool isValidOtcLinearModel(const struct OverTempCal *over_temp_cal, float temp_sensitivity, float sensor_intercept) { - ASSERT_NOT_NULL(over_temp_cal); + CHRE_ASSERT_NOT_NULL(over_temp_cal); // Simple check to ensure that the linear model parameters are: // 1. Within the valid range, AND @@ -1432,7 +1432,7 @@ bool isValidOtcLinearModel(const struct OverTempCal *over_temp_cal, } bool isValidOtcOffset(const float *offset, float offset_temp_celsius) { - ASSERT_NOT_NULL(offset); + CHRE_ASSERT_NOT_NULL(offset); // Simple check to ensure that: // 1. All of the input data is non "zero". @@ -1450,7 +1450,7 @@ bool isValidOtcOffset(const float *offset, float offset_temp_celsius) { float evaluateWeightingFunction(const struct OverTempCal *over_temp_cal, uint64_t offset_age_nanos) { - ASSERT_NOT_NULL(over_temp_cal); + CHRE_ASSERT_NOT_NULL(over_temp_cal); for (size_t i = 0; i < OTC_NUM_WEIGHT_LEVELS; i++) { if (offset_age_nanos <= over_temp_cal->weighting_function[i].offset_age_nanos) { @@ -1464,7 +1464,7 @@ float evaluateWeightingFunction(const struct OverTempCal *over_temp_cal, void modelDataSetAgeUpdate(struct OverTempCal *over_temp_cal, uint64_t timestamp_nanos) { - ASSERT_NOT_NULL(over_temp_cal); + CHRE_ASSERT_NOT_NULL(over_temp_cal); if (over_temp_cal->last_age_update_nanos >= timestamp_nanos) { // Age updates must be monotonic. return; @@ -1496,7 +1496,7 @@ void createDebugTag(struct OverTempCal *over_temp_cal, } void updateDebugData(struct OverTempCal *over_temp_cal) { - ASSERT_NOT_NULL(over_temp_cal); + CHRE_ASSERT_NOT_NULL(over_temp_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 @@ -1550,7 +1550,7 @@ void updateDebugData(struct OverTempCal *over_temp_cal) { void overTempCalDebugPrint(struct OverTempCal *over_temp_cal, uint64_t timestamp_nanos) { - ASSERT_NOT_NULL(over_temp_cal); + CHRE_ASSERT_NOT_NULL(over_temp_cal); // This is a state machine that controls the reporting out of debug data. createDebugTag(over_temp_cal, ":REPORT]"); @@ -1739,9 +1739,9 @@ void overTempCalDebugDescriptors(struct OverTempCal *over_temp_cal, const char *otc_sensor_tag, const char *otc_unit_tag, float otc_unit_conversion) { - ASSERT_NOT_NULL(over_temp_cal); - ASSERT_NOT_NULL(otc_sensor_tag); - ASSERT_NOT_NULL(otc_unit_tag); + CHRE_ASSERT_NOT_NULL(over_temp_cal); + CHRE_ASSERT_NOT_NULL(otc_sensor_tag); + CHRE_ASSERT_NOT_NULL(otc_unit_tag); // Sets the sensor descriptor, displayed units, and unit conversion factor. strcpy(over_temp_cal->otc_sensor_tag, otc_sensor_tag); @@ -1752,10 +1752,10 @@ void overTempCalDebugDescriptors(struct OverTempCal *over_temp_cal, void overTempGetModelError(const struct OverTempCal *over_temp_cal, 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); - ASSERT_NOT_NULL(max_error); + CHRE_ASSERT_NOT_NULL(over_temp_cal); + CHRE_ASSERT_NOT_NULL(temp_sensitivity); + CHRE_ASSERT_NOT_NULL(sensor_intercept); + CHRE_ASSERT_NOT_NULL(max_error); float max_error_test; memset(max_error, 0, 3 * sizeof(float)); 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 index d1b66195..5ab7dd83 100644 --- a/firmware/os/algos/calibration/sample_rate_estimator/sample_rate_estimator.c +++ b/firmware/os/algos/calibration/sample_rate_estimator/sample_rate_estimator.c @@ -19,7 +19,7 @@ #include <string.h> #include "common/math/macros.h" -#include "util/nano_assert.h" +#include "chre/util/nanoapp/assert.h" // Helper function used to reset the sampling rate estimator accumulator. static void sampleRateEstimatorResetAccumulator( @@ -32,7 +32,7 @@ static void sampleRateEstimatorResetAccumulator( void sampleRateEstimatorInit(struct SampleRateEstimator* sample_rate_estimator, size_t num_intervals_to_collect, float max_interval_sec) { - ASSERT_NOT_NULL(sample_rate_estimator); + CHRE_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; 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 index f506eea5..5d965dd6 100644 --- a/firmware/os/algos/calibration/sample_rate_estimator/sample_rate_estimator.h +++ b/firmware/os/algos/calibration/sample_rate_estimator/sample_rate_estimator.h @@ -43,6 +43,7 @@ #define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_SAMPLE_RATE_ESTIMATOR_SAMPLE_RATE_ESTIMATOR_H_ #include <stdbool.h> +#include <stddef.h> #include <stdint.h> #include <sys/types.h> diff --git a/firmware/os/algos/calibration/sphere_fit/sphere_fit_calibration.c b/firmware/os/algos/calibration/sphere_fit/sphere_fit_calibration.c index 8729e0dd..4b3eb4aa 100644 --- a/firmware/os/algos/calibration/sphere_fit/sphere_fit_calibration.c +++ b/firmware/os/algos/calibration/sphere_fit/sphere_fit_calibration.c @@ -27,6 +27,7 @@ #include "common/math/mat.h" #include "common/math/vec.h" +#include "chre/util/nanoapp/assert.h" // FORWARD DECLARATIONS /////////////////////////////////////////////////////////////////////////////// @@ -45,8 +46,8 @@ static bool runCalibration(struct SphereFitCal *sphere_cal, void sphereFitInit(struct SphereFitCal *sphere_cal, const struct LmParams *lm_params, const size_t min_num_points_for_cal) { - ASSERT_NOT_NULL(sphere_cal); - ASSERT_NOT_NULL(lm_params); + CHRE_ASSERT_NOT_NULL(sphere_cal); + CHRE_ASSERT_NOT_NULL(lm_params); // Initialize LM solver. lmSolverInit(&sphere_cal->lm_solver, lm_params, @@ -64,7 +65,7 @@ void sphereFitInit(struct SphereFitCal *sphere_cal, } void sphereFitReset(struct SphereFitCal *sphere_cal) { - ASSERT_NOT_NULL(sphere_cal); + CHRE_ASSERT_NOT_NULL(sphere_cal); // Set state to default (diagonal scale matrix and zero offset). memset(&sphere_cal->x0[0], 0, sizeof(float) * SF_STATE_DIM); @@ -79,8 +80,8 @@ void sphereFitReset(struct SphereFitCal *sphere_cal) { void sphereFitSetSolverData(struct SphereFitCal *sphere_cal, struct LmData *lm_data) { - ASSERT_NOT_NULL(sphere_cal); - ASSERT_NOT_NULL(lm_data); + CHRE_ASSERT_NOT_NULL(sphere_cal); + CHRE_ASSERT_NOT_NULL(lm_data); // Set solver data. lmSolverSetData(&sphere_cal->lm_solver, lm_data); @@ -89,8 +90,8 @@ void sphereFitSetSolverData(struct SphereFitCal *sphere_cal, bool sphereFitRunCal(struct SphereFitCal *sphere_cal, const struct SphereFitData *data, uint64_t timestamp_nanos) { - ASSERT_NOT_NULL(sphere_cal); - ASSERT_NOT_NULL(data); + CHRE_ASSERT_NOT_NULL(sphere_cal); + CHRE_ASSERT_NOT_NULL(data); // Run calibration if have enough points. if (data->num_fit_points >= sphere_cal->min_points_for_cal) { @@ -102,7 +103,7 @@ bool sphereFitRunCal(struct SphereFitCal *sphere_cal, void sphereFitSetInitialBias(struct SphereFitCal *sphere_cal, const float initial_bias[THREE_AXIS_DIM]) { - ASSERT_NOT_NULL(sphere_cal); + CHRE_ASSERT_NOT_NULL(sphere_cal); sphere_cal->x0[eParamOffset1] = initial_bias[0]; sphere_cal->x0[eParamOffset2] = initial_bias[1]; sphere_cal->x0[eParamOffset3] = initial_bias[2]; @@ -110,23 +111,23 @@ void sphereFitSetInitialBias(struct SphereFitCal *sphere_cal, void sphereFitGetLatestCal(const struct SphereFitCal *sphere_cal, struct ThreeAxisCalData *cal_data) { - ASSERT_NOT_NULL(sphere_cal); - ASSERT_NOT_NULL(cal_data); + CHRE_ASSERT_NOT_NULL(sphere_cal); + CHRE_ASSERT_NOT_NULL(cal_data); convertStateToCalStruct(sphere_cal->x, cal_data); cal_data->calibration_time_nanos = sphere_cal->estimate_time_nanos; } void sphereFitResidAndJacobianFunc(const float *state, const void *f_data, float *residual, float *jacobian) { - ASSERT_NOT_NULL(state); - ASSERT_NOT_NULL(f_data); - ASSERT_NOT_NULL(residual); + CHRE_ASSERT_NOT_NULL(state); + CHRE_ASSERT_NOT_NULL(f_data); + CHRE_ASSERT_NOT_NULL(residual); 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; - ASSERT(data->expected_norm > MIN_VALID_DATA_NORM); + CHRE_ASSERT(data->expected_norm > MIN_VALID_DATA_NORM); if (data->expected_norm > MIN_VALID_DATA_NORM) { expected_norm = data->expected_norm; } diff --git a/firmware/os/algos/calibration/util/cal_log.h b/firmware/os/algos/calibration/util/cal_log.h index 57179a7e..2f13151c 100644 --- a/firmware/os/algos/calibration/util/cal_log.h +++ b/firmware/os/algos/calibration/util/cal_log.h @@ -42,6 +42,10 @@ # include <chre.h> # define CAL_DEBUG_LOG(tag, fmt, ...) \ chreLog(CHRE_LOG_INFO, "%s " fmt, tag, ##__VA_ARGS__) +#elif ROHAN_DEBUG_LOG +# include "caraway/logging.h" +# define CAL_DEBUG_LOG(tag, format, ...) \ + LOG_DEBUG("%s " format, tag, ##__VA_ARGS__) #else // CHRE/SLPI Nanoapp Logging. # include "chre/util/nanoapp/log.h" diff --git a/firmware/os/algos/common/math/kasa.c b/firmware/os/algos/common/math/kasa.c index a24a31bf..911afba2 100644 --- a/firmware/os/algos/common/math/kasa.c +++ b/firmware/os/algos/common/math/kasa.c @@ -6,6 +6,7 @@ #include "common/math/mat.h" void kasaReset(struct KasaFit *kasa) { + kasa->acc_mean_x = kasa->acc_mean_y = kasa->acc_mean_z = 0.0f; 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; @@ -16,6 +17,21 @@ void kasaReset(struct KasaFit *kasa) { void kasaInit(struct KasaFit *kasa) { kasaReset(kasa); } void kasaAccumulate(struct KasaFit *kasa, float x, float y, float z) { + // KASA fit runs into numerical accuracy issues for large offset and small + // radii. Assuming that all points are on an sphere we can substract the + // first x,y,z value from all incoming data, making sure that the sphere will + // always go through 0,0,0 ensuring the highest possible numerical accuracy. + if (kasa->nsamples == 0) { + kasa->acc_mean_x = x; + kasa->acc_mean_y = y; + kasa->acc_mean_z = z; + } + + x = x - kasa->acc_mean_x; + y = y - kasa->acc_mean_y; + z = z - kasa->acc_mean_z; + + // Accumulation. float w = x * x + y * y + z * z; kasa->acc_x += x; @@ -108,7 +124,10 @@ int kasaFit(struct KasaFit *kasa, struct Vec3 *bias, float *radius, 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); + // Need to correct the bias with the first sample, which was used to shift + // the sphere in order to have best accuracy. + initVec3(bias, v.x + kasa->acc_mean_x, v.y + kasa->acc_mean_y, + v.z + kasa->acc_mean_z); *radius = r; int success = 0; diff --git a/firmware/os/algos/common/math/kasa.h b/firmware/os/algos/common/math/kasa.h index e9652d60..d3504b6f 100644 --- a/firmware/os/algos/common/math/kasa.h +++ b/firmware/os/algos/common/math/kasa.h @@ -20,6 +20,7 @@ extern "C" { #endif struct KasaFit { + float acc_mean_x, acc_mean_y, acc_mean_z; 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; diff --git a/firmware/os/algos/common/math/levenberg_marquardt.c b/firmware/os/algos/common/math/levenberg_marquardt.c index 89986808..2957253e 100644 --- a/firmware/os/algos/common/math/levenberg_marquardt.c +++ b/firmware/os/algos/common/math/levenberg_marquardt.c @@ -7,6 +7,7 @@ #include "common/math/macros.h" #include "common/math/mat.h" #include "common/math/vec.h" +#include "chre/util/nanoapp/assert.h" // FORWARD DECLARATIONS //////////////////////////////////////////////////////////////////////// @@ -30,9 +31,9 @@ const static float kEps = 1e-10f; //////////////////////////////////////////////////////////////////////// void lmSolverInit(struct LmSolver *solver, const struct LmParams *params, ResidualAndJacobianFunction func) { - ASSERT_NOT_NULL(solver); - ASSERT_NOT_NULL(params); - ASSERT_NOT_NULL(func); + CHRE_ASSERT_NOT_NULL(solver); + CHRE_ASSERT_NOT_NULL(params); + CHRE_ASSERT_NOT_NULL(func); memset(solver, 0, sizeof(struct LmSolver)); memcpy(&solver->params, params, sizeof(struct LmParams)); solver->func = func; @@ -40,8 +41,8 @@ void lmSolverInit(struct LmSolver *solver, const struct LmParams *params, } void lmSolverSetData(struct LmSolver *solver, struct LmData *data) { - ASSERT_NOT_NULL(solver); - ASSERT_NOT_NULL(data); + CHRE_ASSERT_NOT_NULL(solver); + CHRE_ASSERT_NOT_NULL(data); solver->data = data; } @@ -59,10 +60,10 @@ enum LmStatus lmSolverSolve(struct LmSolver *solver, const float *initial_state, // Check pointers (note that f_data can be null if no additional data is // required by the error function). - ASSERT_NOT_NULL(solver); - ASSERT_NOT_NULL(initial_state); - ASSERT_NOT_NULL(state); - ASSERT_NOT_NULL(solver->data); + CHRE_ASSERT_NOT_NULL(solver); + CHRE_ASSERT_NOT_NULL(initial_state); + CHRE_ASSERT_NOT_NULL(state); + CHRE_ASSERT_NOT_NULL(solver->data); // Allocate memory for intermediate variables. float state_new[MAX_LM_STATE_DIMENSION]; @@ -215,10 +216,10 @@ bool computeResidualAndGradients(ResidualAndJacobianFunction func, float *residual, float *gradient, float *hessian) { // Compute residual and Jacobian. - ASSERT_NOT_NULL(state); - ASSERT_NOT_NULL(residual); - ASSERT_NOT_NULL(gradient); - ASSERT_NOT_NULL(hessian); + CHRE_ASSERT_NOT_NULL(state); + CHRE_ASSERT_NOT_NULL(residual); + CHRE_ASSERT_NOT_NULL(gradient); + CHRE_ASSERT_NOT_NULL(hessian); func(state, f_data, residual, jacobian); // Compute the cost function hessian = jacobian' jacobian and diff --git a/firmware/os/algos/common/math/mat.c b/firmware/os/algos/common/math/mat.c index 8b62cceb..12037968 100644 --- a/firmware/os/algos/common/math/mat.c +++ b/firmware/os/algos/common/math/mat.c @@ -16,9 +16,10 @@ #include "common/math/mat.h" -#include <assert.h> #include <float.h> +#include "chre/util/nanoapp/assert.h" + #ifdef _OS_BUILD_ #include <nanohub_math.h> #include <seos.h> @@ -44,13 +45,13 @@ static void mat33Rotate(struct Mat33 *A, float c, float s, uint32_t k, static void mat44SwapRows(struct Mat44 *A, uint32_t i, uint32_t j); void initZeroMatrix(struct Mat33 *A) { - ASSERT_NOT_NULL(A); + CHRE_ASSERT_NOT_NULL(A); memset(A->elem, 0.0f, sizeof(A->elem)); } UNROLLED void initDiagonalMatrix(struct Mat33 *A, float x) { - ASSERT_NOT_NULL(A); + CHRE_ASSERT_NOT_NULL(A); initZeroMatrix(A); uint32_t i; @@ -61,10 +62,10 @@ void initDiagonalMatrix(struct Mat33 *A, float x) { void initMatrixColumns(struct Mat33 *A, const struct Vec3 *v1, const struct Vec3 *v2, const struct Vec3 *v3) { - ASSERT_NOT_NULL(A); - ASSERT_NOT_NULL(v1); - ASSERT_NOT_NULL(v2); - ASSERT_NOT_NULL(v3); + CHRE_ASSERT_NOT_NULL(A); + CHRE_ASSERT_NOT_NULL(v1); + CHRE_ASSERT_NOT_NULL(v2); + CHRE_ASSERT_NOT_NULL(v3); A->elem[0][0] = v1->x; A->elem[0][1] = v2->x; A->elem[0][2] = v3->x; @@ -79,9 +80,9 @@ void initMatrixColumns(struct Mat33 *A, const struct Vec3 *v1, } void mat33Apply(struct Vec3 *out, const struct Mat33 *A, const struct Vec3 *v) { - ASSERT_NOT_NULL(out); - ASSERT_NOT_NULL(A); - ASSERT_NOT_NULL(v); + CHRE_ASSERT_NOT_NULL(out); + CHRE_ASSERT_NOT_NULL(A); + CHRE_ASSERT_NOT_NULL(v); out->x = A->elem[0][0] * v->x + A->elem[0][1] * v->y + A->elem[0][2] * v->z; out->y = A->elem[1][0] * v->x + A->elem[1][1] * v->y + A->elem[1][2] * v->z; out->z = A->elem[2][0] * v->x + A->elem[2][1] * v->y + A->elem[2][2] * v->z; @@ -90,11 +91,11 @@ void mat33Apply(struct Vec3 *out, const struct Mat33 *A, const struct Vec3 *v) { UNROLLED void mat33Multiply(struct Mat33 *out, const struct Mat33 *A, const struct Mat33 *B) { - ASSERT_NOT_NULL(out); - ASSERT_NOT_NULL(A); - ASSERT_NOT_NULL(B); - ASSERT(out != A); - ASSERT(out != B); + CHRE_ASSERT_NOT_NULL(out); + CHRE_ASSERT_NOT_NULL(A); + CHRE_ASSERT_NOT_NULL(B); + CHRE_ASSERT(out != A); + CHRE_ASSERT(out != B); uint32_t i; for (i = 0; i < 3; ++i) { @@ -113,7 +114,7 @@ void mat33Multiply(struct Mat33 *out, const struct Mat33 *A, UNROLLED void mat33ScalarMul(struct Mat33 *A, float c) { - ASSERT_NOT_NULL(A); + CHRE_ASSERT_NOT_NULL(A); uint32_t i; for (i = 0; i < 3; ++i) { uint32_t j; @@ -125,8 +126,8 @@ void mat33ScalarMul(struct Mat33 *A, float c) { UNROLLED void mat33Add(struct Mat33 *out, const struct Mat33 *A) { - ASSERT_NOT_NULL(out); - ASSERT_NOT_NULL(A); + CHRE_ASSERT_NOT_NULL(out); + CHRE_ASSERT_NOT_NULL(A); uint32_t i; for (i = 0; i < 3; ++i) { uint32_t j; @@ -138,8 +139,8 @@ void mat33Add(struct Mat33 *out, const struct Mat33 *A) { UNROLLED void mat33Sub(struct Mat33 *out, const struct Mat33 *A) { - ASSERT_NOT_NULL(out); - ASSERT_NOT_NULL(A); + CHRE_ASSERT_NOT_NULL(out); + CHRE_ASSERT_NOT_NULL(A); uint32_t i; for (i = 0; i < 3; ++i) { uint32_t j; @@ -151,7 +152,7 @@ void mat33Sub(struct Mat33 *out, const struct Mat33 *A) { UNROLLED int mat33IsPositiveSemidefinite(const struct Mat33 *A, float tolerance) { - ASSERT_NOT_NULL(A); + CHRE_ASSERT_NOT_NULL(A); uint32_t i; for (i = 0; i < 3; ++i) { if (A->elem[i][i] < 0.0f) { @@ -174,11 +175,11 @@ int mat33IsPositiveSemidefinite(const struct Mat33 *A, float tolerance) { UNROLLED void mat33MultiplyTransposed(struct Mat33 *out, const struct Mat33 *A, const struct Mat33 *B) { - ASSERT(out != A); - ASSERT(out != B); - ASSERT_NOT_NULL(out); - ASSERT_NOT_NULL(A); - ASSERT_NOT_NULL(B); + CHRE_ASSERT(out != A); + CHRE_ASSERT(out != B); + CHRE_ASSERT_NOT_NULL(out); + CHRE_ASSERT_NOT_NULL(A); + CHRE_ASSERT_NOT_NULL(B); uint32_t i; for (i = 0; i < 3; ++i) { uint32_t j; @@ -197,11 +198,11 @@ void mat33MultiplyTransposed(struct Mat33 *out, const struct Mat33 *A, UNROLLED void mat33MultiplyTransposed2(struct Mat33 *out, const struct Mat33 *A, const struct Mat33 *B) { - ASSERT(out != A); - ASSERT(out != B); - ASSERT_NOT_NULL(out); - ASSERT_NOT_NULL(A); - ASSERT_NOT_NULL(B); + CHRE_ASSERT(out != A); + CHRE_ASSERT(out != B); + CHRE_ASSERT_NOT_NULL(out); + CHRE_ASSERT_NOT_NULL(A); + CHRE_ASSERT_NOT_NULL(B); uint32_t i; for (i = 0; i < 3; ++i) { uint32_t j; @@ -219,8 +220,8 @@ void mat33MultiplyTransposed2(struct Mat33 *out, const struct Mat33 *A, UNROLLED void mat33Invert(struct Mat33 *out, const struct Mat33 *A) { - ASSERT_NOT_NULL(out); - ASSERT_NOT_NULL(A); + CHRE_ASSERT_NOT_NULL(out); + CHRE_ASSERT_NOT_NULL(A); float t; initDiagonalMatrix(out, 1.0f); @@ -248,7 +249,7 @@ void mat33Invert(struct Mat33 *out, const struct Mat33 *A) { } } // divide by zero guard. - ASSERT(fabsf(tmp.elem[i][i]) > 0); + CHRE_ASSERT(fabsf(tmp.elem[i][i]) > 0); if(!(fabsf(tmp.elem[i][i]) > 0)) { return; } @@ -272,8 +273,8 @@ void mat33Invert(struct Mat33 *out, const struct Mat33 *A) { UNROLLED void mat33Transpose(struct Mat33 *out, const struct Mat33 *A) { - ASSERT_NOT_NULL(out); - ASSERT_NOT_NULL(A); + CHRE_ASSERT_NOT_NULL(out); + CHRE_ASSERT_NOT_NULL(A); uint32_t i; for (i = 0; i < 3; ++i) { uint32_t j; @@ -285,7 +286,7 @@ void mat33Transpose(struct Mat33 *out, const struct Mat33 *A) { UNROLLED void mat33SwapRows(struct Mat33 *A, const uint32_t i, const uint32_t j) { - ASSERT_NOT_NULL(A); + CHRE_ASSERT_NOT_NULL(A); const uint32_t N = 3; uint32_t k; @@ -303,9 +304,9 @@ void mat33SwapRows(struct Mat33 *A, const uint32_t i, const uint32_t j) { UNROLLED void mat33GetEigenbasis(struct Mat33 *S, struct Vec3 *eigenvals, struct Mat33 *eigenvecs) { - ASSERT_NOT_NULL(S); - ASSERT_NOT_NULL(eigenvals); - ASSERT_NOT_NULL(eigenvecs); + CHRE_ASSERT_NOT_NULL(S); + CHRE_ASSERT_NOT_NULL(eigenvals); + CHRE_ASSERT_NOT_NULL(eigenvecs); const uint32_t N = 3; uint32_t i, j, k, l, m; @@ -408,7 +409,7 @@ void mat33GetEigenbasis(struct Mat33 *S, struct Vec3 *eigenvals, } float mat33Determinant(const struct Mat33 *A) { - ASSERT_NOT_NULL(A); + CHRE_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] * @@ -420,7 +421,7 @@ float mat33Determinant(const struct Mat33 *A) { // index of largest off-diagonal element in row k UNROLLED uint32_t mat33Maxind(const struct Mat33 *A, uint32_t k) { - ASSERT_NOT_NULL(A); + CHRE_ASSERT_NOT_NULL(A); const uint32_t N = 3; uint32_t m = k + 1; @@ -437,16 +438,16 @@ uint32_t mat33Maxind(const struct Mat33 *A, uint32_t k) { void mat33Rotate(struct Mat33 *A, float c, float s, uint32_t k, uint32_t l, uint32_t i, uint32_t j) { - ASSERT_NOT_NULL(A); + CHRE_ASSERT_NOT_NULL(A); float tmp = c * A->elem[k][l] - s * A->elem[i][j]; A->elem[i][j] = s * A->elem[k][l] + c * A->elem[i][j]; A->elem[k][l] = tmp; } void mat44Apply(struct Vec4 *out, const struct Mat44 *A, const struct Vec4 *v) { - ASSERT_NOT_NULL(out); - ASSERT_NOT_NULL(A); - ASSERT_NOT_NULL(v); + CHRE_ASSERT_NOT_NULL(out); + CHRE_ASSERT_NOT_NULL(A); + CHRE_ASSERT_NOT_NULL(v); out->x = A->elem[0][0] * v->x + A->elem[0][1] * v->y + A->elem[0][2] * v->z + A->elem[0][3] * v->w; @@ -463,8 +464,8 @@ void mat44Apply(struct Vec4 *out, const struct Mat44 *A, const struct Vec4 *v) { UNROLLED void mat44DecomposeLup(struct Mat44 *LU, struct Size4 *pivot) { - ASSERT_NOT_NULL(LU); - ASSERT_NOT_NULL(pivot); + CHRE_ASSERT_NOT_NULL(LU); + CHRE_ASSERT_NOT_NULL(pivot); const uint32_t N = 4; uint32_t i, j, k; @@ -500,7 +501,7 @@ void mat44DecomposeLup(struct Mat44 *LU, struct Size4 *pivot) { UNROLLED void mat44SwapRows(struct Mat44 *A, const uint32_t i, const uint32_t j) { - ASSERT_NOT_NULL(A); + CHRE_ASSERT_NOT_NULL(A); const uint32_t N = 4; uint32_t k; @@ -518,10 +519,10 @@ void mat44SwapRows(struct Mat44 *A, const uint32_t i, const uint32_t j) { UNROLLED void mat44Solve(const struct Mat44 *A, struct Vec4 *x, const struct Vec4 *b, const struct Size4 *pivot) { - ASSERT_NOT_NULL(A); - ASSERT_NOT_NULL(x); - ASSERT_NOT_NULL(b); - ASSERT_NOT_NULL(pivot); + CHRE_ASSERT_NOT_NULL(A); + CHRE_ASSERT_NOT_NULL(x); + CHRE_ASSERT_NOT_NULL(b); + CHRE_ASSERT_NOT_NULL(pivot); const uint32_t N = 4; uint32_t i, k; @@ -556,8 +557,8 @@ void mat44Solve(const struct Mat44 *A, struct Vec4 *x, const struct Vec4 *b, } float matMaxDiagonalElement(const float *square_mat, size_t n) { - ASSERT_NOT_NULL(square_mat); - ASSERT(n > 0); + CHRE_ASSERT_NOT_NULL(square_mat); + CHRE_ASSERT(n > 0); size_t i; float max = square_mat[0]; const size_t n_square = n * n; @@ -571,7 +572,7 @@ float matMaxDiagonalElement(const float *square_mat, size_t n) { } void matAddConstantDiagonal(float *square_mat, float u, size_t n) { - ASSERT_NOT_NULL(square_mat); + CHRE_ASSERT_NOT_NULL(square_mat); size_t i; const size_t n_square = n * n; const size_t offset = n + 1; @@ -582,8 +583,8 @@ void matAddConstantDiagonal(float *square_mat, float u, size_t n) { void matTransposeMultiplyMat(float *out, const float *A, size_t nrows, size_t ncols) { - ASSERT_NOT_NULL(out); - ASSERT_NOT_NULL(A); + CHRE_ASSERT_NOT_NULL(out); + CHRE_ASSERT_NOT_NULL(A); size_t i; size_t j; size_t k; @@ -608,9 +609,9 @@ void matTransposeMultiplyMat(float *out, const float *A, void matMultiplyVec(float *out, const float *A, const float *v, size_t nrows, size_t ncols) { - ASSERT_NOT_NULL(out); - ASSERT_NOT_NULL(A); - ASSERT_NOT_NULL(v); + CHRE_ASSERT_NOT_NULL(out); + CHRE_ASSERT_NOT_NULL(A); + CHRE_ASSERT_NOT_NULL(v); size_t i; for (i = 0; i < nrows; ++i) { const float *row = &A[i * ncols]; @@ -620,9 +621,9 @@ void matMultiplyVec(float *out, const float *A, const float *v, void matTransposeMultiplyVec(float *out, const float *A, const float *v, size_t nrows, size_t ncols) { - ASSERT_NOT_NULL(out); - ASSERT_NOT_NULL(A); - ASSERT_NOT_NULL(v); + CHRE_ASSERT_NOT_NULL(out); + CHRE_ASSERT_NOT_NULL(A); + CHRE_ASSERT_NOT_NULL(v); size_t i, j; for (i = 0; i < ncols; ++i) { out[i] = 0; @@ -633,10 +634,10 @@ void matTransposeMultiplyVec(float *out, const float *A, const float *v, } bool matLinearSolveCholesky(float *x, const float *L, const float *b, size_t n) { - ASSERT_NOT_NULL(x); - ASSERT_NOT_NULL(L); - ASSERT_NOT_NULL(b); - ASSERT(n <= INT32_MAX); + CHRE_ASSERT_NOT_NULL(x); + CHRE_ASSERT_NOT_NULL(L); + CHRE_ASSERT_NOT_NULL(b); + CHRE_ASSERT(n <= INT32_MAX); int32_t i, j; // Loops below require signed integers. int32_t s_n = (int32_t)n; // Signed n. float sum = 0.0f; @@ -667,8 +668,8 @@ bool matLinearSolveCholesky(float *x, const float *L, const float *b, size_t n) } bool matCholeskyDecomposition(float *L, const float *A, size_t n) { - ASSERT_NOT_NULL(L); - ASSERT_NOT_NULL(A); + CHRE_ASSERT_NOT_NULL(L); + CHRE_ASSERT_NOT_NULL(A); size_t i, j, k; float sum = 0.0f; // initialize L to zero. diff --git a/firmware/os/algos/common/math/vec.c b/firmware/os/algos/common/math/vec.c index bb9f929c..794bb413 100644 --- a/firmware/os/algos/common/math/vec.c +++ b/firmware/os/algos/common/math/vec.c @@ -15,13 +15,14 @@ */ #include "common/math/vec.h" + #include "common/math/macros.h" void findOrthogonalVector(float inX, float inY, float inZ, float *outX, float *outY, float *outZ) { - ASSERT_NOT_NULL(outX); - ASSERT_NOT_NULL(outY); - ASSERT_NOT_NULL(outZ); + CHRE_ASSERT_NOT_NULL(outX); + CHRE_ASSERT_NOT_NULL(outY); + CHRE_ASSERT_NOT_NULL(outZ); float x, y, z; // discard the one with the smallest absolute value @@ -40,7 +41,7 @@ void findOrthogonalVector(float inX, float inY, float inZ, float *outX, } float magSquared = x * x + y * y + z * z; - ASSERT(magSquared > 0); + CHRE_ASSERT(magSquared > 0); // Only set invMag if magSquared is non-zero. float invMag = 1.0f; if (magSquared > 0) { @@ -52,9 +53,9 @@ void findOrthogonalVector(float inX, float inY, float inZ, float *outX, } void vecAdd(float *u, const float *v, const float *w, size_t dim) { - ASSERT_NOT_NULL(u); - ASSERT_NOT_NULL(v); - ASSERT_NOT_NULL(w); + CHRE_ASSERT_NOT_NULL(u); + CHRE_ASSERT_NOT_NULL(v); + CHRE_ASSERT_NOT_NULL(w); size_t i; for (i = 0; i < dim; i++) { u[i] = v[i] + w[i]; @@ -62,8 +63,8 @@ void vecAdd(float *u, const float *v, const float *w, size_t dim) { } void vecAddInPlace(float *v, const float *w, size_t dim) { - ASSERT_NOT_NULL(v); - ASSERT_NOT_NULL(w); + CHRE_ASSERT_NOT_NULL(v); + CHRE_ASSERT_NOT_NULL(w); size_t i; for (i = 0; i < dim; i++) { v[i] += w[i]; @@ -71,9 +72,9 @@ void vecAddInPlace(float *v, const float *w, size_t dim) { } void vecSub(float *u, const float *v, const float *w, size_t dim) { - ASSERT_NOT_NULL(u); - ASSERT_NOT_NULL(v); - ASSERT_NOT_NULL(w); + CHRE_ASSERT_NOT_NULL(u); + CHRE_ASSERT_NOT_NULL(v); + CHRE_ASSERT_NOT_NULL(w); size_t i; for (i = 0; i < dim; i++) { u[i] = v[i] - w[i]; @@ -81,8 +82,8 @@ void vecSub(float *u, const float *v, const float *w, size_t dim) { } void vecScalarMul(float *u, const float *v, float c, size_t dim) { - ASSERT_NOT_NULL(u); - ASSERT_NOT_NULL(v); + CHRE_ASSERT_NOT_NULL(u); + CHRE_ASSERT_NOT_NULL(v); size_t i; for (i = 0; i < dim; i++) { u[i] = c * v[i]; @@ -90,7 +91,7 @@ void vecScalarMul(float *u, const float *v, float c, size_t dim) { } void vecScalarMulInPlace(float *v, float c, size_t dim) { - ASSERT_NOT_NULL(v); + CHRE_ASSERT_NOT_NULL(v); size_t i; for (i = 0; i < dim; i++) { v[i] *= c; @@ -98,19 +99,19 @@ void vecScalarMulInPlace(float *v, float c, size_t dim) { } float vecNorm(const float *v, size_t dim) { - ASSERT_NOT_NULL(v); + CHRE_ASSERT_NOT_NULL(v); float norm_sq = vecNormSquared(v, dim); return sqrtf(norm_sq); } float vecNormSquared(const float *v, size_t dim) { - ASSERT_NOT_NULL(v); + CHRE_ASSERT_NOT_NULL(v); return vecDot(v, v, dim); } float vecDot(const float *v, const float *w, size_t dim) { - ASSERT_NOT_NULL(v); - ASSERT_NOT_NULL(w); + CHRE_ASSERT_NOT_NULL(v); + CHRE_ASSERT_NOT_NULL(w); size_t i; float result = 0; for (i = 0; i < dim; ++i) { @@ -120,7 +121,7 @@ float vecDot(const float *v, const float *w, size_t dim) { } float vecMaxAbsoluteValue(const float *v, size_t dim) { - ASSERT_NOT_NULL(v); + CHRE_ASSERT_NOT_NULL(v); float max = NANO_ABS(v[0]); float tmp; size_t i; diff --git a/firmware/os/algos/common/math/vec.h b/firmware/os/algos/common/math/vec.h index e839ad53..8dbf0eb4 100644 --- a/firmware/os/algos/common/math/vec.h +++ b/firmware/os/algos/common/math/vec.h @@ -39,7 +39,8 @@ #endif // NANOHUB_NON_CHRE_API #include <stddef.h> -#include "util/nano_assert.h" + +#include "chre/util/nanoapp/assert.h" #ifdef __cplusplus extern "C" { @@ -55,7 +56,7 @@ struct Vec4 { // 3-DIMENSIONAL VECTOR MATH /////////////////////////////////////////// static inline void initVec3(struct Vec3 *v, float x, float y, float z) { - ASSERT_NOT_NULL(v); + CHRE_ASSERT_NOT_NULL(v); v->x = x; v->y = y; v->z = z; @@ -63,8 +64,8 @@ static inline void initVec3(struct Vec3 *v, float x, float y, float z) { // Updates v as the sum of v and w. static inline void vec3Add(struct Vec3 *v, const struct Vec3 *w) { - ASSERT_NOT_NULL(v); - ASSERT_NOT_NULL(w); + CHRE_ASSERT_NOT_NULL(v); + CHRE_ASSERT_NOT_NULL(w); v->x += w->x; v->y += w->y; v->z += w->z; @@ -73,9 +74,9 @@ static inline void vec3Add(struct Vec3 *v, const struct Vec3 *w) { // 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); + CHRE_ASSERT_NOT_NULL(u); + CHRE_ASSERT_NOT_NULL(v); + CHRE_ASSERT_NOT_NULL(w); u->x = v->x + w->x; u->y = v->y + w->y; u->z = v->z + w->z; @@ -83,8 +84,8 @@ static inline void vec3AddVecs(struct Vec3 *u, const struct Vec3 *v, // Updates v as the subtraction of w from v. static inline void vec3Sub(struct Vec3 *v, const struct Vec3 *w) { - ASSERT_NOT_NULL(v); - ASSERT_NOT_NULL(w); + CHRE_ASSERT_NOT_NULL(v); + CHRE_ASSERT_NOT_NULL(w); v->x -= w->x; v->y -= w->y; v->z -= w->z; @@ -93,9 +94,9 @@ static inline void vec3Sub(struct Vec3 *v, const struct Vec3 *w) { // 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); + CHRE_ASSERT_NOT_NULL(u); + CHRE_ASSERT_NOT_NULL(v); + CHRE_ASSERT_NOT_NULL(w); u->x = v->x - w->x; u->y = v->y - w->y; u->z = v->z - w->z; @@ -103,7 +104,7 @@ static inline void vec3SubVecs(struct Vec3 *u, const struct Vec3 *v, // Scales v by the scalar c, i.e. v = c * v. static inline void vec3ScalarMul(struct Vec3 *v, float c) { - ASSERT_NOT_NULL(v); + CHRE_ASSERT_NOT_NULL(v); v->x *= c; v->y *= c; v->z *= c; @@ -111,29 +112,29 @@ static inline void vec3ScalarMul(struct Vec3 *v, float c) { // Returns the dot product of v and w. static inline float vec3Dot(const struct Vec3 *v, const struct Vec3 *w) { - ASSERT_NOT_NULL(v); - ASSERT_NOT_NULL(w); + CHRE_ASSERT_NOT_NULL(v); + CHRE_ASSERT_NOT_NULL(w); return v->x * w->x + v->y * w->y + v->z * w->z; } // Returns the square of the L2-norm of the given vector. static inline float vec3NormSquared(const struct Vec3 *v) { - ASSERT_NOT_NULL(v); + CHRE_ASSERT_NOT_NULL(v); return vec3Dot(v, v); } // Returns the L2-norm of the given vector. static inline float vec3Norm(const struct Vec3 *v) { - ASSERT_NOT_NULL(v); + CHRE_ASSERT_NOT_NULL(v); return sqrtf(vec3NormSquared(v)); } // Normalizes the provided vector to unit norm. If the provided vector has a // norm of zero, the vector will be unchanged. static inline void vec3Normalize(struct Vec3 *v) { - ASSERT_NOT_NULL(v); + CHRE_ASSERT_NOT_NULL(v); float norm = vec3Norm(v); - ASSERT(norm > 0); + CHRE_ASSERT(norm > 0); // Only normalize if norm is non-zero. if (norm > 0) { float invNorm = 1.0f / norm; @@ -146,9 +147,9 @@ static inline void vec3Normalize(struct Vec3 *v) { // Updates u as the cross product of v and w. static inline void vec3Cross(struct Vec3 *u, const struct Vec3 *v, const struct Vec3 *w) { - ASSERT_NOT_NULL(u); - ASSERT_NOT_NULL(v); - ASSERT_NOT_NULL(w); + CHRE_ASSERT_NOT_NULL(u); + CHRE_ASSERT_NOT_NULL(v); + CHRE_ASSERT_NOT_NULL(w); u->x = v->y * w->z - v->z * w->y; u->y = v->z * w->x - v->x * w->z; u->z = v->x * w->y - v->y * w->x; @@ -167,7 +168,7 @@ void findOrthogonalVector(float inX, float inY, float inZ, float *outX, // Initialize the Vec4 structure with the provided component values. static inline void initVec4(struct Vec4 *v, float x, float y, float z, float w) { - ASSERT_NOT_NULL(v); + CHRE_ASSERT_NOT_NULL(v); v->x = x; v->y = y; v->z = z; diff --git a/firmware/os/cpu/cortexm4/Android.mk b/firmware/os/cpu/cortexm4/Android.mk index db79e029..3abbf949 100644 --- a/firmware/os/cpu/cortexm4/Android.mk +++ b/firmware/os/cpu/cortexm4/Android.mk @@ -23,6 +23,9 @@ LOCAL_PATH := $(call my-dir) include $(CLEAR_NANO_VARS) LOCAL_MODULE := libnanohub_bl_cortexm4 +LOCAL_LICENSE_KINDS := SPDX-license-identifier-Apache-2.0 SPDX-license-identifier-BSD +LOCAL_LICENSE_CONDITIONS := notice +LOCAL_NOTICE_FILE := $(LOCAL_PATH)/../../../NOTICE LOCAL_AUX_CPU := cortexm4 LOCAL_SRC_FILES := \ @@ -35,6 +38,9 @@ include $(BUILD_NANOHUB_BL_STATIC_LIBRARY) include $(CLEAR_NANO_VARS) LOCAL_MODULE := libnanohub_os_cortexm4 +LOCAL_LICENSE_KINDS := SPDX-license-identifier-Apache-2.0 SPDX-license-identifier-BSD +LOCAL_LICENSE_CONDITIONS := notice +LOCAL_NOTICE_FILE := $(LOCAL_PATH)/../../../NOTICE LOCAL_AUX_CPU := cortexm4 LOCAL_SRC_FILES := \ diff --git a/firmware/os/cpu/x86/Android.mk b/firmware/os/cpu/x86/Android.mk index 6a9476b9..4b7afcb3 100644 --- a/firmware/os/cpu/x86/Android.mk +++ b/firmware/os/cpu/x86/Android.mk @@ -23,6 +23,9 @@ LOCAL_PATH := $(call my-dir) include $(CLEAR_NANO_VARS) LOCAL_MODULE := libnanohub_os_x86 +LOCAL_LICENSE_KINDS := SPDX-license-identifier-Apache-2.0 +LOCAL_LICENSE_CONDITIONS := notice +LOCAL_NOTICE_FILE := $(LOCAL_PATH)/../../../NOTICE LOCAL_AUX_CPU := x86 LOCAL_SRC_FILES := \ diff --git a/firmware/os/drivers/bosch_bmi160/bosch_bmi160.c b/firmware/os/drivers/bosch_bmi160/bosch_bmi160.c index 3f272d6c..eae0671e 100644 --- a/firmware/os/drivers/bosch_bmi160/bosch_bmi160.c +++ b/firmware/os/drivers/bosch_bmi160/bosch_bmi160.c @@ -129,9 +129,16 @@ #define BMI160_SPI_WRITE 0x00 #define BMI160_SPI_READ 0x80 +#ifndef BMI160_SPI_BUS_ID #define BMI160_SPI_BUS_ID 1 +#endif +#ifndef BMI160_SPI_SPEED_HZ #define BMI160_SPI_SPEED_HZ 8000000 +#endif #define BMI160_SPI_MODE 3 +#ifndef BMI160_SPI_CS_PIN +#define BMI160_SPI_CS_PIN GPIO_PB(12) +#endif #ifndef BMI160_INT1_IRQ #define BMI160_INT1_IRQ EXTI9_5_IRQn @@ -400,7 +407,7 @@ enum SensorState { static const char * getStateName(int32_t s) { // keep this in sync with SensorState static const char* const l[] = {"BOOT", "VERIFY_ID", "INIT", "IDLE", "PWR_UP", - "PWR-DN", "CFG_CHANGE", "INT1", "INT2", "CALIB", "STEP_CNT", "SYNC", "SAVE_CALIB"}; + "PWR-DN", "CFG_CHANGE", "INT1", "INT2", "CALIB", "TEST", "STEP_CNT", "SYNC", "SAVE_CALIB"}; if (s >= 0 && s < SENSOR_NUM_OF_STATE) { return l[s]; } @@ -3825,7 +3832,7 @@ static bool startTask(uint32_t task_id) T(mode).cpha = SPI_CPHA_TRAILING_EDGE; T(mode).nssChange = true; T(mode).format = SPI_FORMAT_MSB_FIRST; - T(cs) = GPIO_PB(12); + T(cs) = BMI160_SPI_CS_PIN; T(watermark) = 0; diff --git a/firmware/os/drivers/invensense_icm40600/invensense_icm40600.c b/firmware/os/drivers/invensense_icm40600/invensense_icm40600.c index dc85f551..9410b192 100644 --- a/firmware/os/drivers/invensense_icm40600/invensense_icm40600.c +++ b/firmware/os/drivers/invensense_icm40600/invensense_icm40600.c @@ -3258,9 +3258,6 @@ static void endTask(void) #ifdef ACCEL_CAL_ENABLED accelCalDestroy(&T(accel_cal)); #endif -#ifdef GYRO_CAL_ENABLED - gyroCalDestroy(&T(gyro_cal)); -#endif slabAllocatorDestroy(T(mDataSlab)); spiMasterRelease(T(spiDev)); diff --git a/firmware/os/drivers/rohm_rpr0521/rohm_rpr0521.c b/firmware/os/drivers/rohm_rpr0521/rohm_rpr0521.c index edb8fb2b..ab0e05fc 100644 --- a/firmware/os/drivers/rohm_rpr0521/rohm_rpr0521.c +++ b/firmware/os/drivers/rohm_rpr0521/rohm_rpr0521.c @@ -328,21 +328,54 @@ static struct I2cTransfer *allocXfer(uint8_t state) } } - osLog(LOG_ERROR, "[BMP280]: Ran out of i2c buffers!"); + ERROR_PRINT("Ran out of i2c buffers!\n"); return NULL; } +static inline void releaseXfer(struct I2cTransfer *xfer) +{ + xfer->inUse = false; +} + +static bool i2cWrite(struct I2cTransfer *xfer, size_t txSize) +{ + int ret = i2cMasterTx(I2C_BUS_ID, I2C_ADDR, xfer->txrxBuf, txSize, i2cCallback, xfer); + if (ret < 0) { + ERROR_PRINT("%s: i2cMasterTx failed (%d)\n", __func__, ret); + releaseXfer(xfer); + } + + return (ret == 0); +} + // Helper function to write a one byte register. Returns true if we got a // successful return value from i2cMasterTx(). static bool writeRegister(uint8_t reg, uint8_t value, uint8_t state) { struct I2cTransfer *xfer = allocXfer(state); - int ret = -1; if (xfer != NULL) { xfer->txrxBuf[0] = reg; xfer->txrxBuf[1] = value; - ret = i2cMasterTx(I2C_BUS_ID, I2C_ADDR, xfer->txrxBuf, 2, i2cCallback, xfer); + return i2cWrite(xfer, 2); + } + + return false; +} + +static bool readRegister(uint8_t reg, uint8_t len, uint8_t state) +{ + struct I2cTransfer *xfer = allocXfer(state); + int ret = -1; + + if (xfer != NULL) { + xfer->txrxBuf[0] = reg; + ret = i2cMasterTxRx(I2C_BUS_ID, I2C_ADDR, + xfer->txrxBuf, 1, xfer->txrxBuf, len, i2cCallback, xfer); + if (ret < 0) { + ERROR_PRINT("%s: i2cMasterTxRx failed (%d)\n", __func__, ret); + releaseXfer(xfer); + } } return (ret == 0); @@ -492,9 +525,7 @@ static bool sensorCfgDataProx(void *data, void *cookie) xfer->txrxBuf[0] = ROHM_RPR0521_REG_PS_OFFSET_LSB; xfer->txrxBuf[1] = offset & 0xFF; xfer->txrxBuf[2] = (offset >> 8) & 0x3; - i2cMasterTx(I2C_BUS_ID, I2C_ADDR, xfer->txrxBuf, 3, i2cCallback, xfer); - - return true; + return i2cWrite(xfer, 3); } return false; @@ -577,11 +608,7 @@ static void handle_i2c_event(struct I2cTransfer *xfer) switch (xfer->state) { case SENSOR_STATE_RESET: - newXfer = allocXfer(SENSOR_STATE_VERIFY_ID); - if (newXfer != NULL) { - newXfer->txrxBuf[0] = ROHM_RPR0521_REG_ID; - i2cMasterTxRx(I2C_BUS_ID, I2C_ADDR, newXfer->txrxBuf, 1, newXfer->txrxBuf, 1, i2cCallback, newXfer); - } + readRegister(ROHM_RPR0521_REG_ID, 1, SENSOR_STATE_VERIFY_ID); break; case SENSOR_STATE_VERIFY_ID: @@ -598,7 +625,7 @@ static void handle_i2c_event(struct I2cTransfer *xfer) newXfer->txrxBuf[0] = ROHM_RPR0521_REG_ALS_PS_CONTROL; newXfer->txrxBuf[1] = (ROHM_RPR0521_GAIN_ALS0 << 4) | (ROHM_RPR0521_GAIN_ALS1 << 2) | ROHM_RPR0521_LED_CURRENT; newXfer->txrxBuf[2] = (ROHM_RPR0521_GAIN_PS << 4) | PS_PERSISTENCE_ACTIVE_AT_EACH_MEASUREMENT_END; - i2cMasterTx(I2C_BUS_ID, I2C_ADDR, newXfer->txrxBuf, 3, i2cCallback, newXfer); + i2cWrite(newXfer, 3); } break; @@ -609,7 +636,7 @@ static void handle_i2c_event(struct I2cTransfer *xfer) newXfer->txrxBuf[0] = ROHM_RPR0521_REG_PS_OFFSET_LSB; newXfer->txrxBuf[1] = 0; newXfer->txrxBuf[2] = 0; - i2cMasterTx(I2C_BUS_ID, I2C_ADDR, newXfer->txrxBuf, 3, i2cCallback, newXfer); + i2cWrite(newXfer, 3); } break; @@ -622,7 +649,7 @@ static void handle_i2c_event(struct I2cTransfer *xfer) newXfer->txrxBuf[2] = (ROHM_RPR0521_THRESHOLD_ASSERT_NEAR & 0xFF00) >> 8; newXfer->txrxBuf[3] = (ROHM_RPR0521_THRESHOLD_DEASSERT_NEAR & 0xFF); newXfer->txrxBuf[4] = (ROHM_RPR0521_THRESHOLD_DEASSERT_NEAR & 0xFF00) >> 8; - i2cMasterTx(I2C_BUS_ID, I2C_ADDR, newXfer->txrxBuf, 5, i2cCallback, newXfer); + i2cWrite(newXfer, 5); } break; @@ -710,7 +737,7 @@ static void handle_i2c_event(struct I2cTransfer *xfer) break; } - xfer->inUse = false; + releaseXfer(xfer); } /* @@ -755,8 +782,6 @@ static void end_app(void) static void handle_event(uint32_t evtType, const void* evtData) { - struct I2cTransfer *xfer; - switch (evtType) { case EVT_APP_START: i2cMasterRequest(I2C_BUS_ID, I2C_SPEED); @@ -770,22 +795,13 @@ static void handle_event(uint32_t evtType, const void* evtData) break; case EVT_SENSOR_ALS_TIMER: - xfer = allocXfer(SENSOR_STATE_ALS_SAMPLING); - if (xfer != NULL) { - xfer->txrxBuf[0] = ROHM_RPR0521_REG_ALS_DATA0_LSB; - i2cMasterTxRx(I2C_BUS_ID, I2C_ADDR, xfer->txrxBuf, 1, xfer->txrxBuf, 4, i2cCallback, xfer); - } + readRegister(ROHM_RPR0521_REG_ALS_DATA0_LSB, 4, SENSOR_STATE_ALS_SAMPLING); break; case EVT_SENSOR_PROX_INTERRUPT: // Over-read to read the INTERRUPT register to clear the interrupt - xfer = allocXfer(SENSOR_STATE_PROX_SAMPLING); - if (xfer != NULL) { - xfer->txrxBuf[0] = ROHM_RPR0521_REG_PS_DATA_LSB; - i2cMasterTxRx(I2C_BUS_ID, I2C_ADDR, xfer->txrxBuf, 1, xfer->txrxBuf, 7, i2cCallback, xfer); - } + readRegister(ROHM_RPR0521_REG_PS_DATA_LSB, 7, SENSOR_STATE_PROX_SAMPLING); break; - } } diff --git a/firmware/os/drivers/st_lsm6dsm/st_lsm6dsm.c b/firmware/os/drivers/st_lsm6dsm/st_lsm6dsm.c index d8e7e87b..0c9930c8 100644 --- a/firmware/os/drivers/st_lsm6dsm/st_lsm6dsm.c +++ b/firmware/os/drivers/st_lsm6dsm/st_lsm6dsm.c @@ -5209,9 +5209,6 @@ static void lsm6dsm_endTask(void) #ifdef LSM6DSM_MAGN_CALIB_ENABLED magCalDestroy(&T(magnCal)); #endif /* LSM6DSM_MAGN_CALIB_ENABLED */ -#ifdef LSM6DSM_GYRO_CALIB_ENABLED - gyroCalDestroy(&T(gyroCal)); -#endif /* LSM6DSM_GYRO_CALIB_ENABLED */ lsm6dsm_disableInterrupt(T(int1), &T(isr1)); #ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED diff --git a/firmware/os/platform/native/Android.mk b/firmware/os/platform/native/Android.mk index 54ed6f52..4dae4ffb 100644 --- a/firmware/os/platform/native/Android.mk +++ b/firmware/os/platform/native/Android.mk @@ -25,6 +25,9 @@ NANOHUB_PALTFORM_PATH := $(LOCAL_PATH) include $(CLEAR_NANO_VARS) LOCAL_MODULE := libnanohub_os_native +LOCAL_LICENSE_KINDS := SPDX-license-identifier-Apache-2.0 +LOCAL_LICENSE_CONDITIONS := notice +LOCAL_NOTICE_FILE := $(LOCAL_PATH)/../../../NOTICE LOCAL_AUX_ARCH := native LOCAL_SRC_FILES := \ diff --git a/firmware/os/platform/stm32/Android.mk b/firmware/os/platform/stm32/Android.mk index b61dc3b7..346a7e70 100644 --- a/firmware/os/platform/stm32/Android.mk +++ b/firmware/os/platform/stm32/Android.mk @@ -29,6 +29,9 @@ NANOHUB_PALTFORM_PATH := $(LOCAL_PATH) include $(CLEAR_NANO_VARS) LOCAL_MODULE := libnanohub_bl_stm32 +LOCAL_LICENSE_KINDS := SPDX-license-identifier-Apache-2.0 +LOCAL_LICENSE_CONDITIONS := notice +LOCAL_NOTICE_FILE := $(LOCAL_PATH)/../../../NOTICE LOCAL_AUX_ARCH := stm32 LOCAL_SRC_FILES := \ @@ -45,6 +48,9 @@ include $(BUILD_NANOHUB_BL_STATIC_LIBRARY) include $(CLEAR_NANO_VARS) LOCAL_MODULE := libnanohub_os_stm32 +LOCAL_LICENSE_KINDS := SPDX-license-identifier-Apache-2.0 +LOCAL_LICENSE_CONDITIONS := notice +LOCAL_NOTICE_FILE := $(LOCAL_PATH)/../../../NOTICE LOCAL_AUX_ARCH := stm32 LOCAL_SRC_FILES := \ diff --git a/firmware/os/platform/stm32/eeData.c b/firmware/os/platform/stm32/eeData.c index 2710ba07..fac71281 100644 --- a/firmware/os/platform/stm32/eeData.c +++ b/firmware/os/platform/stm32/eeData.c @@ -171,7 +171,7 @@ bool eeDataEraseOldVersion(uint32_t name, void *vaddr) uint32_t *addr = (uint32_t*)vaddr; uint32_t v; - // sanity check + // validate name and address if (!eeIsValidName(name) || addr < __eedata_start || addr >= (__eedata_end - 1)) return false; diff --git a/firmware/os/platform/stm32/i2c.c b/firmware/os/platform/stm32/i2c.c index 8ae35cf7..d553202d 100644 --- a/firmware/os/platform/stm32/i2c.c +++ b/firmware/os/platform/stm32/i2c.c @@ -177,7 +177,6 @@ struct StmI2cDev { const struct StmI2cBoardCfg *board; struct I2cStmState state; - uint32_t next; uint32_t last; struct Gpio *scl; @@ -244,8 +243,10 @@ static inline struct StmI2cXfer *stmI2cGetXfer(void) static inline void stmI2cPutXfer(struct StmI2cXfer *xfer) { - if (xfer) + if (xfer) { + atomicWrite32bits(&xfer->id, 0); atomicBitsetClearBit(mXfersValid, xfer - mXfers); + } } static inline void stmI2cAckEnable(struct StmI2cDev *pdev) @@ -486,13 +487,30 @@ static void stmI2cSlaveNakRxed(struct StmI2cDev *pdev) regs->SR1 &= ~I2C_SR1_AF; } +static inline struct StmI2cXfer *stmI2cGetNextPendingXfer(uint8_t busId) +{ + uint32_t currId = UINT32_MAX; + struct StmI2cXfer *pendingXfer = NULL; + + for (int i = 0; i < I2C_MAX_QUEUE_DEPTH; i++) { + struct StmI2cXfer *xfer = &mXfers[i]; + if (xfer->busId == busId) { + uint32_t xferId = atomicRead32bits(&xfer->id); + if (xferId > 0 && xferId <= currId) { + pendingXfer = xfer; + currId = xferId; + } + } + } + + return pendingXfer; +} + static inline void stmI2cMasterTxRxDone(struct StmI2cDev *pdev, int err) { struct I2cStmState *state = &pdev->state; size_t txOffst = state->tx.offset; size_t rxOffst = state->rx.offset; - uint32_t id; - int i; struct StmI2cXfer *xfer; if (pdev->board->sleepDev >= 0) @@ -502,34 +520,26 @@ static inline void stmI2cMasterTxRxDone(struct StmI2cDev *pdev, int err) state->rx.offset = 0; stmI2cInvokeTxCallback(state, txOffst, rxOffst, err); - do { - id = atomicAdd32bits(&pdev->next, 1); - } while (!id); - - for (i=0; i<I2C_MAX_QUEUE_DEPTH; i++) { - xfer = &mXfers[i]; - - if (xfer->busId == (pdev - mStmI2cDevs) && - atomicCmpXchg32bits(&xfer->id, id, 0)) { - pdev->addr = xfer->addr; - state->tx.cbuf = xfer->txBuf; - state->tx.offset = 0; - state->tx.size = xfer->txSize; - state->tx.callback = xfer->callback; - state->tx.cookie = xfer->cookie; - state->rx.buf = xfer->rxBuf; - state->rx.offset = 0; - state->rx.size = xfer->rxSize; - state->rx.callback = NULL; - state->rx.cookie = NULL; - state->tid = xfer->tid; - atomicWriteByte(&state->masterState, STM_I2C_MASTER_START); - if (pdev->board->sleepDev >= 0) - platRequestDevInSleepMode(pdev->board->sleepDev, 12); - stmI2cPutXfer(xfer); - stmI2cStartEnable(pdev); - return; - } + xfer = stmI2cGetNextPendingXfer(pdev - mStmI2cDevs); + if (xfer) { + atomicWriteByte(&state->masterState, STM_I2C_MASTER_START); + pdev->addr = xfer->addr; + state->tx.cbuf = xfer->txBuf; + state->tx.offset = 0; + state->tx.size = xfer->txSize; + state->tx.callback = xfer->callback; + state->tx.cookie = xfer->cookie; + state->rx.buf = xfer->rxBuf; + state->rx.offset = 0; + state->rx.size = xfer->rxSize; + state->rx.callback = NULL; + state->rx.cookie = NULL; + state->tid = xfer->tid; + if (pdev->board->sleepDev >= 0) + platRequestDevInSleepMode(pdev->board->sleepDev, 12); + stmI2cPutXfer(xfer); + stmI2cStartEnable(pdev); + return; } atomicWriteByte(&state->masterState, STM_I2C_MASTER_IDLE); @@ -839,7 +849,6 @@ int i2cMasterRequest(uint32_t busId, uint32_t speed) pdev->cfg = cfg; pdev->board = board; - pdev->next = 2; pdev->last = 1; atomicBitsetInit(mXfersValid, I2C_MAX_QUEUE_DEPTH); @@ -943,7 +952,7 @@ int i2cMasterTxRx(uint32_t busId, uint32_t addr, STM_I2C_MASTER_IDLE, STM_I2C_MASTER_START)) { // it is possible for this transfer to already be complete by the // time we get here. if so, transfer->id will have been set to 0. - if (atomicCmpXchg32bits(&xfer->id, id, 0)) { + if (atomicRead32bits(&xfer->id) != 0) { pdev->addr = xfer->addr; state->tx.cbuf = xfer->txBuf; state->tx.offset = 0; diff --git a/firmware/variant/argonkey/Android.mk b/firmware/variant/argonkey/Android.mk index 773dc114..17f60af8 100644 --- a/firmware/variant/argonkey/Android.mk +++ b/firmware/variant/argonkey/Android.mk @@ -19,6 +19,9 @@ LOCAL_PATH := $(call my-dir) include $(CLEAR_NANO_VARS) LOCAL_MODULE := libnanohub_os_argonkey +LOCAL_LICENSE_KINDS := SPDX-license-identifier-Apache-2.0 +LOCAL_LICENSE_CONDITIONS := notice +LOCAL_NOTICE_FILE := $(LOCAL_PATH)/../../NOTICE LOCAL_MODULE_TAGS := optional LOCAL_AUX_OS_VARIANT := argonkey diff --git a/firmware/variant/linux/Android.mk b/firmware/variant/linux/Android.mk index 669b221a..79e1eeb9 100644 --- a/firmware/variant/linux/Android.mk +++ b/firmware/variant/linux/Android.mk @@ -19,6 +19,9 @@ LOCAL_PATH := $(call my-dir) include $(CLEAR_NANO_VARS) LOCAL_MODULE := libnanohub_os_linux +LOCAL_LICENSE_KINDS := SPDX-license-identifier-Apache-2.0 +LOCAL_LICENSE_CONDITIONS := notice +LOCAL_NOTICE_FILE := $(LOCAL_PATH)/../../NOTICE LOCAL_MODULE_TAGS := optional LOCAL_AUX_OS_VARIANT := linux diff --git a/firmware/variant/lunchbox/Android.mk b/firmware/variant/lunchbox/Android.mk index d743a54b..f157e17f 100644 --- a/firmware/variant/lunchbox/Android.mk +++ b/firmware/variant/lunchbox/Android.mk @@ -19,6 +19,9 @@ LOCAL_PATH := $(call my-dir) include $(CLEAR_NANO_VARS) LOCAL_MODULE := libnanohub_os_lunchbox +LOCAL_LICENSE_KINDS := SPDX-license-identifier-Apache-2.0 +LOCAL_LICENSE_CONDITIONS := notice +LOCAL_NOTICE_FILE := $(LOCAL_PATH)/../../NOTICE LOCAL_MODULE_TAGS := optional LOCAL_AUX_OS_VARIANT := lunchbox diff --git a/firmware/variant/neonkey/Android.mk b/firmware/variant/neonkey/Android.mk index f53a2ed0..5f01e261 100644 --- a/firmware/variant/neonkey/Android.mk +++ b/firmware/variant/neonkey/Android.mk @@ -19,6 +19,9 @@ LOCAL_PATH := $(call my-dir) include $(CLEAR_NANO_VARS) LOCAL_MODULE := libnanohub_os_neonkey +LOCAL_LICENSE_KINDS := SPDX-license-identifier-Apache-2.0 +LOCAL_LICENSE_CONDITIONS := notice +LOCAL_NOTICE_FILE := $(LOCAL_PATH)/../../NOTICE LOCAL_MODULE_TAGS := optional LOCAL_AUX_OS_VARIANT := neonkey diff --git a/firmware/variant/nucleo/Android.mk b/firmware/variant/nucleo/Android.mk index 1e00b258..043bed7e 100644 --- a/firmware/variant/nucleo/Android.mk +++ b/firmware/variant/nucleo/Android.mk @@ -19,6 +19,9 @@ LOCAL_PATH := $(call my-dir) include $(CLEAR_NANO_VARS) LOCAL_MODULE := libnanohub_os_nucleo +LOCAL_LICENSE_KINDS := SPDX-license-identifier-Apache-2.0 +LOCAL_LICENSE_CONDITIONS := notice +LOCAL_NOTICE_FILE := $(LOCAL_PATH)/../../NOTICE LOCAL_MODULE_TAGS := optional LOCAL_AUX_OS_VARIANT := nucleo diff --git a/lefty/Android.mk b/lefty/Android.mk index c3caaaa5..ede94a8a 100644 --- a/lefty/Android.mk +++ b/lefty/Android.mk @@ -24,6 +24,8 @@ ifeq ($(NANOHUB_SENSORHAL_LEFTY_IMPL_ENABLED), true) include $(CLEAR_VARS) LOCAL_MODULE := vendor.google_clockwork.lefty@1.0-impl.nanohub +LOCAL_LICENSE_KINDS := SPDX-license-identifier-Apache-2.0 +LOCAL_LICENSE_CONDITIONS := notice LOCAL_MODULE_RELATIVE_PATH := hw LOCAL_MODULE_TAGS := optional @@ -58,6 +60,8 @@ ifeq ($(NANOHUB_SENSORHAL_LEFTY_SERVICE_ENABLED), true) include $(CLEAR_VARS) LOCAL_MODULE := liblefty_service_nanohub +LOCAL_LICENSE_KINDS := SPDX-license-identifier-Apache-2.0 +LOCAL_LICENSE_CONDITIONS := notice LOCAL_MODULE_TAGS := optional LOCAL_MODULE_OWNER := google diff --git a/lib/Android.bp b/lib/Android.bp index 58b71268..9d322f67 100644 --- a/lib/Android.bp +++ b/lib/Android.bp @@ -14,6 +14,15 @@ // limitations under the License. // +package { + // See: http://go/android-license-faq + // A large-scale-change added 'default_applicable_licenses' to import + // all of the 'license_kinds' from "device_google_contexthub_license" + // to get the below license kinds: + // SPDX-license-identifier-Apache-2.0 + default_applicable_licenses: ["device_google_contexthub_license"], +} + cc_library_headers { name: "libnanohub_common_headers", vendor_available: true, diff --git a/lib/Android.mk b/lib/Android.mk index c47b6af5..1ee51a72 100644 --- a/lib/Android.mk +++ b/lib/Android.mk @@ -27,6 +27,8 @@ src_includes := \ include $(CLEAR_NANO_VARS) LOCAL_MODULE := libnanohub_common_bl +LOCAL_LICENSE_KINDS := SPDX-license-identifier-Apache-2.0 +LOCAL_LICENSE_CONDITIONS := notice LOCAL_MODULE_TAGS := optional LOCAL_SRC_FILES := $(src_files) @@ -38,6 +40,8 @@ include $(BUILD_NANOHUB_BL_STATIC_LIBRARY) include $(CLEAR_NANO_VARS) LOCAL_MODULE := libnanohub_common_os +LOCAL_LICENSE_KINDS := SPDX-license-identifier-Apache-2.0 +LOCAL_LICENSE_CONDITIONS := notice LOCAL_MODULE_TAGS := optional LOCAL_SRC_FILES := nanohub/softcrc.c diff --git a/sensorhal/Android.mk b/sensorhal/Android.mk index d789d167..b8d280be 100644 --- a/sensorhal/Android.mk +++ b/sensorhal/Android.mk @@ -46,15 +46,23 @@ include $(CLEAR_VARS) ifeq ($(NANOHUB_SENSORHAL_NAME_OVERRIDE),) ifeq ($(TARGET_DEVICE),angler_treble) LOCAL_MODULE := sensors.angler +LOCAL_LICENSE_KINDS := SPDX-license-identifier-Apache-2.0 +LOCAL_LICENSE_CONDITIONS := notice else ifeq ($(TARGET_DEVICE),bullhead_treble) LOCAL_MODULE := sensors.bullhead +LOCAL_LICENSE_KINDS := SPDX-license-identifier-Apache-2.0 +LOCAL_LICENSE_CONDITIONS := notice else LOCAL_MODULE := sensors.$(TARGET_DEVICE) +LOCAL_LICENSE_KINDS := SPDX-license-identifier-Apache-2.0 +LOCAL_LICENSE_CONDITIONS := notice endif endif else LOCAL_MODULE := $(NANOHUB_SENSORHAL_NAME_OVERRIDE) +LOCAL_LICENSE_KINDS := SPDX-license-identifier-Apache-2.0 +LOCAL_LICENSE_CONDITIONS := notice endif LOCAL_MODULE_RELATIVE_PATH := hw @@ -103,6 +111,8 @@ include $(BUILD_SHARED_LIBRARY) include $(CLEAR_VARS) LOCAL_MODULE := libhubconnection +LOCAL_LICENSE_KINDS := SPDX-license-identifier-Apache-2.0 +LOCAL_LICENSE_CONDITIONS := notice LOCAL_MODULE_TAGS := optional LOCAL_MODULE_OWNER := google LOCAL_PROPRIETARY_MODULE := true diff --git a/util/Android.bp b/util/Android.bp new file mode 100644 index 00000000..296562ea --- /dev/null +++ b/util/Android.bp @@ -0,0 +1,31 @@ +// +// Copyright (C) 2021 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. + +package { + default_applicable_licenses: ["device_google_contexthub_util_license"], +} + +// Added automatically by a large-scale-change +// See: http://go/android-license-faq +license { + name: "device_google_contexthub_util_license", + visibility: [":__subpackages__"], + license_kinds: [ + "SPDX-license-identifier-Apache-2.0", + ], + license_text: [ + "NOTICE", + ], +} diff --git a/util/common/Android.bp b/util/common/Android.bp index ac6d65d2..d791455e 100644 --- a/util/common/Android.bp +++ b/util/common/Android.bp @@ -12,6 +12,15 @@ // 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. +package { + // See: http://go/android-license-faq + // A large-scale-change added 'default_applicable_licenses' to import + // all of the 'license_kinds' from "device_google_contexthub_util_license" + // to get the below license kinds: + // SPDX-license-identifier-Apache-2.0 + default_applicable_licenses: ["device_google_contexthub_util_license"], +} + cc_library_static { name: "libhubutilcommon", srcs: [ diff --git a/util/nanoapp_cmd/Android.bp b/util/nanoapp_cmd/Android.bp index 62aabae5..402a851f 100644 --- a/util/nanoapp_cmd/Android.bp +++ b/util/nanoapp_cmd/Android.bp @@ -14,6 +14,15 @@ // limitations under the License. // +package { + // See: http://go/android-license-faq + // A large-scale-change added 'default_applicable_licenses' to import + // all of the 'license_kinds' from "device_google_contexthub_util_license" + // to get the below license kinds: + // SPDX-license-identifier-Apache-2.0 + default_applicable_licenses: ["device_google_contexthub_util_license"], +} + cc_binary { name: "nanoapp_cmd", diff --git a/util/nanoapp_encr/Android.bp b/util/nanoapp_encr/Android.bp index 5e9ff1e3..b7b4d022 100644 --- a/util/nanoapp_encr/Android.bp +++ b/util/nanoapp_encr/Android.bp @@ -14,6 +14,15 @@ // limitations under the License. // +package { + // See: http://go/android-license-faq + // A large-scale-change added 'default_applicable_licenses' to import + // all of the 'license_kinds' from "device_google_contexthub_util_license" + // to get the below license kinds: + // SPDX-license-identifier-Apache-2.0 + default_applicable_licenses: ["device_google_contexthub_util_license"], +} + cc_binary_host { name: "nanoapp_encr", diff --git a/util/nanoapp_postprocess/Android.bp b/util/nanoapp_postprocess/Android.bp index 6a8400f0..b5cdf992 100644 --- a/util/nanoapp_postprocess/Android.bp +++ b/util/nanoapp_postprocess/Android.bp @@ -14,6 +14,15 @@ // limitations under the License. // +package { + // See: http://go/android-license-faq + // A large-scale-change added 'default_applicable_licenses' to import + // all of the 'license_kinds' from "device_google_contexthub_util_license" + // to get the below license kinds: + // SPDX-license-identifier-Apache-2.0 + default_applicable_licenses: ["device_google_contexthub_util_license"], +} + cc_binary_host { name: "nanoapp_postprocess", diff --git a/util/nanoapp_sign/Android.bp b/util/nanoapp_sign/Android.bp index 752ba878..85eb17da 100644 --- a/util/nanoapp_sign/Android.bp +++ b/util/nanoapp_sign/Android.bp @@ -14,6 +14,15 @@ // limitations under the License. // +package { + // See: http://go/android-license-faq + // A large-scale-change added 'default_applicable_licenses' to import + // all of the 'license_kinds' from "device_google_contexthub_util_license" + // to get the below license kinds: + // SPDX-license-identifier-Apache-2.0 + default_applicable_licenses: ["device_google_contexthub_util_license"], +} + cc_binary_host { name: "nanoapp_sign", diff --git a/util/nanotool/Android.bp b/util/nanotool/Android.bp index 0dc1ec0c..bf8b3204 100644 --- a/util/nanotool/Android.bp +++ b/util/nanotool/Android.bp @@ -14,6 +14,15 @@ // limitations under the License. // +package { + // See: http://go/android-license-faq + // A large-scale-change added 'default_applicable_licenses' to import + // all of the 'license_kinds' from "device_google_contexthub_util_license" + // to get the below license kinds: + // SPDX-license-identifier-Apache-2.0 + default_applicable_licenses: ["device_google_contexthub_util_license"], +} + NANOTOOL_VERSION = "1.2.0" cc_binary { diff --git a/util/sensortest/Android.bp b/util/sensortest/Android.bp index e1de6aee..de02d1f6 100644 --- a/util/sensortest/Android.bp +++ b/util/sensortest/Android.bp @@ -12,6 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. +package { + // See: http://go/android-license-faq + // A large-scale-change added 'default_applicable_licenses' to import + // all of the 'license_kinds' from "device_google_contexthub_util_license" + // to get the below license kinds: + // SPDX-license-identifier-Apache-2.0 + default_applicable_licenses: ["device_google_contexthub_util_license"], +} + cc_binary { name: "sensortest", diff --git a/util/stm32_flash/Android.bp b/util/stm32_flash/Android.bp index 18e6f7b7..53f40139 100644 --- a/util/stm32_flash/Android.bp +++ b/util/stm32_flash/Android.bp @@ -14,6 +14,15 @@ // limitations under the License. // +package { + // See: http://go/android-license-faq + // A large-scale-change added 'default_applicable_licenses' to import + // all of the 'license_kinds' from "device_google_contexthub_util_license" + // to get the below license kinds: + // SPDX-license-identifier-Apache-2.0 + default_applicable_licenses: ["device_google_contexthub_util_license"], +} + cc_binary { name: "stm32_flash", |