对比新文件 |
| | |
| | | /* |
| | | * Copyright (c) 2019-2023 Beijing Hanwei Innovation Technology Ltd. Co. and |
| | | * its subsidiaries and affiliates (collectly called MKSEMI). |
| | | * |
| | | * All rights reserved. |
| | | * |
| | | * Redistribution and use in source and binary forms, with or without |
| | | * modification, are permitted provided that the following conditions are met: |
| | | * |
| | | * 1. Redistributions of source code must retain the above copyright notice, |
| | | * this list of conditions and the following disclaimer. |
| | | * |
| | | * 2. Redistributions in binary form, except as embedded into an MKSEMI |
| | | * integrated circuit in a product or a software update for such product, |
| | | * must reproduce the above copyright notice, this list of conditions and |
| | | * the following disclaimer in the documentation and/or other materials |
| | | * provided with the distribution. |
| | | * |
| | | * 3. Neither the name of MKSEMI nor the names of its contributors may be used |
| | | * to endorse or promote products derived from this software without |
| | | * specific prior written permission. |
| | | * |
| | | * 4. This software, with or without modification, must only be used with a |
| | | * MKSEMI integrated circuit. |
| | | * |
| | | * 5. Any software provided in binary form under this license must not be |
| | | * reverse engineered, decompiled, modified and/or disassembled. |
| | | * |
| | | * THIS SOFTWARE IS PROVIDED BY MKSEMI "AS IS" AND ANY EXPRESS OR IMPLIED |
| | | * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF |
| | | * MERCHANTABILITY, NONINFRINGEMENT, AND FITNESS FOR A PARTICULAR PURPOSE ARE |
| | | * DISCLAIMED. IN NO EVENT SHALL MKSEMI OR CONTRIBUTORS BE LIABLE FOR ANY |
| | | * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES |
| | | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; |
| | | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND |
| | | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT |
| | | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF |
| | | * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. |
| | | */ |
| | | |
| | | #include "mk_trace.h" |
| | | #include "mk_clock.h" |
| | | #include "mk_misc.h" |
| | | #include "aoa.h" |
| | | #include "uwb_api.h" |
| | | #include "lib_ranging.h" |
| | | #include "lib_aoa.h" |
| | | #if PDOA_3D_EN |
| | | #include "lib_pdoa_3d.h" |
| | | #endif |
| | | #include "board.h" |
| | | |
| | | uint8_t aoa_done_flag = 1; |
| | | static struct AOA_REPORT_T report; |
| | | |
| | | /*************************************************************************************************/ |
| | | /*! |
| | | * \brief WSF event handler for aoa task. |
| | | * |
| | | * \param event WSF event mask. |
| | | * \param msg WSF message. |
| | | * |
| | | * \return None. |
| | | */ |
| | | /*************************************************************************************************/ |
| | | void aoa_handler(wsfEventMask_t event, const void *param) |
| | | { |
| | | const wsfMsgHdr_t *msg = (const wsfMsgHdr_t *)param; |
| | | |
| | | if (msg != NULL) |
| | | { |
| | | switch (msg->event) |
| | | { |
| | | case AOA_DONE_MSG: |
| | | { |
| | | const struct AOA_DONE_IND_T *ind = (const struct AOA_DONE_IND_T *)param; |
| | | // TODO: aoa caculation |
| | | if (sts_valid_check()) |
| | | { |
| | | report.sts_valid = 1; |
| | | ranging_first_path_detect(ind->rssi); |
| | | |
| | | #if PDOA_3D_EN |
| | | uint16_t target_addr = uwbs_peer_short_addr_get(); |
| | | uint8_t mac_addr[8]; |
| | | mac_addr[0] = target_addr & 0xff; |
| | | mac_addr[1] = (target_addr >> 8) & 0xff; |
| | | // calculate PDoA angles |
| | | pdoa_3d_calculate(mac_addr, &report.elevation, &report.azimuth); |
| | | pdoa_fom_get(NULL, &report.fom); |
| | | #elif AOA_EN |
| | | // update PDoA IQ and calculate AoA angles (depends on aoa_aux_cfg) |
| | | aoa_calculate(&report.elevation, &report.azimuth); |
| | | aoa_fom_get(NULL, &report.fom); |
| | | #endif |
| | | |
| | | #if RSSI_EN |
| | | LOG_INFO(TRACE_MODULE_APP, "RSSI: %ddBm, SNR: %ddB \r\n", ind->rssi, ind->snr); |
| | | #endif |
| | | |
| | | // azimuth correction |
| | | uint16_t distance = 0; |
| | | board_ranging_result_correct(&distance, &report.azimuth, &report.elevation); |
| | | |
| | | report.pdoa[0] = pdoa_select_get(0, 3); |
| | | report.pdoa[1] = pdoa_select_get(1, 3); |
| | | report.pdoa[2] = pdoa_select_get(2, 3); |
| | | |
| | | float *sts_rssi = sts_rssi_output_get(); |
| | | report.sts_rssi[0] = sts_rssi[0]; |
| | | report.sts_rssi[1] = sts_rssi[1]; |
| | | report.sts_rssi[2] = sts_rssi[2]; |
| | | report.sts_rssi[3] = sts_rssi[3]; |
| | | |
| | | float *iq = sts_first_path_iq_get(); |
| | | report.iq[0] = iq[0]; |
| | | report.iq[1] = iq[1]; |
| | | report.iq[2] = iq[2]; |
| | | report.iq[3] = iq[3]; |
| | | report.iq[4] = iq[4]; |
| | | report.iq[5] = iq[5]; |
| | | report.iq[6] = iq[6]; |
| | | report.iq[7] = iq[7]; |
| | | } |
| | | else |
| | | { |
| | | report.sts_valid = 0; |
| | | } |
| | | uwbapi_report_ranging_data(&report); |
| | | aoa_done_flag = 1; |
| | | } |
| | | break; |
| | | |
| | | default: |
| | | break; |
| | | } |
| | | } |
| | | // Handle events |
| | | else if (event) |
| | | { |
| | | } |
| | | } |