flipperzero-firmware/lib/ST25RFAL002/source/st25r3916/st25r3916_aat.c
2020-10-20 14:12:28 +03:00

330 lines
12 KiB
C
Executable File

/******************************************************************************
* \attention
*
* <h2><center>&copy; COPYRIGHT 2020 STMicroelectronics</center></h2>
*
* Licensed under ST MYLIBERTY SOFTWARE LICENSE AGREEMENT (the "License");
* You may not use this file except in compliance with the License.
* You may obtain a copy of the License at:
*
* www.st.com/myliberty
*
* 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,
* AND SPECIFICALLY DISCLAIMING THE IMPLIED WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE, AND NON-INFRINGEMENT.
* See the License for the specific language governing permissions and
* limitations under the License.
*
******************************************************************************/
/*
* PROJECT: ST25R3916 firmware
* Revision:
* LANGUAGE: ISO C99
*/
/*! \file st25r3916_aat.c
*
* \author
*
* \brief ST25R3916 Antenna Tuning
*
* The antenna tuning algorithm tries to find the optimal settings for
* the AAT_A and AAT_B registers, which are connected to variable capacitors
* to tune the antenna matching.
*
*/
/*
******************************************************************************
* INCLUDES
******************************************************************************
*/
#include "st25r3916_aat.h"
#include "utils.h"
#include "st_errno.h"
#include "st25r3916.h"
#include "st25r3916_com.h"
#include "platform.h"
#include "rfal_chip.h"
/*
******************************************************************************
* GLOBAL DEFINES
******************************************************************************
*/
#define ST25R3916_AAT_CAP_DELAY_MAX 10 /*!< Max Variable Capacitor settle delay */
/*
******************************************************************************
* GLOBAL MACROS
******************************************************************************
*/
#define st25r3916AatLog(...) /* platformLog(__VA_ARGS__) */ /*!< Logging macro */
/*
******************************************************************************
* LOCAL FUNCTION PROTOTYPES
******************************************************************************
*/
static ReturnCode aatHillClimb(const struct st25r3916AatTuneParams *tuningParams, struct st25r3916AatTuneResult *tuningStatus);
static int32_t aatGreedyDescent(uint32_t *f_min, const struct st25r3916AatTuneParams *tuningParams, struct st25r3916AatTuneResult *tuningStatus, int32_t previousDir);
static int32_t aatSteepestDescent(uint32_t *f_min, const struct st25r3916AatTuneParams *tuningParams, struct st25r3916AatTuneResult *tuningStatus, int32_t previousDir, int32_t previousDir2);
static ReturnCode aatMeasure(uint8_t serCap, uint8_t parCap, uint8_t *amplitude, uint8_t *phase, uint16_t *measureCnt);
static uint32_t aatCalcF(const struct st25r3916AatTuneParams *tuningParams, uint8_t amplitude, uint8_t phase);
static ReturnCode aatStepDacVals(const struct st25r3916AatTuneParams *tuningParams,uint8_t *a, uint8_t *b, int32_t dir);
/*******************************************************************************/
ReturnCode st25r3916AatTune(const struct st25r3916AatTuneParams *tuningParams, struct st25r3916AatTuneResult *tuningStatus)
{
ReturnCode err;
const struct st25r3916AatTuneParams *tp = tuningParams;
struct st25r3916AatTuneResult *ts = tuningStatus;
struct st25r3916AatTuneParams defaultTuningParams =
{
.aat_a_min=0,
.aat_a_max=255,
.aat_a_start=127,
.aat_a_stepWidth=32,
.aat_b_min=0,
.aat_b_max=255,
.aat_b_start=127,
.aat_b_stepWidth=32,
.phaTarget=128,
.phaWeight=2,
.ampTarget=196,
.ampWeight=1,
.doDynamicSteps=true,
.measureLimit=50,
};
struct st25r3916AatTuneResult defaultTuneResult;
if ((NULL != tp) && (
(tp->aat_a_min > tp->aat_a_max )
|| (tp->aat_a_start < tp->aat_a_min )
|| (tp->aat_a_start > tp->aat_a_max )
|| (tp->aat_b_min > tp->aat_b_max )
|| (tp->aat_b_start < tp->aat_b_min )
|| (tp->aat_b_start > tp->aat_b_max )
))
{
return ERR_PARAM;
}
if (NULL == tp)
{ /* Start from current caps with default params */
st25r3916ReadRegister(ST25R3916_REG_ANT_TUNE_A, &defaultTuningParams.aat_a_start);
st25r3916ReadRegister(ST25R3916_REG_ANT_TUNE_B, &defaultTuningParams.aat_b_start);
tp = &defaultTuningParams;
}
if (NULL == ts){ts = &defaultTuneResult;}
ts->measureCnt = 0; /* Clear current measure count */
err = aatHillClimb(tp, ts);
return err;
}
/*******************************************************************************/
static ReturnCode aatHillClimb(const struct st25r3916AatTuneParams *tuningParams, struct st25r3916AatTuneResult *tuningStatus)
{
ReturnCode err = ERR_NONE;
uint32_t f_min;
int32_t direction, gdirection;
uint8_t amp,phs;
struct st25r3916AatTuneParams tp = *tuningParams; // local copy to obey const
tuningStatus->aat_a = tuningParams->aat_a_start;
tuningStatus->aat_b = tuningParams->aat_b_start;
/* Get a proper start value */
aatMeasure(tuningStatus->aat_a,tuningStatus->aat_b,&amp,&phs,&tuningStatus->measureCnt);
f_min = aatCalcF(&tp, amp, phs);
direction = 0;
st25r3916AatLog("%d %d: %d***\n",tuningStatus->aat_a,tuningStatus->aat_b,f_min);
do {
direction = 0; /* Initially and after reducing step sizes we don't have a previous direction */
do {
/* With the greedy step below always executed aftwards the -direction does never need to be investigated */
direction = aatSteepestDescent(&f_min, &tp, tuningStatus, direction, -direction);
if (tuningStatus->measureCnt > tp.measureLimit)
{
err = ERR_OVERRUN;
break;
}
do
{
gdirection = aatGreedyDescent(&f_min, &tp, tuningStatus, direction);
if (tuningStatus->measureCnt > tp.measureLimit) {
err = ERR_OVERRUN;
break;
}
} while (0 != gdirection);
} while (0 != direction);
tp.aat_a_stepWidth /= 2U; /* Reduce step sizes */
tp.aat_b_stepWidth /= 2U;
} while (tp.doDynamicSteps && ((tp.aat_a_stepWidth>0U) || (tp.aat_b_stepWidth>0U)));
return err;
}
/*******************************************************************************/
static int32_t aatSteepestDescent(uint32_t *f_min, const struct st25r3916AatTuneParams *tuningParams, struct st25r3916AatTuneResult *tuningStatus, int32_t previousDir, int32_t previousDir2)
{
int32_t i;
uint8_t amp,phs;
uint32_t f;
int32_t bestdir = 0; /* Negative direction: decrease, Positive: increase. (-)1: aat_a, (-)2: aat_b */
for (i = -2; i <= 2; i++)
{
uint8_t a = tuningStatus->aat_a , b = tuningStatus->aat_b;
if ((0==i) || (i==-previousDir) || (i==-previousDir2))
{ /* Skip no direction and avoid going backwards */
continue;
}
if (0U!=aatStepDacVals(tuningParams, &a, &b, i))
{ /* If stepping did not change the value, omit this direction */
continue;
}
aatMeasure(a,b,&amp,&phs,&tuningStatus->measureCnt);
f = aatCalcF(tuningParams, amp, phs);
st25r3916AatLog("%d : %d %d: %d",i,a, b, f);
if (f < *f_min)
{ /* Value is better than all previous ones */
st25r3916AatLog("*");
*f_min = f;
bestdir = i;
}
st25r3916AatLog("\n");
}
if (0!=bestdir)
{ /* Walk into the best direction */
aatStepDacVals(tuningParams, &tuningStatus->aat_a, &tuningStatus->aat_b, bestdir);
}
return bestdir;
}
/*******************************************************************************/
static int32_t aatGreedyDescent(uint32_t *f_min, const struct st25r3916AatTuneParams *tuningParams, struct st25r3916AatTuneResult *tuningStatus, int32_t previousDir)
{
uint8_t amp,phs;
uint32_t f;
uint8_t a = tuningStatus->aat_a , b = tuningStatus->aat_b;
if (0U != aatStepDacVals(tuningParams, &a, &b, previousDir))
{ /* If stepping did not change the value, omit this direction */
return 0;
}
aatMeasure(a,b,&amp,&phs,&tuningStatus->measureCnt);
f = aatCalcF(tuningParams, amp, phs);
st25r3916AatLog("g : %d %d: %d",a, b, f);
if (f < *f_min)
{ /* Value is better than previous one */
st25r3916AatLog("*\n");
tuningStatus->aat_a = a;
tuningStatus->aat_b = b;
*f_min = f;
return previousDir;
}
st25r3916AatLog("\n");
return 0;
}
/*******************************************************************************/
static uint32_t aatCalcF(const struct st25r3916AatTuneParams *tuningParams, uint8_t amplitude, uint8_t phase)
{
/* f(amp, pha) = (ampWeight * |amp - ampTarget|) + (phaWeight * |pha - phaTarget|) */
uint8_t ampTarget = tuningParams->ampTarget;
uint8_t phaTarget = tuningParams->phaTarget;
uint32_t ampWeight = tuningParams->ampWeight;
uint32_t phaWeight = tuningParams->phaWeight;
/* Temp variables to avoid MISRA R10.8 (cast on composite expression) */
uint8_t ad = ((amplitude > ampTarget) ? (amplitude - ampTarget) : (ampTarget - amplitude));
uint8_t pd = ((phase > phaTarget) ? (phase - phaTarget) : (phaTarget - phase));
uint32_t ampDelta = (uint32_t)ad;
uint32_t phaDelta = (uint32_t)pd;
return ((ampWeight * ampDelta) + (phaWeight * phaDelta));
}
/*******************************************************************************/
static ReturnCode aatStepDacVals(const struct st25r3916AatTuneParams *tuningParams,uint8_t *a, uint8_t *b, int32_t dir)
{
int16_t aat_a = (int16_t)*a, aat_b = (int16_t)*b;
switch (abs(dir))
{ /* Advance by steps size in requested direction */
case 1:
aat_a = (dir<0)?(aat_a - (int16_t)tuningParams->aat_a_stepWidth):(aat_a + (int16_t)tuningParams->aat_a_stepWidth);
if(aat_a < (int16_t)tuningParams->aat_a_min){ aat_a = (int16_t)tuningParams->aat_a_min; }
if(aat_a > (int16_t)tuningParams->aat_a_max){ aat_a = (int16_t)tuningParams->aat_a_max; }
if ((int16_t)*a == aat_a) {return ERR_PARAM;}
break;
case 2:
aat_b = (dir<0)?(aat_b - (int16_t)tuningParams->aat_b_stepWidth):(aat_b + (int16_t)tuningParams->aat_b_stepWidth);
if(aat_b < (int16_t)tuningParams->aat_b_min){ aat_b = (int16_t)tuningParams->aat_b_min; }
if(aat_b > (int16_t)tuningParams->aat_b_max){ aat_b = (int16_t)tuningParams->aat_b_max; }
if ((int16_t)*b == aat_b) {return ERR_PARAM;}
break;
default:
return ERR_REQUEST;
}
/* We only get here if actual values have changed. In all other cases an error is returned */
*a = (uint8_t)aat_a;
*b = (uint8_t)aat_b;
return ERR_NONE;
}
/*******************************************************************************/
static ReturnCode aatMeasure(uint8_t serCap, uint8_t parCap, uint8_t *amplitude, uint8_t *phase, uint16_t *measureCnt)
{
ReturnCode err;
*amplitude = 0;
*phase = 0;
st25r3916WriteRegister(ST25R3916_REG_ANT_TUNE_A, serCap);
st25r3916WriteRegister(ST25R3916_REG_ANT_TUNE_B, parCap);
/* Wait till caps have settled.. */
platformDelay( ST25R3916_AAT_CAP_DELAY_MAX );
/* Get amplitude and phase .. */
err = rfalChipMeasureAmplitude(amplitude);
if (ERR_NONE == err)
{
err = rfalChipMeasurePhase(phase);
}
if( measureCnt != NULL )
{
(*measureCnt)++;
}
return err;
}