mirror of
https://github.com/opentx/opentx.git
synced 2025-07-24 00:35:18 +03:00
CRLF removed
This commit is contained in:
parent
691f4aa0ae
commit
e9bcb05a93
14 changed files with 7545 additions and 7545 deletions
|
@ -18,37 +18,37 @@
|
|||
* GNU General Public License for more details.
|
||||
*/
|
||||
|
||||
#include "opentx.h"
|
||||
|
||||
uint8_t g_beepCnt;
|
||||
uint8_t beepAgain = 0;
|
||||
uint8_t beepAgainOrig = 0;
|
||||
uint8_t beepOn = false;
|
||||
bool warble = false;
|
||||
bool warbleC;
|
||||
|
||||
// The various "beep" tone lengths
|
||||
static const pm_uint8_t beepTab[] PROGMEM = {
|
||||
// key, trim, warn2, warn1, error
|
||||
1, 1, 2, 10, 60, //xShort
|
||||
1, 1, 4, 20, 80, //short
|
||||
1, 1, 8, 30, 100, //normal
|
||||
2, 2, 15, 40, 120, //long
|
||||
5, 5, 30, 50, 150, //xLong
|
||||
};
|
||||
|
||||
void beep(uint8_t val)
|
||||
{
|
||||
#if defined(HAPTIC) && !defined(AUDIO)
|
||||
haptic.event(val==0 ? AU_KEYPAD_UP : (val==4 ? AU_ERROR : AU_TIMER_LT10+beepAgain));
|
||||
#endif
|
||||
|
||||
#if !defined(AUDIO)
|
||||
if (g_eeGeneral.alarmsFlash && val>1) flashCounter = FLASH_DURATION;
|
||||
#endif
|
||||
|
||||
if (g_eeGeneral.beepMode>0 || (g_eeGeneral.beepMode==0 && val!=0) || (g_eeGeneral.beepMode==-1 && val>=3)) {
|
||||
_beep(pgm_read_byte(beepTab+5*(2+g_eeGeneral.beepLength)+val));
|
||||
}
|
||||
}
|
||||
|
||||
#include "opentx.h"
|
||||
|
||||
uint8_t g_beepCnt;
|
||||
uint8_t beepAgain = 0;
|
||||
uint8_t beepAgainOrig = 0;
|
||||
uint8_t beepOn = false;
|
||||
bool warble = false;
|
||||
bool warbleC;
|
||||
|
||||
// The various "beep" tone lengths
|
||||
static const pm_uint8_t beepTab[] PROGMEM = {
|
||||
// key, trim, warn2, warn1, error
|
||||
1, 1, 2, 10, 60, //xShort
|
||||
1, 1, 4, 20, 80, //short
|
||||
1, 1, 8, 30, 100, //normal
|
||||
2, 2, 15, 40, 120, //long
|
||||
5, 5, 30, 50, 150, //xLong
|
||||
};
|
||||
|
||||
void beep(uint8_t val)
|
||||
{
|
||||
#if defined(HAPTIC) && !defined(AUDIO)
|
||||
haptic.event(val==0 ? AU_KEYPAD_UP : (val==4 ? AU_ERROR : AU_TIMER_LT10+beepAgain));
|
||||
#endif
|
||||
|
||||
#if !defined(AUDIO)
|
||||
if (g_eeGeneral.alarmsFlash && val>1) flashCounter = FLASH_DURATION;
|
||||
#endif
|
||||
|
||||
if (g_eeGeneral.beepMode>0 || (g_eeGeneral.beepMode==0 && val!=0) || (g_eeGeneral.beepMode==-1 && val>=3)) {
|
||||
_beep(pgm_read_byte(beepTab+5*(2+g_eeGeneral.beepLength)+val));
|
||||
}
|
||||
}
|
||||
|
||||
|
|
1334
radio/src/cli.cpp
1334
radio/src/cli.cpp
File diff suppressed because it is too large
Load diff
|
@ -1,437 +1,437 @@
|
|||
/*
|
||||
* Copyright (C) OpenTX
|
||||
*
|
||||
* Based on code named
|
||||
* th9x - http://code.google.com/p/th9x
|
||||
* er9x - http://code.google.com/p/er9x
|
||||
* gruvin9x - http://code.google.com/p/gruvin9x
|
||||
*
|
||||
* License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
|
||||
*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License version 2 as
|
||||
* published by the Free Software Foundation.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*/
|
||||
|
||||
#include "opentx.h"
|
||||
|
||||
#if defined(XCURVES)
|
||||
int8_t *curveEnd[MAX_CURVES];
|
||||
void loadCurves()
|
||||
{
|
||||
int8_t * tmp = g_model.points;
|
||||
for (int i=0; i<MAX_CURVES; i++) {
|
||||
switch (g_model.curves[i].type) {
|
||||
case CURVE_TYPE_STANDARD:
|
||||
tmp += 5+g_model.curves[i].points;
|
||||
break;
|
||||
case CURVE_TYPE_CUSTOM:
|
||||
tmp += 8+2*g_model.curves[i].points;
|
||||
break;
|
||||
default:
|
||||
TRACE("Wrong curve type! Fixing...");
|
||||
g_model.curves[i].type = CURVE_TYPE_STANDARD;
|
||||
tmp += 5+g_model.curves[i].points;
|
||||
break;
|
||||
}
|
||||
curveEnd[i] = tmp;
|
||||
}
|
||||
}
|
||||
int8_t *curveAddress(uint8_t idx)
|
||||
{
|
||||
return idx==0 ? g_model.points : curveEnd[idx-1];
|
||||
}
|
||||
#else
|
||||
int8_t *curveAddress(uint8_t idx)
|
||||
{
|
||||
return &g_model.points[idx==0 ? 0 : 5*idx+g_model.curves[idx-1]];
|
||||
}
|
||||
|
||||
CurveInfo curveInfo(uint8_t idx)
|
||||
{
|
||||
CurveInfo result;
|
||||
result.crv = curveAddress(idx);
|
||||
int8_t *next = curveAddress(idx+1);
|
||||
uint8_t size = next - result.crv;
|
||||
if ((size & 1) == 0) {
|
||||
result.points = (size / 2) + 1;
|
||||
result.custom = true;
|
||||
}
|
||||
else {
|
||||
result.points = size;
|
||||
result.custom = false;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined(XCURVES)
|
||||
#define CUSTOM_POINT_X(points, count, idx) ((idx)==0 ? -100 : (((idx)==(count)-1) ? 100 : points[(count)+(idx)-1]))
|
||||
s32 compute_tangent(CurveInfo * crv, int8_t * points, int i)
|
||||
{
|
||||
s32 m=0;
|
||||
uint8_t num_points = crv->points + 5;
|
||||
#define MMULT 1024
|
||||
if (i == 0) {
|
||||
//linear interpolation between 1st 2 points
|
||||
//keep 3 decimal-places for m
|
||||
if (crv->type == CURVE_TYPE_CUSTOM) {
|
||||
int8_t x0 = CUSTOM_POINT_X(points, num_points, 0);
|
||||
int8_t x1 = CUSTOM_POINT_X(points, num_points, 1);
|
||||
if (x1 > x0) m = (MMULT * (points[1] - points[0])) / (x1 - x0);
|
||||
}
|
||||
else {
|
||||
s32 delta = (2 * 100) / (num_points - 1);
|
||||
m = (MMULT * (points[1] - points[0])) / delta;
|
||||
}
|
||||
}
|
||||
else if (i == num_points - 1) {
|
||||
//linear interpolation between last 2 points
|
||||
//keep 3 decimal-places for m
|
||||
if (crv->type == CURVE_TYPE_CUSTOM) {
|
||||
int8_t x0 = CUSTOM_POINT_X(points, num_points, num_points-2);
|
||||
int8_t x1 = CUSTOM_POINT_X(points, num_points, num_points-1);
|
||||
if (x1 > x0) m = (MMULT * (points[num_points-1] - points[num_points-2])) / (x1 - x0);
|
||||
}
|
||||
else {
|
||||
s32 delta = (2 * 100) / (num_points - 1);
|
||||
m = (MMULT * (points[num_points-1] - points[num_points-2])) / delta;
|
||||
}
|
||||
}
|
||||
else {
|
||||
//apply monotone rules from
|
||||
//http://en.wikipedia.org/wiki/Monotone_cubic_interpolation
|
||||
//1) compute slopes of secant lines
|
||||
s32 d0=0, d1=0;
|
||||
if (crv->type == CURVE_TYPE_CUSTOM) {
|
||||
int8_t x0 = CUSTOM_POINT_X(points, num_points, i-1);
|
||||
int8_t x1 = CUSTOM_POINT_X(points, num_points, i);
|
||||
int8_t x2 = CUSTOM_POINT_X(points, num_points, i+1);
|
||||
if (x1 > x0) d0 = (MMULT * (points[i] - points[i-1])) / (x1 - x0);
|
||||
if (x2 > x1) d1 = (MMULT * (points[i+1] - points[i])) / (x2 - x1);
|
||||
}
|
||||
else {
|
||||
s32 delta = (2 * 100) / (num_points - 1);
|
||||
d0 = (MMULT * (points[i] - points[i-1])) / (delta);
|
||||
d1 = (MMULT * (points[i+1] - points[i])) / (delta);
|
||||
}
|
||||
//2) compute initial average tangent
|
||||
m = (d0 + d1) / 2;
|
||||
//3 check for horizontal lines
|
||||
if (d0 == 0 || d1 == 0 || (d0 > 0 && d1 < 0) || (d0 < 0 && d1 > 0)) {
|
||||
m = 0;
|
||||
}
|
||||
else if (MMULT * m / d0 > 3 * MMULT) {
|
||||
m = 3 * d0;
|
||||
}
|
||||
else if (MMULT * m / d1 > 3 * MMULT) {
|
||||
m = 3 * d1;
|
||||
}
|
||||
}
|
||||
return m;
|
||||
}
|
||||
|
||||
/* The following is a hermite cubic spline.
|
||||
The basis functions can be found here:
|
||||
http://en.wikipedia.org/wiki/Cubic_Hermite_spline
|
||||
The tangents are computed via the 'cubic monotone' rules (allowing for local-maxima)
|
||||
*/
|
||||
int16_t hermite_spline(int16_t x, uint8_t idx)
|
||||
{
|
||||
CurveInfo &crv = g_model.curves[idx];
|
||||
int8_t *points = curveAddress(idx);
|
||||
uint8_t count = crv.points+5;
|
||||
bool custom = (crv.type == CURVE_TYPE_CUSTOM);
|
||||
|
||||
if (x < -RESX)
|
||||
x = -RESX;
|
||||
else if (x > RESX)
|
||||
x = RESX;
|
||||
|
||||
for (int i=0; i<count-1; i++) {
|
||||
s32 p0x, p3x;
|
||||
if (custom) {
|
||||
p0x = (i>0 ? calc100toRESX(points[count+i-1]) : -RESX);
|
||||
p3x = (i<count-2 ? calc100toRESX(points[count+i]) : RESX);
|
||||
}
|
||||
else {
|
||||
p0x = -RESX + (i*2*RESX)/(count-1);
|
||||
p3x = -RESX + ((i+1)*2*RESX)/(count-1);
|
||||
}
|
||||
|
||||
if (x >= p0x && x <= p3x) {
|
||||
s32 p0y = calc100toRESX(points[i]);
|
||||
s32 p3y = calc100toRESX(points[i+1]);
|
||||
s32 m0 = compute_tangent(&crv, points, i);
|
||||
s32 m3 = compute_tangent(&crv, points, i+1);
|
||||
s32 y;
|
||||
s32 h = p3x - p0x;
|
||||
s32 t = (h > 0 ? (MMULT * (x - p0x)) / h : 0);
|
||||
s32 t2 = t * t / MMULT;
|
||||
s32 t3 = t2 * t / MMULT;
|
||||
s32 h00 = 2*t3 - 3*t2 + MMULT;
|
||||
s32 h10 = t3 - 2*t2 + t;
|
||||
s32 h01 = -2*t3 + 3*t2;
|
||||
s32 h11 = t3 - t2;
|
||||
y = p0y * h00 + h * (m0 * h10 / MMULT) + p3y * h01 + h * (m3 * h11 / MMULT);
|
||||
y /= MMULT;
|
||||
return y;
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
#endif
|
||||
|
||||
int intpol(int x, uint8_t idx) // -100, -75, -50, -25, 0 ,25 ,50, 75, 100
|
||||
{
|
||||
#if defined(XCURVES)
|
||||
CurveInfo &crv = g_model.curves[idx];
|
||||
int8_t *points = curveAddress(idx);
|
||||
uint8_t count = crv.points+5;
|
||||
bool custom = (crv.type == CURVE_TYPE_CUSTOM);
|
||||
#else
|
||||
CurveInfo crv = curveInfo(idx);
|
||||
int8_t *points = crv.crv;
|
||||
uint8_t count = crv.points;
|
||||
bool custom = crv.custom;
|
||||
#endif
|
||||
int16_t erg = 0;
|
||||
|
||||
x += RESXu;
|
||||
|
||||
if (x <= 0) {
|
||||
erg = (int16_t)points[0] * (RESX/4);
|
||||
}
|
||||
else if (x >= (RESX*2)) {
|
||||
erg = (int16_t)points[count-1] * (RESX/4);
|
||||
}
|
||||
else {
|
||||
uint16_t a=0, b=0;
|
||||
uint8_t i;
|
||||
if (custom) {
|
||||
for (i=0; i<count-1; i++) {
|
||||
a = b;
|
||||
b = (i==count-2 ? 2*RESX : RESX + calc100toRESX(points[count+i]));
|
||||
if ((uint16_t)x<=b) break;
|
||||
}
|
||||
}
|
||||
else {
|
||||
uint16_t d = (RESX * 2) / (count-1);
|
||||
i = (uint16_t)x / d;
|
||||
a = i * d;
|
||||
b = a + d;
|
||||
}
|
||||
erg = (int16_t)points[i]*(RESX/4) + ((int32_t)(x-a) * (points[i+1]-points[i]) * (RESX/4)) / ((b-a));
|
||||
}
|
||||
|
||||
return erg / 25; // 100*D5/RESX;
|
||||
}
|
||||
|
||||
#if defined(CURVES) && defined(XCURVES)
|
||||
int applyCurve(int x, CurveRef & curve)
|
||||
{
|
||||
switch (curve.type) {
|
||||
case CURVE_REF_DIFF:
|
||||
{
|
||||
#if defined(CPUARM)
|
||||
int curveParam = GET_GVAR_PREC1(curve.value, -100, 100, mixerCurrentFlightMode);
|
||||
if (curveParam > 0 && x < 0)
|
||||
x = (x * (1000 - curveParam)) / 1000;
|
||||
else if (curveParam < 0 && x > 0)
|
||||
x = (x * (1000 + curveParam)) / 1000;
|
||||
return x;
|
||||
#else
|
||||
int curveParam = calc100to256(GET_GVAR(curve.value, -100, 100, mixerCurrentFlightMode));
|
||||
if (curveParam > 0 && x < 0)
|
||||
x = (x * (256 - curveParam)) >> 8;
|
||||
else if (curveParam < 0 && x > 0)
|
||||
x = (x * (256 + curveParam)) >> 8;
|
||||
return x;
|
||||
#endif
|
||||
}
|
||||
|
||||
case CURVE_REF_EXPO:
|
||||
{
|
||||
#if defined(CPUARM)
|
||||
int curveParam = GET_GVAR_PREC1(curve.value, -100, 100, mixerCurrentFlightMode) / 10;
|
||||
#else
|
||||
int curveParam = GET_GVAR(curve.value, -100, 100, mixerCurrentFlightMode);
|
||||
#endif
|
||||
return expo(x, curveParam);
|
||||
}
|
||||
|
||||
case CURVE_REF_FUNC:
|
||||
switch (curve.value) {
|
||||
case CURVE_X_GT0:
|
||||
if (x < 0) x = 0; //x|x>0
|
||||
return x;
|
||||
case CURVE_X_LT0:
|
||||
if (x > 0) x = 0; //x|x<0
|
||||
return x;
|
||||
case CURVE_ABS_X: // x|abs(x)
|
||||
return abs(x);
|
||||
case CURVE_F_GT0: //f|f>0
|
||||
return x > 0 ? RESX : 0;
|
||||
case CURVE_F_LT0: //f|f<0
|
||||
return x < 0 ? -RESX : 0;
|
||||
case CURVE_ABS_F: //f|abs(f)
|
||||
return x > 0 ? RESX : -RESX;
|
||||
}
|
||||
break;
|
||||
|
||||
case CURVE_REF_CUSTOM:
|
||||
{
|
||||
int curveParam = curve.value;
|
||||
if (curveParam < 0) {
|
||||
x = -x;
|
||||
curveParam = -curveParam;
|
||||
}
|
||||
if (curveParam > 0 && curveParam <= MAX_CURVES) {
|
||||
return applyCustomCurve(x, curveParam - 1);
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return x;
|
||||
}
|
||||
|
||||
int applyCustomCurve(int x, uint8_t idx)
|
||||
{
|
||||
if (idx >= MAX_CURVES)
|
||||
return 0;
|
||||
|
||||
CurveInfo &crv = g_model.curves[idx];
|
||||
if (crv.smooth)
|
||||
return hermite_spline(x, idx);
|
||||
else
|
||||
return intpol(x, idx);
|
||||
}
|
||||
#elif defined(CURVES)
|
||||
int applyCurve(int x, int8_t idx)
|
||||
{
|
||||
/* already tried to have only one return at the end */
|
||||
switch(idx) {
|
||||
case CURVE_NONE:
|
||||
return x;
|
||||
case CURVE_X_GT0:
|
||||
if (x < 0) x = 0; //x|x>0
|
||||
return x;
|
||||
case CURVE_X_LT0:
|
||||
if (x > 0) x = 0; //x|x<0
|
||||
return x;
|
||||
case CURVE_ABS_X: // x|abs(x)
|
||||
return abs(x);
|
||||
case CURVE_F_GT0: //f|f>0
|
||||
return x > 0 ? RESX : 0;
|
||||
case CURVE_F_LT0: //f|f<0
|
||||
return x < 0 ? -RESX : 0;
|
||||
case CURVE_ABS_F: //f|abs(f)
|
||||
return x > 0 ? RESX : -RESX;
|
||||
}
|
||||
if (idx < 0) {
|
||||
x = -x;
|
||||
idx = -idx + CURVE_BASE - 1;
|
||||
}
|
||||
return applyCustomCurve(x, idx - CURVE_BASE);
|
||||
}
|
||||
#endif
|
||||
|
||||
// #define EXTENDED_EXPO
|
||||
// increases range of expo curve but costs about 82 bytes flash
|
||||
|
||||
// expo-funktion:
|
||||
// ---------------
|
||||
// kmplot
|
||||
// f(x,k)=exp(ln(x)*k/10) ;P[0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20]
|
||||
// f(x,k)=x*x*x*k/10 + x*(1-k/10) ;P[0,1,2,3,4,5,6,7,8,9,10]
|
||||
// f(x,k)=x*x*k/10 + x*(1-k/10) ;P[0,1,2,3,4,5,6,7,8,9,10]
|
||||
// f(x,k)=1+(x-1)*(x-1)*(x-1)*k/10 + (x-1)*(1-k/10) ;P[0,1,2,3,4,5,6,7,8,9,10]
|
||||
// don't know what this above should be, just confusing in my opinion,
|
||||
|
||||
// here is the real explanation
|
||||
// actually the real formula is
|
||||
/*
|
||||
f(x) = exp( ln(x) * 10^k)
|
||||
if it is 10^k or e^k or 2^k etc. just defines the max distortion of the expo curve; I think 10 is useful
|
||||
this gives values from 0 to 1 for x and output; k must be between -1 and +1
|
||||
we do not like to calculate with floating point. Therefore we rescale for x from 0 to 1024 and for k from -100 to +100
|
||||
f(x) = 1024 * ( e^( ln(x/1024) * 10^(k/100) ) )
|
||||
This would be really hard to be calculated by such a microcontroller
|
||||
Therefore Thomas Husterer compared a few usual function something like x^3, x^4*something, which look similar
|
||||
Actually the formula
|
||||
f(x) = k*x^3+x*(1-k)
|
||||
gives a similar form and should have even advantages compared to a original exp curve.
|
||||
This function again expect x from 0 to 1 and k only from 0 to 1
|
||||
Therefore rescaling is needed like before:
|
||||
f(x) = 1024* ((k/100)*(x/1024)^3 + (x/1024)*(100-k)/100)
|
||||
some mathematical tricks
|
||||
f(x) = (k*x*x*x/(1024*1024) + x*(100-k)) / 100
|
||||
for better rounding results we add the 50
|
||||
f(x) = (k*x*x*x/(1024*1024) + x*(100-k) + 50) / 100
|
||||
|
||||
because we now understand the formula, we can optimize it further
|
||||
--> calc100to256(k) --> eliminates /100 by replacing with /256 which is just a simple shift right 8
|
||||
k is now between 0 and 256
|
||||
f(x) = (k*x*x*x/(1024*1024) + x*(256-k) + 128) / 256
|
||||
*/
|
||||
|
||||
// input parameters;
|
||||
// x 0 to 1024;
|
||||
// k 0 to 100;
|
||||
// output between 0 and 1024
|
||||
unsigned int expou(unsigned int x, unsigned int k)
|
||||
{
|
||||
#if defined(EXTENDED_EXPO)
|
||||
bool extended;
|
||||
if (k>80) {
|
||||
extended=true;
|
||||
}
|
||||
else {
|
||||
k += (k>>2); // use bigger values before extend, because the effect is anyway very very low
|
||||
extended=false;
|
||||
}
|
||||
#endif
|
||||
|
||||
k = calc100to256(k);
|
||||
|
||||
uint32_t value = (uint32_t) x*x;
|
||||
value *= (uint32_t)k;
|
||||
value >>= 8;
|
||||
value *= (uint32_t)x;
|
||||
|
||||
#if defined(EXTENDED_EXPO)
|
||||
if (extended) { // for higher values do more multiplications to get a stronger expo curve
|
||||
value >>= 16;
|
||||
value *= (uint32_t)x;
|
||||
value >>= 4;
|
||||
value *= (uint32_t)x;
|
||||
}
|
||||
#endif
|
||||
|
||||
value >>= 12;
|
||||
value += (uint32_t)(256-k)*x+128;
|
||||
|
||||
return value>>8;
|
||||
}
|
||||
|
||||
int expo(int x, int k)
|
||||
{
|
||||
if (k == 0) return x;
|
||||
int y;
|
||||
bool neg = (x < 0);
|
||||
|
||||
if (neg) x = -x;
|
||||
if (k<0) {
|
||||
y = RESXu-expou(RESXu-x, -k);
|
||||
}
|
||||
else {
|
||||
y = expou(x, k);
|
||||
}
|
||||
return neg? -y : y;
|
||||
}
|
||||
/*
|
||||
* Copyright (C) OpenTX
|
||||
*
|
||||
* Based on code named
|
||||
* th9x - http://code.google.com/p/th9x
|
||||
* er9x - http://code.google.com/p/er9x
|
||||
* gruvin9x - http://code.google.com/p/gruvin9x
|
||||
*
|
||||
* License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
|
||||
*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License version 2 as
|
||||
* published by the Free Software Foundation.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*/
|
||||
|
||||
#include "opentx.h"
|
||||
|
||||
#if defined(XCURVES)
|
||||
int8_t *curveEnd[MAX_CURVES];
|
||||
void loadCurves()
|
||||
{
|
||||
int8_t * tmp = g_model.points;
|
||||
for (int i=0; i<MAX_CURVES; i++) {
|
||||
switch (g_model.curves[i].type) {
|
||||
case CURVE_TYPE_STANDARD:
|
||||
tmp += 5+g_model.curves[i].points;
|
||||
break;
|
||||
case CURVE_TYPE_CUSTOM:
|
||||
tmp += 8+2*g_model.curves[i].points;
|
||||
break;
|
||||
default:
|
||||
TRACE("Wrong curve type! Fixing...");
|
||||
g_model.curves[i].type = CURVE_TYPE_STANDARD;
|
||||
tmp += 5+g_model.curves[i].points;
|
||||
break;
|
||||
}
|
||||
curveEnd[i] = tmp;
|
||||
}
|
||||
}
|
||||
int8_t *curveAddress(uint8_t idx)
|
||||
{
|
||||
return idx==0 ? g_model.points : curveEnd[idx-1];
|
||||
}
|
||||
#else
|
||||
int8_t *curveAddress(uint8_t idx)
|
||||
{
|
||||
return &g_model.points[idx==0 ? 0 : 5*idx+g_model.curves[idx-1]];
|
||||
}
|
||||
|
||||
CurveInfo curveInfo(uint8_t idx)
|
||||
{
|
||||
CurveInfo result;
|
||||
result.crv = curveAddress(idx);
|
||||
int8_t *next = curveAddress(idx+1);
|
||||
uint8_t size = next - result.crv;
|
||||
if ((size & 1) == 0) {
|
||||
result.points = (size / 2) + 1;
|
||||
result.custom = true;
|
||||
}
|
||||
else {
|
||||
result.points = size;
|
||||
result.custom = false;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined(XCURVES)
|
||||
#define CUSTOM_POINT_X(points, count, idx) ((idx)==0 ? -100 : (((idx)==(count)-1) ? 100 : points[(count)+(idx)-1]))
|
||||
s32 compute_tangent(CurveInfo * crv, int8_t * points, int i)
|
||||
{
|
||||
s32 m=0;
|
||||
uint8_t num_points = crv->points + 5;
|
||||
#define MMULT 1024
|
||||
if (i == 0) {
|
||||
//linear interpolation between 1st 2 points
|
||||
//keep 3 decimal-places for m
|
||||
if (crv->type == CURVE_TYPE_CUSTOM) {
|
||||
int8_t x0 = CUSTOM_POINT_X(points, num_points, 0);
|
||||
int8_t x1 = CUSTOM_POINT_X(points, num_points, 1);
|
||||
if (x1 > x0) m = (MMULT * (points[1] - points[0])) / (x1 - x0);
|
||||
}
|
||||
else {
|
||||
s32 delta = (2 * 100) / (num_points - 1);
|
||||
m = (MMULT * (points[1] - points[0])) / delta;
|
||||
}
|
||||
}
|
||||
else if (i == num_points - 1) {
|
||||
//linear interpolation between last 2 points
|
||||
//keep 3 decimal-places for m
|
||||
if (crv->type == CURVE_TYPE_CUSTOM) {
|
||||
int8_t x0 = CUSTOM_POINT_X(points, num_points, num_points-2);
|
||||
int8_t x1 = CUSTOM_POINT_X(points, num_points, num_points-1);
|
||||
if (x1 > x0) m = (MMULT * (points[num_points-1] - points[num_points-2])) / (x1 - x0);
|
||||
}
|
||||
else {
|
||||
s32 delta = (2 * 100) / (num_points - 1);
|
||||
m = (MMULT * (points[num_points-1] - points[num_points-2])) / delta;
|
||||
}
|
||||
}
|
||||
else {
|
||||
//apply monotone rules from
|
||||
//http://en.wikipedia.org/wiki/Monotone_cubic_interpolation
|
||||
//1) compute slopes of secant lines
|
||||
s32 d0=0, d1=0;
|
||||
if (crv->type == CURVE_TYPE_CUSTOM) {
|
||||
int8_t x0 = CUSTOM_POINT_X(points, num_points, i-1);
|
||||
int8_t x1 = CUSTOM_POINT_X(points, num_points, i);
|
||||
int8_t x2 = CUSTOM_POINT_X(points, num_points, i+1);
|
||||
if (x1 > x0) d0 = (MMULT * (points[i] - points[i-1])) / (x1 - x0);
|
||||
if (x2 > x1) d1 = (MMULT * (points[i+1] - points[i])) / (x2 - x1);
|
||||
}
|
||||
else {
|
||||
s32 delta = (2 * 100) / (num_points - 1);
|
||||
d0 = (MMULT * (points[i] - points[i-1])) / (delta);
|
||||
d1 = (MMULT * (points[i+1] - points[i])) / (delta);
|
||||
}
|
||||
//2) compute initial average tangent
|
||||
m = (d0 + d1) / 2;
|
||||
//3 check for horizontal lines
|
||||
if (d0 == 0 || d1 == 0 || (d0 > 0 && d1 < 0) || (d0 < 0 && d1 > 0)) {
|
||||
m = 0;
|
||||
}
|
||||
else if (MMULT * m / d0 > 3 * MMULT) {
|
||||
m = 3 * d0;
|
||||
}
|
||||
else if (MMULT * m / d1 > 3 * MMULT) {
|
||||
m = 3 * d1;
|
||||
}
|
||||
}
|
||||
return m;
|
||||
}
|
||||
|
||||
/* The following is a hermite cubic spline.
|
||||
The basis functions can be found here:
|
||||
http://en.wikipedia.org/wiki/Cubic_Hermite_spline
|
||||
The tangents are computed via the 'cubic monotone' rules (allowing for local-maxima)
|
||||
*/
|
||||
int16_t hermite_spline(int16_t x, uint8_t idx)
|
||||
{
|
||||
CurveInfo &crv = g_model.curves[idx];
|
||||
int8_t *points = curveAddress(idx);
|
||||
uint8_t count = crv.points+5;
|
||||
bool custom = (crv.type == CURVE_TYPE_CUSTOM);
|
||||
|
||||
if (x < -RESX)
|
||||
x = -RESX;
|
||||
else if (x > RESX)
|
||||
x = RESX;
|
||||
|
||||
for (int i=0; i<count-1; i++) {
|
||||
s32 p0x, p3x;
|
||||
if (custom) {
|
||||
p0x = (i>0 ? calc100toRESX(points[count+i-1]) : -RESX);
|
||||
p3x = (i<count-2 ? calc100toRESX(points[count+i]) : RESX);
|
||||
}
|
||||
else {
|
||||
p0x = -RESX + (i*2*RESX)/(count-1);
|
||||
p3x = -RESX + ((i+1)*2*RESX)/(count-1);
|
||||
}
|
||||
|
||||
if (x >= p0x && x <= p3x) {
|
||||
s32 p0y = calc100toRESX(points[i]);
|
||||
s32 p3y = calc100toRESX(points[i+1]);
|
||||
s32 m0 = compute_tangent(&crv, points, i);
|
||||
s32 m3 = compute_tangent(&crv, points, i+1);
|
||||
s32 y;
|
||||
s32 h = p3x - p0x;
|
||||
s32 t = (h > 0 ? (MMULT * (x - p0x)) / h : 0);
|
||||
s32 t2 = t * t / MMULT;
|
||||
s32 t3 = t2 * t / MMULT;
|
||||
s32 h00 = 2*t3 - 3*t2 + MMULT;
|
||||
s32 h10 = t3 - 2*t2 + t;
|
||||
s32 h01 = -2*t3 + 3*t2;
|
||||
s32 h11 = t3 - t2;
|
||||
y = p0y * h00 + h * (m0 * h10 / MMULT) + p3y * h01 + h * (m3 * h11 / MMULT);
|
||||
y /= MMULT;
|
||||
return y;
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
#endif
|
||||
|
||||
int intpol(int x, uint8_t idx) // -100, -75, -50, -25, 0 ,25 ,50, 75, 100
|
||||
{
|
||||
#if defined(XCURVES)
|
||||
CurveInfo &crv = g_model.curves[idx];
|
||||
int8_t *points = curveAddress(idx);
|
||||
uint8_t count = crv.points+5;
|
||||
bool custom = (crv.type == CURVE_TYPE_CUSTOM);
|
||||
#else
|
||||
CurveInfo crv = curveInfo(idx);
|
||||
int8_t *points = crv.crv;
|
||||
uint8_t count = crv.points;
|
||||
bool custom = crv.custom;
|
||||
#endif
|
||||
int16_t erg = 0;
|
||||
|
||||
x += RESXu;
|
||||
|
||||
if (x <= 0) {
|
||||
erg = (int16_t)points[0] * (RESX/4);
|
||||
}
|
||||
else if (x >= (RESX*2)) {
|
||||
erg = (int16_t)points[count-1] * (RESX/4);
|
||||
}
|
||||
else {
|
||||
uint16_t a=0, b=0;
|
||||
uint8_t i;
|
||||
if (custom) {
|
||||
for (i=0; i<count-1; i++) {
|
||||
a = b;
|
||||
b = (i==count-2 ? 2*RESX : RESX + calc100toRESX(points[count+i]));
|
||||
if ((uint16_t)x<=b) break;
|
||||
}
|
||||
}
|
||||
else {
|
||||
uint16_t d = (RESX * 2) / (count-1);
|
||||
i = (uint16_t)x / d;
|
||||
a = i * d;
|
||||
b = a + d;
|
||||
}
|
||||
erg = (int16_t)points[i]*(RESX/4) + ((int32_t)(x-a) * (points[i+1]-points[i]) * (RESX/4)) / ((b-a));
|
||||
}
|
||||
|
||||
return erg / 25; // 100*D5/RESX;
|
||||
}
|
||||
|
||||
#if defined(CURVES) && defined(XCURVES)
|
||||
int applyCurve(int x, CurveRef & curve)
|
||||
{
|
||||
switch (curve.type) {
|
||||
case CURVE_REF_DIFF:
|
||||
{
|
||||
#if defined(CPUARM)
|
||||
int curveParam = GET_GVAR_PREC1(curve.value, -100, 100, mixerCurrentFlightMode);
|
||||
if (curveParam > 0 && x < 0)
|
||||
x = (x * (1000 - curveParam)) / 1000;
|
||||
else if (curveParam < 0 && x > 0)
|
||||
x = (x * (1000 + curveParam)) / 1000;
|
||||
return x;
|
||||
#else
|
||||
int curveParam = calc100to256(GET_GVAR(curve.value, -100, 100, mixerCurrentFlightMode));
|
||||
if (curveParam > 0 && x < 0)
|
||||
x = (x * (256 - curveParam)) >> 8;
|
||||
else if (curveParam < 0 && x > 0)
|
||||
x = (x * (256 + curveParam)) >> 8;
|
||||
return x;
|
||||
#endif
|
||||
}
|
||||
|
||||
case CURVE_REF_EXPO:
|
||||
{
|
||||
#if defined(CPUARM)
|
||||
int curveParam = GET_GVAR_PREC1(curve.value, -100, 100, mixerCurrentFlightMode) / 10;
|
||||
#else
|
||||
int curveParam = GET_GVAR(curve.value, -100, 100, mixerCurrentFlightMode);
|
||||
#endif
|
||||
return expo(x, curveParam);
|
||||
}
|
||||
|
||||
case CURVE_REF_FUNC:
|
||||
switch (curve.value) {
|
||||
case CURVE_X_GT0:
|
||||
if (x < 0) x = 0; //x|x>0
|
||||
return x;
|
||||
case CURVE_X_LT0:
|
||||
if (x > 0) x = 0; //x|x<0
|
||||
return x;
|
||||
case CURVE_ABS_X: // x|abs(x)
|
||||
return abs(x);
|
||||
case CURVE_F_GT0: //f|f>0
|
||||
return x > 0 ? RESX : 0;
|
||||
case CURVE_F_LT0: //f|f<0
|
||||
return x < 0 ? -RESX : 0;
|
||||
case CURVE_ABS_F: //f|abs(f)
|
||||
return x > 0 ? RESX : -RESX;
|
||||
}
|
||||
break;
|
||||
|
||||
case CURVE_REF_CUSTOM:
|
||||
{
|
||||
int curveParam = curve.value;
|
||||
if (curveParam < 0) {
|
||||
x = -x;
|
||||
curveParam = -curveParam;
|
||||
}
|
||||
if (curveParam > 0 && curveParam <= MAX_CURVES) {
|
||||
return applyCustomCurve(x, curveParam - 1);
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return x;
|
||||
}
|
||||
|
||||
int applyCustomCurve(int x, uint8_t idx)
|
||||
{
|
||||
if (idx >= MAX_CURVES)
|
||||
return 0;
|
||||
|
||||
CurveInfo &crv = g_model.curves[idx];
|
||||
if (crv.smooth)
|
||||
return hermite_spline(x, idx);
|
||||
else
|
||||
return intpol(x, idx);
|
||||
}
|
||||
#elif defined(CURVES)
|
||||
int applyCurve(int x, int8_t idx)
|
||||
{
|
||||
/* already tried to have only one return at the end */
|
||||
switch(idx) {
|
||||
case CURVE_NONE:
|
||||
return x;
|
||||
case CURVE_X_GT0:
|
||||
if (x < 0) x = 0; //x|x>0
|
||||
return x;
|
||||
case CURVE_X_LT0:
|
||||
if (x > 0) x = 0; //x|x<0
|
||||
return x;
|
||||
case CURVE_ABS_X: // x|abs(x)
|
||||
return abs(x);
|
||||
case CURVE_F_GT0: //f|f>0
|
||||
return x > 0 ? RESX : 0;
|
||||
case CURVE_F_LT0: //f|f<0
|
||||
return x < 0 ? -RESX : 0;
|
||||
case CURVE_ABS_F: //f|abs(f)
|
||||
return x > 0 ? RESX : -RESX;
|
||||
}
|
||||
if (idx < 0) {
|
||||
x = -x;
|
||||
idx = -idx + CURVE_BASE - 1;
|
||||
}
|
||||
return applyCustomCurve(x, idx - CURVE_BASE);
|
||||
}
|
||||
#endif
|
||||
|
||||
// #define EXTENDED_EXPO
|
||||
// increases range of expo curve but costs about 82 bytes flash
|
||||
|
||||
// expo-funktion:
|
||||
// ---------------
|
||||
// kmplot
|
||||
// f(x,k)=exp(ln(x)*k/10) ;P[0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20]
|
||||
// f(x,k)=x*x*x*k/10 + x*(1-k/10) ;P[0,1,2,3,4,5,6,7,8,9,10]
|
||||
// f(x,k)=x*x*k/10 + x*(1-k/10) ;P[0,1,2,3,4,5,6,7,8,9,10]
|
||||
// f(x,k)=1+(x-1)*(x-1)*(x-1)*k/10 + (x-1)*(1-k/10) ;P[0,1,2,3,4,5,6,7,8,9,10]
|
||||
// don't know what this above should be, just confusing in my opinion,
|
||||
|
||||
// here is the real explanation
|
||||
// actually the real formula is
|
||||
/*
|
||||
f(x) = exp( ln(x) * 10^k)
|
||||
if it is 10^k or e^k or 2^k etc. just defines the max distortion of the expo curve; I think 10 is useful
|
||||
this gives values from 0 to 1 for x and output; k must be between -1 and +1
|
||||
we do not like to calculate with floating point. Therefore we rescale for x from 0 to 1024 and for k from -100 to +100
|
||||
f(x) = 1024 * ( e^( ln(x/1024) * 10^(k/100) ) )
|
||||
This would be really hard to be calculated by such a microcontroller
|
||||
Therefore Thomas Husterer compared a few usual function something like x^3, x^4*something, which look similar
|
||||
Actually the formula
|
||||
f(x) = k*x^3+x*(1-k)
|
||||
gives a similar form and should have even advantages compared to a original exp curve.
|
||||
This function again expect x from 0 to 1 and k only from 0 to 1
|
||||
Therefore rescaling is needed like before:
|
||||
f(x) = 1024* ((k/100)*(x/1024)^3 + (x/1024)*(100-k)/100)
|
||||
some mathematical tricks
|
||||
f(x) = (k*x*x*x/(1024*1024) + x*(100-k)) / 100
|
||||
for better rounding results we add the 50
|
||||
f(x) = (k*x*x*x/(1024*1024) + x*(100-k) + 50) / 100
|
||||
|
||||
because we now understand the formula, we can optimize it further
|
||||
--> calc100to256(k) --> eliminates /100 by replacing with /256 which is just a simple shift right 8
|
||||
k is now between 0 and 256
|
||||
f(x) = (k*x*x*x/(1024*1024) + x*(256-k) + 128) / 256
|
||||
*/
|
||||
|
||||
// input parameters;
|
||||
// x 0 to 1024;
|
||||
// k 0 to 100;
|
||||
// output between 0 and 1024
|
||||
unsigned int expou(unsigned int x, unsigned int k)
|
||||
{
|
||||
#if defined(EXTENDED_EXPO)
|
||||
bool extended;
|
||||
if (k>80) {
|
||||
extended=true;
|
||||
}
|
||||
else {
|
||||
k += (k>>2); // use bigger values before extend, because the effect is anyway very very low
|
||||
extended=false;
|
||||
}
|
||||
#endif
|
||||
|
||||
k = calc100to256(k);
|
||||
|
||||
uint32_t value = (uint32_t) x*x;
|
||||
value *= (uint32_t)k;
|
||||
value >>= 8;
|
||||
value *= (uint32_t)x;
|
||||
|
||||
#if defined(EXTENDED_EXPO)
|
||||
if (extended) { // for higher values do more multiplications to get a stronger expo curve
|
||||
value >>= 16;
|
||||
value *= (uint32_t)x;
|
||||
value >>= 4;
|
||||
value *= (uint32_t)x;
|
||||
}
|
||||
#endif
|
||||
|
||||
value >>= 12;
|
||||
value += (uint32_t)(256-k)*x+128;
|
||||
|
||||
return value>>8;
|
||||
}
|
||||
|
||||
int expo(int x, int k)
|
||||
{
|
||||
if (k == 0) return x;
|
||||
int y;
|
||||
bool neg = (x < 0);
|
||||
|
||||
if (neg) x = -x;
|
||||
if (k<0) {
|
||||
y = RESXu-expou(RESXu-x, -k);
|
||||
}
|
||||
else {
|
||||
y = expou(x, k);
|
||||
}
|
||||
return neg? -y : y;
|
||||
}
|
||||
|
|
|
@ -18,86 +18,86 @@
|
|||
* GNU General Public License for more details.
|
||||
*/
|
||||
|
||||
#include "opentx.h"
|
||||
#include "stamp.h"
|
||||
#include <stdarg.h>
|
||||
|
||||
#if defined(SIMU)
|
||||
traceCallbackFunc traceCallback = 0;
|
||||
#endif
|
||||
|
||||
#if defined(SIMU)
|
||||
#define PRINTF_BUFFER_SIZE 1024
|
||||
void debugPrintf(const char * format, ...)
|
||||
{
|
||||
va_list arglist;
|
||||
char tmp[PRINTF_BUFFER_SIZE];
|
||||
|
||||
va_start(arglist, format);
|
||||
vsnprintf(tmp, PRINTF_BUFFER_SIZE, format, arglist);
|
||||
va_end(arglist);
|
||||
fputs(tmp, stdout);
|
||||
fflush(stdout);
|
||||
if (traceCallback) {
|
||||
traceCallback(tmp);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined(DEBUG_TRACE_BUFFER)
|
||||
static struct TraceElement traceBuffer[TRACE_BUFFER_LEN];
|
||||
static uint8_t traceBufferPos;
|
||||
gtime_t filltm(gtime_t *t, struct gtm *tp);
|
||||
|
||||
void trace_event(enum TraceEvent event, uint32_t data)
|
||||
{
|
||||
if (traceBufferPos >= TRACE_BUFFER_LEN) return;
|
||||
__disable_irq();
|
||||
struct TraceElement * p = &traceBuffer[traceBufferPos++];
|
||||
__enable_irq();
|
||||
p->time = g_rtcTime;
|
||||
p->time_ms = g_ms100;
|
||||
p->event = event;
|
||||
p->data = data;
|
||||
}
|
||||
|
||||
void trace_event_i(enum TraceEvent event, uint32_t data)
|
||||
{
|
||||
if (traceBufferPos >= TRACE_BUFFER_LEN) return;
|
||||
struct TraceElement * p = &traceBuffer[traceBufferPos++];
|
||||
p->time = g_rtcTime;
|
||||
p->time_ms = g_ms100;
|
||||
p->event = event;
|
||||
p->data = data;
|
||||
}
|
||||
|
||||
|
||||
const struct TraceElement * getTraceElement(uint16_t idx)
|
||||
{
|
||||
if (idx < TRACE_BUFFER_LEN) return &traceBuffer[idx];
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
void dumpTraceBuffer()
|
||||
{
|
||||
TRACE("Dump of Trace Buffer (" VERSION " " DATE " " TIME "):");
|
||||
TRACE("# Time Event Data");
|
||||
for(int n = 0; n < TRACE_BUFFER_LEN; ++n) {
|
||||
struct gtm tp;
|
||||
filltm(&traceBuffer[n].time, &tp);
|
||||
TRACE_INFO_WP("%02d ", n);
|
||||
TRACE_INFO_WP("%4d-%02d-%02d,%02d:%02d:%02d.%02d0", tp.tm_year+1900, tp.tm_mon+1, tp.tm_mday, tp.tm_hour, tp.tm_min, tp.tm_sec, traceBuffer[n].time_ms);
|
||||
TRACE(" %03d 0x%08x", traceBuffer[n].event, traceBuffer[n].data);
|
||||
if (traceBuffer[n].time == 0 && traceBuffer[n].time_ms == 0) break;
|
||||
#if !defined(SIMU)
|
||||
if ((n % 5) == 0) {
|
||||
while (!serial2TxFifo.empty()) {
|
||||
CoTickDelay(1);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
TRACE("End of Trace Buffer dump");
|
||||
}
|
||||
#endif
|
||||
#include "opentx.h"
|
||||
#include "stamp.h"
|
||||
#include <stdarg.h>
|
||||
|
||||
#if defined(SIMU)
|
||||
traceCallbackFunc traceCallback = 0;
|
||||
#endif
|
||||
|
||||
#if defined(SIMU)
|
||||
#define PRINTF_BUFFER_SIZE 1024
|
||||
void debugPrintf(const char * format, ...)
|
||||
{
|
||||
va_list arglist;
|
||||
char tmp[PRINTF_BUFFER_SIZE];
|
||||
|
||||
va_start(arglist, format);
|
||||
vsnprintf(tmp, PRINTF_BUFFER_SIZE, format, arglist);
|
||||
va_end(arglist);
|
||||
fputs(tmp, stdout);
|
||||
fflush(stdout);
|
||||
if (traceCallback) {
|
||||
traceCallback(tmp);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined(DEBUG_TRACE_BUFFER)
|
||||
static struct TraceElement traceBuffer[TRACE_BUFFER_LEN];
|
||||
static uint8_t traceBufferPos;
|
||||
gtime_t filltm(gtime_t *t, struct gtm *tp);
|
||||
|
||||
void trace_event(enum TraceEvent event, uint32_t data)
|
||||
{
|
||||
if (traceBufferPos >= TRACE_BUFFER_LEN) return;
|
||||
__disable_irq();
|
||||
struct TraceElement * p = &traceBuffer[traceBufferPos++];
|
||||
__enable_irq();
|
||||
p->time = g_rtcTime;
|
||||
p->time_ms = g_ms100;
|
||||
p->event = event;
|
||||
p->data = data;
|
||||
}
|
||||
|
||||
void trace_event_i(enum TraceEvent event, uint32_t data)
|
||||
{
|
||||
if (traceBufferPos >= TRACE_BUFFER_LEN) return;
|
||||
struct TraceElement * p = &traceBuffer[traceBufferPos++];
|
||||
p->time = g_rtcTime;
|
||||
p->time_ms = g_ms100;
|
||||
p->event = event;
|
||||
p->data = data;
|
||||
}
|
||||
|
||||
|
||||
const struct TraceElement * getTraceElement(uint16_t idx)
|
||||
{
|
||||
if (idx < TRACE_BUFFER_LEN) return &traceBuffer[idx];
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
void dumpTraceBuffer()
|
||||
{
|
||||
TRACE("Dump of Trace Buffer (" VERSION " " DATE " " TIME "):");
|
||||
TRACE("# Time Event Data");
|
||||
for(int n = 0; n < TRACE_BUFFER_LEN; ++n) {
|
||||
struct gtm tp;
|
||||
filltm(&traceBuffer[n].time, &tp);
|
||||
TRACE_INFO_WP("%02d ", n);
|
||||
TRACE_INFO_WP("%4d-%02d-%02d,%02d:%02d:%02d.%02d0", tp.tm_year+1900, tp.tm_mon+1, tp.tm_mday, tp.tm_hour, tp.tm_min, tp.tm_sec, traceBuffer[n].time_ms);
|
||||
TRACE(" %03d 0x%08x", traceBuffer[n].event, traceBuffer[n].data);
|
||||
if (traceBuffer[n].time == 0 && traceBuffer[n].time_ms == 0) break;
|
||||
#if !defined(SIMU)
|
||||
if ((n % 5) == 0) {
|
||||
while (!serial2TxFifo.empty()) {
|
||||
CoTickDelay(1);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
TRACE("End of Trace Buffer dump");
|
||||
}
|
||||
#endif
|
||||
|
|
|
@ -1,373 +1,373 @@
|
|||
/*
|
||||
* Copyright (C) OpenTX
|
||||
*
|
||||
* Based on code named
|
||||
* th9x - http://code.google.com/p/th9x
|
||||
* er9x - http://code.google.com/p/er9x
|
||||
* gruvin9x - http://code.google.com/p/gruvin9x
|
||||
*
|
||||
* License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
|
||||
*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License version 2 as
|
||||
* published by the Free Software Foundation.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*/
|
||||
|
||||
#include "opentx.h"
|
||||
#include "ff.h"
|
||||
|
||||
FIL g_oLogFile = {0};
|
||||
const pm_char * g_logError = NULL;
|
||||
uint8_t logDelay;
|
||||
|
||||
#if defined(PCBTARANIS) || defined(PCBFLAMENCO) || defined(PCBHORUS)
|
||||
#define get2PosState(sw) (switchState(SW_ ## sw ## 0) ? -1 : 1)
|
||||
#else
|
||||
#define get2PosState(sw) (switchState(SW_ ## sw) ? -1 : 1)
|
||||
#endif
|
||||
|
||||
#define get3PosState(sw) (switchState(SW_ ## sw ## 0) ? -1 : (switchState(SW_ ## sw ## 2) ? 1 : 0))
|
||||
|
||||
const pm_char * openLogs()
|
||||
{
|
||||
// Determine and set log file filename
|
||||
FRESULT result;
|
||||
char filename[34]; // /LOGS/modelnamexxx-2013-01-01.log
|
||||
|
||||
if (!sdMounted())
|
||||
return STR_NO_SDCARD;
|
||||
|
||||
if (sdGetFreeSectors() == 0)
|
||||
return STR_SDCARD_FULL;
|
||||
|
||||
// check and create folder here
|
||||
strcpy_P(filename, STR_LOGS_PATH);
|
||||
const char * error = sdCheckAndCreateDirectory(filename);
|
||||
if (error) {
|
||||
return error;
|
||||
}
|
||||
|
||||
filename[sizeof(LOGS_PATH)-1] = '/';
|
||||
memcpy(&filename[sizeof(LOGS_PATH)], g_model.header.name, sizeof(g_model.header.name));
|
||||
filename[sizeof(LOGS_PATH)+sizeof(g_model.header.name)] = '\0';
|
||||
|
||||
uint8_t i = sizeof(LOGS_PATH)+sizeof(g_model.header.name)-1;
|
||||
uint8_t len = 0;
|
||||
while (i>sizeof(LOGS_PATH)-1) {
|
||||
if (!len && filename[i])
|
||||
len = i+1;
|
||||
if (len) {
|
||||
if (filename[i])
|
||||
filename[i] = idx2char(filename[i]);
|
||||
else
|
||||
filename[i] = '_';
|
||||
}
|
||||
i--;
|
||||
}
|
||||
|
||||
if (len == 0) {
|
||||
#if defined(EEPROM)
|
||||
uint8_t num = g_eeGeneral.currModel + 1;
|
||||
#else
|
||||
// TODO
|
||||
uint8_t num = 1;
|
||||
#endif
|
||||
strcpy_P(&filename[sizeof(LOGS_PATH)], STR_MODEL);
|
||||
filename[sizeof(LOGS_PATH) + PSIZE(TR_MODEL)] = (char)((num / 10) + '0');
|
||||
filename[sizeof(LOGS_PATH) + PSIZE(TR_MODEL) + 1] = (char)((num % 10) + '0');
|
||||
len = sizeof(LOGS_PATH) + PSIZE(TR_MODEL) + 2;
|
||||
}
|
||||
|
||||
char * tmp = &filename[len];
|
||||
|
||||
#if defined(RTCLOCK)
|
||||
tmp = strAppendDate(&filename[len]);
|
||||
#endif
|
||||
|
||||
strcpy_P(tmp, STR_LOGS_EXT);
|
||||
|
||||
result = f_open(&g_oLogFile, filename, FA_OPEN_ALWAYS | FA_WRITE);
|
||||
if (result != FR_OK) {
|
||||
return SDCARD_ERROR(result);
|
||||
}
|
||||
|
||||
if (f_size(&g_oLogFile) == 0) {
|
||||
writeHeader();
|
||||
}
|
||||
else {
|
||||
result = f_lseek(&g_oLogFile, f_size(&g_oLogFile)); // append
|
||||
if (result != FR_OK) {
|
||||
return SDCARD_ERROR(result);
|
||||
}
|
||||
}
|
||||
|
||||
return NULL;
|
||||
}
|
||||
|
||||
tmr10ms_t lastLogTime = 0;
|
||||
|
||||
void closeLogs()
|
||||
{
|
||||
if (f_close(&g_oLogFile) != FR_OK) {
|
||||
// close failed, forget file
|
||||
g_oLogFile.fs = 0;
|
||||
}
|
||||
lastLogTime = 0;
|
||||
}
|
||||
|
||||
#if !defined(CPUARM)
|
||||
getvalue_t getConvertedTelemetryValue(getvalue_t val, uint8_t unit)
|
||||
{
|
||||
convertUnit(val, unit);
|
||||
return val;
|
||||
}
|
||||
#endif
|
||||
|
||||
void writeHeader()
|
||||
{
|
||||
#if defined(RTCLOCK)
|
||||
f_puts("Date,Time,", &g_oLogFile);
|
||||
#else
|
||||
f_puts("Time,", &g_oLogFile);
|
||||
#endif
|
||||
|
||||
#if defined(FRSKY)
|
||||
#if !defined(CPUARM)
|
||||
f_puts("Buffer,RX,TX,A1,A2,", &g_oLogFile);
|
||||
#if defined(FRSKY_HUB)
|
||||
if (IS_USR_PROTO_FRSKY_HUB()) {
|
||||
f_puts("GPS Date,GPS Time,Long,Lat,Course,GPS Speed(kts),GPS Alt,Baro Alt(", &g_oLogFile);
|
||||
f_puts(TELEMETRY_BARO_ALT_UNIT, &g_oLogFile);
|
||||
f_puts("),Vertical Speed,Air Speed(kts),Temp1,Temp2,RPM,Fuel," TELEMETRY_CELLS_LABEL "Current,Consumption,Vfas,AccelX,AccelY,AccelZ,", &g_oLogFile);
|
||||
}
|
||||
#endif
|
||||
#if defined(WS_HOW_HIGH)
|
||||
if (IS_USR_PROTO_WS_HOW_HIGH()) {
|
||||
f_puts("WSHH Alt,", &g_oLogFile);
|
||||
}
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#if defined(CPUARM)
|
||||
char label[TELEM_LABEL_LEN+7];
|
||||
for (int i=0; i<MAX_SENSORS; i++) {
|
||||
TelemetrySensor & sensor = g_model.telemetrySensors[i];
|
||||
if (sensor.logs) {
|
||||
memset(label, 0, sizeof(label));
|
||||
zchar2str(label, sensor.label, TELEM_LABEL_LEN);
|
||||
if (sensor.unit != UNIT_RAW && sensor.unit != UNIT_GPS && sensor.unit != UNIT_DATETIME) {
|
||||
strcat(label, "(");
|
||||
strncat(label, STR_VTELEMUNIT+1+3*sensor.unit, 3);
|
||||
strcat(label, ")");
|
||||
}
|
||||
strcat(label, ",");
|
||||
f_puts(label, &g_oLogFile);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#if defined(PCBTARANIS) || defined(PCBHORUS)
|
||||
for (uint8_t i=1; i<NUM_STICKS+NUM_POTS+1; i++) {
|
||||
const char * p = STR_VSRCRAW + i * STR_VSRCRAW[0] + 2;
|
||||
for (uint8_t j=0; j<STR_VSRCRAW[0]-1; ++j) {
|
||||
if (!*p) break;
|
||||
f_putc(*p, &g_oLogFile);
|
||||
++p;
|
||||
}
|
||||
f_putc(',', &g_oLogFile);
|
||||
}
|
||||
#define STR_SWITCHES_LOG_HEADER "SA,SB,SC,SD,SE,SF,SG,SH"
|
||||
f_puts(STR_SWITCHES_LOG_HEADER ",LS" "\n", &g_oLogFile);
|
||||
#else
|
||||
f_puts("Rud,Ele,Thr,Ail,P1,P2,P3,THR,RUD,ELE,3POS,AIL,GEA,TRN\n", &g_oLogFile);
|
||||
#endif
|
||||
}
|
||||
|
||||
uint32_t getLogicalSwitchesStates(uint8_t first)
|
||||
{
|
||||
uint32_t result = 0;
|
||||
for (uint8_t i=0; i<32; i++) {
|
||||
result |= (getSwitch(SWSRC_FIRST_LOGICAL_SWITCH+first+i) << i);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
void writeLogs()
|
||||
{
|
||||
static const pm_char * error_displayed = NULL;
|
||||
|
||||
if (isFunctionActive(FUNCTION_LOGS) && logDelay > 0) {
|
||||
tmr10ms_t tmr10ms = get_tmr10ms();
|
||||
if (lastLogTime == 0 || (tmr10ms_t)(tmr10ms - lastLogTime) >= (tmr10ms_t)logDelay*10) {
|
||||
lastLogTime = tmr10ms;
|
||||
|
||||
if (!g_oLogFile.fs) {
|
||||
const pm_char * result = openLogs();
|
||||
if (result != NULL) {
|
||||
if (result != error_displayed) {
|
||||
error_displayed = result;
|
||||
POPUP_WARNING(result);
|
||||
}
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
#if defined(RTCLOCK)
|
||||
{
|
||||
static struct gtm utm;
|
||||
static gtime_t lastRtcTime = 0;
|
||||
if (g_rtcTime != lastRtcTime) {
|
||||
lastRtcTime = g_rtcTime;
|
||||
gettime(&utm);
|
||||
}
|
||||
f_printf(&g_oLogFile, "%4d-%02d-%02d,%02d:%02d:%02d.%02d0,", utm.tm_year+1900, utm.tm_mon+1, utm.tm_mday, utm.tm_hour, utm.tm_min, utm.tm_sec, g_ms100);
|
||||
}
|
||||
#else
|
||||
f_printf(&g_oLogFile, "%d,", tmr10ms);
|
||||
#endif
|
||||
|
||||
#if defined(FRSKY)
|
||||
#if !defined(CPUARM)
|
||||
f_printf(&g_oLogFile, "%d,%d,%d,", frskyStreaming, RAW_FRSKY_MINMAX(frskyData.rssi[0]), RAW_FRSKY_MINMAX(frskyData.rssi[1]));
|
||||
for (uint8_t i=0; i<MAX_FRSKY_A_CHANNELS; i++) {
|
||||
int16_t converted_value = applyChannelRatio(i, RAW_FRSKY_MINMAX(frskyData.analog[i]));
|
||||
f_printf(&g_oLogFile, "%d.%02d,", converted_value/100, converted_value%100);
|
||||
}
|
||||
|
||||
#if defined(FRSKY_HUB)
|
||||
TELEMETRY_BARO_ALT_PREPARE();
|
||||
|
||||
if (IS_USR_PROTO_FRSKY_HUB()) {
|
||||
f_printf(&g_oLogFile, "%4d-%02d-%02d,%02d:%02d:%02d,%03d.%04d%c,%03d.%04d%c,%03d.%02d," TELEMETRY_GPS_SPEED_FORMAT TELEMETRY_GPS_ALT_FORMAT TELEMETRY_BARO_ALT_FORMAT TELEMETRY_VSPEED_FORMAT TELEMETRY_ASPEED_FORMAT "%d,%d,%d,%d," TELEMETRY_CELLS_FORMAT TELEMETRY_CURRENT_FORMAT "%d," TELEMETRY_VFAS_FORMAT "%d,%d,%d,",
|
||||
frskyData.hub.year+2000,
|
||||
frskyData.hub.month,
|
||||
frskyData.hub.day,
|
||||
frskyData.hub.hour,
|
||||
frskyData.hub.min,
|
||||
frskyData.hub.sec,
|
||||
frskyData.hub.gpsLongitude_bp,
|
||||
frskyData.hub.gpsLongitude_ap,
|
||||
frskyData.hub.gpsLongitudeEW ? frskyData.hub.gpsLongitudeEW : '-',
|
||||
frskyData.hub.gpsLatitude_bp,
|
||||
frskyData.hub.gpsLatitude_ap,
|
||||
frskyData.hub.gpsLatitudeNS ? frskyData.hub.gpsLatitudeNS : '-',
|
||||
frskyData.hub.gpsCourse_bp,
|
||||
frskyData.hub.gpsCourse_ap,
|
||||
TELEMETRY_GPS_SPEED_ARGS
|
||||
TELEMETRY_GPS_ALT_ARGS
|
||||
TELEMETRY_BARO_ALT_ARGS
|
||||
TELEMETRY_VSPEED_ARGS
|
||||
TELEMETRY_ASPEED_ARGS
|
||||
frskyData.hub.temperature1,
|
||||
frskyData.hub.temperature2,
|
||||
frskyData.hub.rpm,
|
||||
frskyData.hub.fuelLevel,
|
||||
TELEMETRY_CELLS_ARGS
|
||||
TELEMETRY_CURRENT_ARGS
|
||||
frskyData.hub.currentConsumption,
|
||||
TELEMETRY_VFAS_ARGS
|
||||
frskyData.hub.accelX,
|
||||
frskyData.hub.accelY,
|
||||
frskyData.hub.accelZ);
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined(WS_HOW_HIGH)
|
||||
if (IS_USR_PROTO_WS_HOW_HIGH()) {
|
||||
f_printf(&g_oLogFile, "%d,", TELEMETRY_RELATIVE_BARO_ALT_BP);
|
||||
}
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#if defined(CPUARM)
|
||||
for (int i=0; i<MAX_SENSORS; i++) {
|
||||
TelemetrySensor & sensor = g_model.telemetrySensors[i];
|
||||
TelemetryItem & telemetryItem = telemetryItems[i];
|
||||
if (sensor.logs) {
|
||||
if (sensor.unit == UNIT_GPS) {
|
||||
if (telemetryItem.gps.longitudeEW && telemetryItem.gps.latitudeNS)
|
||||
f_printf(&g_oLogFile, "%03d.%04d%c %03d.%04d%c,", telemetryItem.gps.longitude_bp, telemetryItem.gps.longitude_ap, telemetryItem.gps.longitudeEW, telemetryItem.gps.latitude_bp, telemetryItem.gps.latitude_ap, telemetryItem.gps.latitudeNS);
|
||||
else
|
||||
f_printf(&g_oLogFile, ",");
|
||||
}
|
||||
else if (sensor.unit == UNIT_DATETIME) {
|
||||
if (telemetryItem.datetime.datestate)
|
||||
f_printf(&g_oLogFile, "%4d-%02d-%02d %02d:%02d:%02d,", telemetryItem.datetime.year, telemetryItem.datetime.month, telemetryItem.datetime.day, telemetryItem.datetime.hour, telemetryItem.datetime.min, telemetryItem.datetime.sec);
|
||||
else
|
||||
f_printf(&g_oLogFile, ",");
|
||||
}
|
||||
else if (sensor.prec == 2) {
|
||||
div_t qr = div(telemetryItem.value, 100);
|
||||
if (telemetryItem.value < 0) f_printf(&g_oLogFile, "-");
|
||||
f_printf(&g_oLogFile, "%d.%02d,", abs(qr.quot), abs(qr.rem));
|
||||
}
|
||||
else if (sensor.prec == 1) {
|
||||
div_t qr = div(telemetryItem.value, 10);
|
||||
if (telemetryItem.value < 0) f_printf(&g_oLogFile, "-");
|
||||
f_printf(&g_oLogFile, "%d.%d,", abs(qr.quot), abs(qr.rem));
|
||||
}
|
||||
else {
|
||||
f_printf(&g_oLogFile, "%d,", telemetryItem.value);
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
#endif
|
||||
|
||||
for (uint8_t i=0; i<NUM_STICKS+NUM_POTS; i++) {
|
||||
f_printf(&g_oLogFile, "%d,", calibratedStick[i]);
|
||||
}
|
||||
|
||||
#if defined(PCBFLAMENCO)
|
||||
int result = f_printf(&g_oLogFile, "%d,%d,%d,%d\n",
|
||||
get3PosState(SA),
|
||||
get3PosState(SB),
|
||||
// get3PosState(SC),
|
||||
get2PosState(SE),
|
||||
get3PosState(SF));
|
||||
#elif defined(PCBTARANIS) || defined(PCBHORUS)
|
||||
int result = f_printf(&g_oLogFile, "%d,%d,%d,%d,%d,%d,%d,%d,%lu%lu\n",
|
||||
get3PosState(SA),
|
||||
get3PosState(SB),
|
||||
get3PosState(SC),
|
||||
get3PosState(SD),
|
||||
get3PosState(SE),
|
||||
get2PosState(SF),
|
||||
get3PosState(SG),
|
||||
get2PosState(SH),
|
||||
getLogicalSwitchesStates(32),
|
||||
getLogicalSwitchesStates(0));
|
||||
#else
|
||||
int result = f_printf(&g_oLogFile, "%d,%d,%d,%d,%d,%d,%d\n",
|
||||
get2PosState(THR),
|
||||
get2PosState(RUD),
|
||||
get2PosState(ELE),
|
||||
get3PosState(ID),
|
||||
get2PosState(AIL),
|
||||
get2PosState(GEA),
|
||||
get2PosState(TRN));
|
||||
#endif
|
||||
|
||||
if (result<0 && !error_displayed) {
|
||||
error_displayed = STR_SDCARD_ERROR;
|
||||
POPUP_WARNING(STR_SDCARD_ERROR);
|
||||
closeLogs();
|
||||
}
|
||||
}
|
||||
}
|
||||
else {
|
||||
error_displayed = NULL;
|
||||
if (g_oLogFile.fs) {
|
||||
closeLogs();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*
|
||||
* Copyright (C) OpenTX
|
||||
*
|
||||
* Based on code named
|
||||
* th9x - http://code.google.com/p/th9x
|
||||
* er9x - http://code.google.com/p/er9x
|
||||
* gruvin9x - http://code.google.com/p/gruvin9x
|
||||
*
|
||||
* License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
|
||||
*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License version 2 as
|
||||
* published by the Free Software Foundation.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*/
|
||||
|
||||
#include "opentx.h"
|
||||
#include "ff.h"
|
||||
|
||||
FIL g_oLogFile = {0};
|
||||
const pm_char * g_logError = NULL;
|
||||
uint8_t logDelay;
|
||||
|
||||
#if defined(PCBTARANIS) || defined(PCBFLAMENCO) || defined(PCBHORUS)
|
||||
#define get2PosState(sw) (switchState(SW_ ## sw ## 0) ? -1 : 1)
|
||||
#else
|
||||
#define get2PosState(sw) (switchState(SW_ ## sw) ? -1 : 1)
|
||||
#endif
|
||||
|
||||
#define get3PosState(sw) (switchState(SW_ ## sw ## 0) ? -1 : (switchState(SW_ ## sw ## 2) ? 1 : 0))
|
||||
|
||||
const pm_char * openLogs()
|
||||
{
|
||||
// Determine and set log file filename
|
||||
FRESULT result;
|
||||
char filename[34]; // /LOGS/modelnamexxx-2013-01-01.log
|
||||
|
||||
if (!sdMounted())
|
||||
return STR_NO_SDCARD;
|
||||
|
||||
if (sdGetFreeSectors() == 0)
|
||||
return STR_SDCARD_FULL;
|
||||
|
||||
// check and create folder here
|
||||
strcpy_P(filename, STR_LOGS_PATH);
|
||||
const char * error = sdCheckAndCreateDirectory(filename);
|
||||
if (error) {
|
||||
return error;
|
||||
}
|
||||
|
||||
filename[sizeof(LOGS_PATH)-1] = '/';
|
||||
memcpy(&filename[sizeof(LOGS_PATH)], g_model.header.name, sizeof(g_model.header.name));
|
||||
filename[sizeof(LOGS_PATH)+sizeof(g_model.header.name)] = '\0';
|
||||
|
||||
uint8_t i = sizeof(LOGS_PATH)+sizeof(g_model.header.name)-1;
|
||||
uint8_t len = 0;
|
||||
while (i>sizeof(LOGS_PATH)-1) {
|
||||
if (!len && filename[i])
|
||||
len = i+1;
|
||||
if (len) {
|
||||
if (filename[i])
|
||||
filename[i] = idx2char(filename[i]);
|
||||
else
|
||||
filename[i] = '_';
|
||||
}
|
||||
i--;
|
||||
}
|
||||
|
||||
if (len == 0) {
|
||||
#if defined(EEPROM)
|
||||
uint8_t num = g_eeGeneral.currModel + 1;
|
||||
#else
|
||||
// TODO
|
||||
uint8_t num = 1;
|
||||
#endif
|
||||
strcpy_P(&filename[sizeof(LOGS_PATH)], STR_MODEL);
|
||||
filename[sizeof(LOGS_PATH) + PSIZE(TR_MODEL)] = (char)((num / 10) + '0');
|
||||
filename[sizeof(LOGS_PATH) + PSIZE(TR_MODEL) + 1] = (char)((num % 10) + '0');
|
||||
len = sizeof(LOGS_PATH) + PSIZE(TR_MODEL) + 2;
|
||||
}
|
||||
|
||||
char * tmp = &filename[len];
|
||||
|
||||
#if defined(RTCLOCK)
|
||||
tmp = strAppendDate(&filename[len]);
|
||||
#endif
|
||||
|
||||
strcpy_P(tmp, STR_LOGS_EXT);
|
||||
|
||||
result = f_open(&g_oLogFile, filename, FA_OPEN_ALWAYS | FA_WRITE);
|
||||
if (result != FR_OK) {
|
||||
return SDCARD_ERROR(result);
|
||||
}
|
||||
|
||||
if (f_size(&g_oLogFile) == 0) {
|
||||
writeHeader();
|
||||
}
|
||||
else {
|
||||
result = f_lseek(&g_oLogFile, f_size(&g_oLogFile)); // append
|
||||
if (result != FR_OK) {
|
||||
return SDCARD_ERROR(result);
|
||||
}
|
||||
}
|
||||
|
||||
return NULL;
|
||||
}
|
||||
|
||||
tmr10ms_t lastLogTime = 0;
|
||||
|
||||
void closeLogs()
|
||||
{
|
||||
if (f_close(&g_oLogFile) != FR_OK) {
|
||||
// close failed, forget file
|
||||
g_oLogFile.fs = 0;
|
||||
}
|
||||
lastLogTime = 0;
|
||||
}
|
||||
|
||||
#if !defined(CPUARM)
|
||||
getvalue_t getConvertedTelemetryValue(getvalue_t val, uint8_t unit)
|
||||
{
|
||||
convertUnit(val, unit);
|
||||
return val;
|
||||
}
|
||||
#endif
|
||||
|
||||
void writeHeader()
|
||||
{
|
||||
#if defined(RTCLOCK)
|
||||
f_puts("Date,Time,", &g_oLogFile);
|
||||
#else
|
||||
f_puts("Time,", &g_oLogFile);
|
||||
#endif
|
||||
|
||||
#if defined(FRSKY)
|
||||
#if !defined(CPUARM)
|
||||
f_puts("Buffer,RX,TX,A1,A2,", &g_oLogFile);
|
||||
#if defined(FRSKY_HUB)
|
||||
if (IS_USR_PROTO_FRSKY_HUB()) {
|
||||
f_puts("GPS Date,GPS Time,Long,Lat,Course,GPS Speed(kts),GPS Alt,Baro Alt(", &g_oLogFile);
|
||||
f_puts(TELEMETRY_BARO_ALT_UNIT, &g_oLogFile);
|
||||
f_puts("),Vertical Speed,Air Speed(kts),Temp1,Temp2,RPM,Fuel," TELEMETRY_CELLS_LABEL "Current,Consumption,Vfas,AccelX,AccelY,AccelZ,", &g_oLogFile);
|
||||
}
|
||||
#endif
|
||||
#if defined(WS_HOW_HIGH)
|
||||
if (IS_USR_PROTO_WS_HOW_HIGH()) {
|
||||
f_puts("WSHH Alt,", &g_oLogFile);
|
||||
}
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#if defined(CPUARM)
|
||||
char label[TELEM_LABEL_LEN+7];
|
||||
for (int i=0; i<MAX_SENSORS; i++) {
|
||||
TelemetrySensor & sensor = g_model.telemetrySensors[i];
|
||||
if (sensor.logs) {
|
||||
memset(label, 0, sizeof(label));
|
||||
zchar2str(label, sensor.label, TELEM_LABEL_LEN);
|
||||
if (sensor.unit != UNIT_RAW && sensor.unit != UNIT_GPS && sensor.unit != UNIT_DATETIME) {
|
||||
strcat(label, "(");
|
||||
strncat(label, STR_VTELEMUNIT+1+3*sensor.unit, 3);
|
||||
strcat(label, ")");
|
||||
}
|
||||
strcat(label, ",");
|
||||
f_puts(label, &g_oLogFile);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#if defined(PCBTARANIS) || defined(PCBHORUS)
|
||||
for (uint8_t i=1; i<NUM_STICKS+NUM_POTS+1; i++) {
|
||||
const char * p = STR_VSRCRAW + i * STR_VSRCRAW[0] + 2;
|
||||
for (uint8_t j=0; j<STR_VSRCRAW[0]-1; ++j) {
|
||||
if (!*p) break;
|
||||
f_putc(*p, &g_oLogFile);
|
||||
++p;
|
||||
}
|
||||
f_putc(',', &g_oLogFile);
|
||||
}
|
||||
#define STR_SWITCHES_LOG_HEADER "SA,SB,SC,SD,SE,SF,SG,SH"
|
||||
f_puts(STR_SWITCHES_LOG_HEADER ",LS" "\n", &g_oLogFile);
|
||||
#else
|
||||
f_puts("Rud,Ele,Thr,Ail,P1,P2,P3,THR,RUD,ELE,3POS,AIL,GEA,TRN\n", &g_oLogFile);
|
||||
#endif
|
||||
}
|
||||
|
||||
uint32_t getLogicalSwitchesStates(uint8_t first)
|
||||
{
|
||||
uint32_t result = 0;
|
||||
for (uint8_t i=0; i<32; i++) {
|
||||
result |= (getSwitch(SWSRC_FIRST_LOGICAL_SWITCH+first+i) << i);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
void writeLogs()
|
||||
{
|
||||
static const pm_char * error_displayed = NULL;
|
||||
|
||||
if (isFunctionActive(FUNCTION_LOGS) && logDelay > 0) {
|
||||
tmr10ms_t tmr10ms = get_tmr10ms();
|
||||
if (lastLogTime == 0 || (tmr10ms_t)(tmr10ms - lastLogTime) >= (tmr10ms_t)logDelay*10) {
|
||||
lastLogTime = tmr10ms;
|
||||
|
||||
if (!g_oLogFile.fs) {
|
||||
const pm_char * result = openLogs();
|
||||
if (result != NULL) {
|
||||
if (result != error_displayed) {
|
||||
error_displayed = result;
|
||||
POPUP_WARNING(result);
|
||||
}
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
#if defined(RTCLOCK)
|
||||
{
|
||||
static struct gtm utm;
|
||||
static gtime_t lastRtcTime = 0;
|
||||
if (g_rtcTime != lastRtcTime) {
|
||||
lastRtcTime = g_rtcTime;
|
||||
gettime(&utm);
|
||||
}
|
||||
f_printf(&g_oLogFile, "%4d-%02d-%02d,%02d:%02d:%02d.%02d0,", utm.tm_year+1900, utm.tm_mon+1, utm.tm_mday, utm.tm_hour, utm.tm_min, utm.tm_sec, g_ms100);
|
||||
}
|
||||
#else
|
||||
f_printf(&g_oLogFile, "%d,", tmr10ms);
|
||||
#endif
|
||||
|
||||
#if defined(FRSKY)
|
||||
#if !defined(CPUARM)
|
||||
f_printf(&g_oLogFile, "%d,%d,%d,", frskyStreaming, RAW_FRSKY_MINMAX(frskyData.rssi[0]), RAW_FRSKY_MINMAX(frskyData.rssi[1]));
|
||||
for (uint8_t i=0; i<MAX_FRSKY_A_CHANNELS; i++) {
|
||||
int16_t converted_value = applyChannelRatio(i, RAW_FRSKY_MINMAX(frskyData.analog[i]));
|
||||
f_printf(&g_oLogFile, "%d.%02d,", converted_value/100, converted_value%100);
|
||||
}
|
||||
|
||||
#if defined(FRSKY_HUB)
|
||||
TELEMETRY_BARO_ALT_PREPARE();
|
||||
|
||||
if (IS_USR_PROTO_FRSKY_HUB()) {
|
||||
f_printf(&g_oLogFile, "%4d-%02d-%02d,%02d:%02d:%02d,%03d.%04d%c,%03d.%04d%c,%03d.%02d," TELEMETRY_GPS_SPEED_FORMAT TELEMETRY_GPS_ALT_FORMAT TELEMETRY_BARO_ALT_FORMAT TELEMETRY_VSPEED_FORMAT TELEMETRY_ASPEED_FORMAT "%d,%d,%d,%d," TELEMETRY_CELLS_FORMAT TELEMETRY_CURRENT_FORMAT "%d," TELEMETRY_VFAS_FORMAT "%d,%d,%d,",
|
||||
frskyData.hub.year+2000,
|
||||
frskyData.hub.month,
|
||||
frskyData.hub.day,
|
||||
frskyData.hub.hour,
|
||||
frskyData.hub.min,
|
||||
frskyData.hub.sec,
|
||||
frskyData.hub.gpsLongitude_bp,
|
||||
frskyData.hub.gpsLongitude_ap,
|
||||
frskyData.hub.gpsLongitudeEW ? frskyData.hub.gpsLongitudeEW : '-',
|
||||
frskyData.hub.gpsLatitude_bp,
|
||||
frskyData.hub.gpsLatitude_ap,
|
||||
frskyData.hub.gpsLatitudeNS ? frskyData.hub.gpsLatitudeNS : '-',
|
||||
frskyData.hub.gpsCourse_bp,
|
||||
frskyData.hub.gpsCourse_ap,
|
||||
TELEMETRY_GPS_SPEED_ARGS
|
||||
TELEMETRY_GPS_ALT_ARGS
|
||||
TELEMETRY_BARO_ALT_ARGS
|
||||
TELEMETRY_VSPEED_ARGS
|
||||
TELEMETRY_ASPEED_ARGS
|
||||
frskyData.hub.temperature1,
|
||||
frskyData.hub.temperature2,
|
||||
frskyData.hub.rpm,
|
||||
frskyData.hub.fuelLevel,
|
||||
TELEMETRY_CELLS_ARGS
|
||||
TELEMETRY_CURRENT_ARGS
|
||||
frskyData.hub.currentConsumption,
|
||||
TELEMETRY_VFAS_ARGS
|
||||
frskyData.hub.accelX,
|
||||
frskyData.hub.accelY,
|
||||
frskyData.hub.accelZ);
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined(WS_HOW_HIGH)
|
||||
if (IS_USR_PROTO_WS_HOW_HIGH()) {
|
||||
f_printf(&g_oLogFile, "%d,", TELEMETRY_RELATIVE_BARO_ALT_BP);
|
||||
}
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#if defined(CPUARM)
|
||||
for (int i=0; i<MAX_SENSORS; i++) {
|
||||
TelemetrySensor & sensor = g_model.telemetrySensors[i];
|
||||
TelemetryItem & telemetryItem = telemetryItems[i];
|
||||
if (sensor.logs) {
|
||||
if (sensor.unit == UNIT_GPS) {
|
||||
if (telemetryItem.gps.longitudeEW && telemetryItem.gps.latitudeNS)
|
||||
f_printf(&g_oLogFile, "%03d.%04d%c %03d.%04d%c,", telemetryItem.gps.longitude_bp, telemetryItem.gps.longitude_ap, telemetryItem.gps.longitudeEW, telemetryItem.gps.latitude_bp, telemetryItem.gps.latitude_ap, telemetryItem.gps.latitudeNS);
|
||||
else
|
||||
f_printf(&g_oLogFile, ",");
|
||||
}
|
||||
else if (sensor.unit == UNIT_DATETIME) {
|
||||
if (telemetryItem.datetime.datestate)
|
||||
f_printf(&g_oLogFile, "%4d-%02d-%02d %02d:%02d:%02d,", telemetryItem.datetime.year, telemetryItem.datetime.month, telemetryItem.datetime.day, telemetryItem.datetime.hour, telemetryItem.datetime.min, telemetryItem.datetime.sec);
|
||||
else
|
||||
f_printf(&g_oLogFile, ",");
|
||||
}
|
||||
else if (sensor.prec == 2) {
|
||||
div_t qr = div(telemetryItem.value, 100);
|
||||
if (telemetryItem.value < 0) f_printf(&g_oLogFile, "-");
|
||||
f_printf(&g_oLogFile, "%d.%02d,", abs(qr.quot), abs(qr.rem));
|
||||
}
|
||||
else if (sensor.prec == 1) {
|
||||
div_t qr = div(telemetryItem.value, 10);
|
||||
if (telemetryItem.value < 0) f_printf(&g_oLogFile, "-");
|
||||
f_printf(&g_oLogFile, "%d.%d,", abs(qr.quot), abs(qr.rem));
|
||||
}
|
||||
else {
|
||||
f_printf(&g_oLogFile, "%d,", telemetryItem.value);
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
#endif
|
||||
|
||||
for (uint8_t i=0; i<NUM_STICKS+NUM_POTS; i++) {
|
||||
f_printf(&g_oLogFile, "%d,", calibratedStick[i]);
|
||||
}
|
||||
|
||||
#if defined(PCBFLAMENCO)
|
||||
int result = f_printf(&g_oLogFile, "%d,%d,%d,%d\n",
|
||||
get3PosState(SA),
|
||||
get3PosState(SB),
|
||||
// get3PosState(SC),
|
||||
get2PosState(SE),
|
||||
get3PosState(SF));
|
||||
#elif defined(PCBTARANIS) || defined(PCBHORUS)
|
||||
int result = f_printf(&g_oLogFile, "%d,%d,%d,%d,%d,%d,%d,%d,%lu%lu\n",
|
||||
get3PosState(SA),
|
||||
get3PosState(SB),
|
||||
get3PosState(SC),
|
||||
get3PosState(SD),
|
||||
get3PosState(SE),
|
||||
get2PosState(SF),
|
||||
get3PosState(SG),
|
||||
get2PosState(SH),
|
||||
getLogicalSwitchesStates(32),
|
||||
getLogicalSwitchesStates(0));
|
||||
#else
|
||||
int result = f_printf(&g_oLogFile, "%d,%d,%d,%d,%d,%d,%d\n",
|
||||
get2PosState(THR),
|
||||
get2PosState(RUD),
|
||||
get2PosState(ELE),
|
||||
get3PosState(ID),
|
||||
get2PosState(AIL),
|
||||
get2PosState(GEA),
|
||||
get2PosState(TRN));
|
||||
#endif
|
||||
|
||||
if (result<0 && !error_displayed) {
|
||||
error_displayed = STR_SDCARD_ERROR;
|
||||
POPUP_WARNING(STR_SDCARD_ERROR);
|
||||
closeLogs();
|
||||
}
|
||||
}
|
||||
}
|
||||
else {
|
||||
error_displayed = NULL;
|
||||
if (g_oLogFile.fs) {
|
||||
closeLogs();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
|
|
@ -1,186 +1,186 @@
|
|||
/*
|
||||
* Copyright (C) OpenTX
|
||||
*
|
||||
* Based on code named
|
||||
* th9x - http://code.google.com/p/th9x
|
||||
* er9x - http://code.google.com/p/er9x
|
||||
* gruvin9x - http://code.google.com/p/gruvin9x
|
||||
*
|
||||
* License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
|
||||
*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License version 2 as
|
||||
* published by the Free Software Foundation.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*/
|
||||
|
||||
#include "opentx.h"
|
||||
|
||||
#if !defined(CPUARM)
|
||||
// #define CORRECT_NEGATIVE_SHIFTS
|
||||
// open.20.fsguruh; shift right operations do the rounding different for negative values compared to positive values
|
||||
// so all negative divisions round always further down, which give absolute values bigger compared to a usual division
|
||||
// this is noticable on the display, because instead of -100.0 -99.9 is shown; While in praxis I doublt somebody will notice a
|
||||
// difference this is more a mental thing. Maybe people are distracted, because the easy calculations are obviously wrong
|
||||
// this define would correct this, but costs 34 bytes code for stock version
|
||||
|
||||
// currently we set this to active always, because it might cause a fault about 1% compared positive and negative values
|
||||
// is done now in makefile
|
||||
|
||||
int16_t calc100to256_16Bits(int16_t x) // return x*2.56
|
||||
{
|
||||
// y = 2*x + x/2 +x/16-x/512-x/2048
|
||||
// 512 and 2048 are out of scope from int8 input --> forget it
|
||||
#ifdef CORRECT_NEGATIVE_SHIFTS
|
||||
int16_t res=(int16_t)x<<1;
|
||||
//int8_t sign=(uint8_t) x>>7;
|
||||
int8_t sign=(x<0?1:0);
|
||||
|
||||
x-=sign;
|
||||
res+=(x>>1);
|
||||
res+=sign;
|
||||
res+=(x>>4);
|
||||
res+=sign;
|
||||
return res;
|
||||
#else
|
||||
return ((int16_t)x<<1)+(x>>1)+(x>>4);
|
||||
#endif
|
||||
}
|
||||
|
||||
int16_t calc100to256(int8_t x) // return x*2.56
|
||||
{
|
||||
return calc100to256_16Bits(x);
|
||||
}
|
||||
|
||||
int16_t calc100toRESX_16Bits(int16_t x) // return x*10.24
|
||||
{
|
||||
#ifdef CORRECT_NEGATIVE_SHIFTS
|
||||
int16_t res= ((int16_t)x*41)>>2;
|
||||
int8_t sign=(x<0?1:0);
|
||||
//int8_t sign=(uint8_t) x>>7;
|
||||
x-=sign;
|
||||
res-=(x>>6);
|
||||
res-=sign;
|
||||
return res;
|
||||
#else
|
||||
// return (int16_t)x*10 + x/4 - x/64;
|
||||
return ((x*41)>>2) - (x>>6);
|
||||
#endif
|
||||
}
|
||||
|
||||
int16_t calc100toRESX(int8_t x) // return x*10.24
|
||||
{
|
||||
return calc100toRESX_16Bits(x);
|
||||
}
|
||||
|
||||
// return x*1.024
|
||||
int16_t calc1000toRESX(int16_t x) // improve calc time by Pat MacKenzie
|
||||
{
|
||||
// return x + x/32 - x/128 + x/512;
|
||||
int16_t y = x>>5;
|
||||
x+=y;
|
||||
y=y>>2;
|
||||
x-=y;
|
||||
return x+(y>>2);
|
||||
}
|
||||
|
||||
int16_t calcRESXto1000(int16_t x) // return x/1.024
|
||||
{
|
||||
// *1000/1024 = x - x/32 + x/128
|
||||
return (x - (x>>5) + (x>>7));
|
||||
}
|
||||
|
||||
int8_t calcRESXto100(int16_t x)
|
||||
{
|
||||
return (x*25) >> 8;
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined(HELI) || defined(FRSKY_HUB) || defined(CPUARM)
|
||||
uint16_t isqrt32(uint32_t n)
|
||||
{
|
||||
uint16_t c = 0x8000;
|
||||
uint16_t g = 0x8000;
|
||||
|
||||
for (;;) {
|
||||
if ((uint32_t)g*g > n)
|
||||
g ^= c;
|
||||
c >>= 1;
|
||||
if(c == 0)
|
||||
return g;
|
||||
g |= c;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
/*
|
||||
Division by 10 and rounding or fixed point arithmetic values
|
||||
|
||||
Examples:
|
||||
value -> result
|
||||
105 -> 11
|
||||
104 -> 10
|
||||
-205 -> -21
|
||||
-204 -> -20
|
||||
*/
|
||||
|
||||
#if defined(PCBTARANIS)
|
||||
double gpsToDouble(bool neg, int16_t bp, int16_t ap)
|
||||
{
|
||||
double result = ap;
|
||||
result /= 10000;
|
||||
result += (bp % 100);
|
||||
result /= 60;
|
||||
result += (bp / 100);
|
||||
return neg?-result:result;
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined(FRSKY_HUB) && !defined(CPUARM)
|
||||
void extractLatitudeLongitude(uint32_t * latitude, uint32_t * longitude)
|
||||
{
|
||||
div_t qr = div(frskyData.hub.gpsLatitude_bp, 100);
|
||||
*latitude = ((uint32_t)(qr.quot) * 1000000) + (((uint32_t)(qr.rem) * 10000 + frskyData.hub.gpsLatitude_ap) * 5) / 3;
|
||||
|
||||
qr = div(frskyData.hub.gpsLongitude_bp, 100);
|
||||
*longitude = ((uint32_t)(qr.quot) * 1000000) + (((uint32_t)(qr.rem) * 10000 + frskyData.hub.gpsLongitude_ap) * 5) / 3;
|
||||
}
|
||||
|
||||
void getGpsPilotPosition()
|
||||
{
|
||||
extractLatitudeLongitude(&frskyData.hub.pilotLatitude, &frskyData.hub.pilotLongitude);
|
||||
uint32_t lat = frskyData.hub.pilotLatitude / 10000;
|
||||
uint32_t angle2 = (lat*lat) / 10000;
|
||||
uint32_t angle4 = angle2 * angle2;
|
||||
frskyData.hub.distFromEarthAxis = 139*(((uint32_t)10000000-((angle2*(uint32_t)123370)/81)+(angle4/25))/12500);
|
||||
// TRACE("frskyData.hub.distFromEarthAxis=%d", frskyData.hub.distFromEarthAxis);
|
||||
}
|
||||
|
||||
void getGpsDistance()
|
||||
{
|
||||
uint32_t lat, lng;
|
||||
|
||||
extractLatitudeLongitude(&lat, &lng);
|
||||
|
||||
// printf("lat=%d (%d), long=%d (%d)\n", lat, abs(lat - frskyData.hub.pilotLatitude), lng, abs(lng - frskyData.hub.pilotLongitude));
|
||||
|
||||
uint32_t angle = (lat > frskyData.hub.pilotLatitude) ? lat - frskyData.hub.pilotLatitude : frskyData.hub.pilotLatitude - lat;
|
||||
uint32_t dist = EARTH_RADIUS * angle / 1000000;
|
||||
uint32_t result = dist*dist;
|
||||
|
||||
angle = (lng > frskyData.hub.pilotLongitude) ? lng - frskyData.hub.pilotLongitude : frskyData.hub.pilotLongitude - lng;
|
||||
dist = frskyData.hub.distFromEarthAxis * angle / 1000000;
|
||||
result += dist*dist;
|
||||
|
||||
dist = abs(TELEMETRY_BARO_ALT_AVAILABLE() ? TELEMETRY_RELATIVE_BARO_ALT_BP : TELEMETRY_RELATIVE_GPS_ALT_BP);
|
||||
result += dist*dist;
|
||||
|
||||
frskyData.hub.gpsDistance = isqrt32(result);
|
||||
if (frskyData.hub.gpsDistance > frskyData.hub.maxGpsDistance)
|
||||
frskyData.hub.maxGpsDistance = frskyData.hub.gpsDistance;
|
||||
}
|
||||
#endif
|
||||
/*
|
||||
* Copyright (C) OpenTX
|
||||
*
|
||||
* Based on code named
|
||||
* th9x - http://code.google.com/p/th9x
|
||||
* er9x - http://code.google.com/p/er9x
|
||||
* gruvin9x - http://code.google.com/p/gruvin9x
|
||||
*
|
||||
* License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
|
||||
*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License version 2 as
|
||||
* published by the Free Software Foundation.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*/
|
||||
|
||||
#include "opentx.h"
|
||||
|
||||
#if !defined(CPUARM)
|
||||
// #define CORRECT_NEGATIVE_SHIFTS
|
||||
// open.20.fsguruh; shift right operations do the rounding different for negative values compared to positive values
|
||||
// so all negative divisions round always further down, which give absolute values bigger compared to a usual division
|
||||
// this is noticable on the display, because instead of -100.0 -99.9 is shown; While in praxis I doublt somebody will notice a
|
||||
// difference this is more a mental thing. Maybe people are distracted, because the easy calculations are obviously wrong
|
||||
// this define would correct this, but costs 34 bytes code for stock version
|
||||
|
||||
// currently we set this to active always, because it might cause a fault about 1% compared positive and negative values
|
||||
// is done now in makefile
|
||||
|
||||
int16_t calc100to256_16Bits(int16_t x) // return x*2.56
|
||||
{
|
||||
// y = 2*x + x/2 +x/16-x/512-x/2048
|
||||
// 512 and 2048 are out of scope from int8 input --> forget it
|
||||
#ifdef CORRECT_NEGATIVE_SHIFTS
|
||||
int16_t res=(int16_t)x<<1;
|
||||
//int8_t sign=(uint8_t) x>>7;
|
||||
int8_t sign=(x<0?1:0);
|
||||
|
||||
x-=sign;
|
||||
res+=(x>>1);
|
||||
res+=sign;
|
||||
res+=(x>>4);
|
||||
res+=sign;
|
||||
return res;
|
||||
#else
|
||||
return ((int16_t)x<<1)+(x>>1)+(x>>4);
|
||||
#endif
|
||||
}
|
||||
|
||||
int16_t calc100to256(int8_t x) // return x*2.56
|
||||
{
|
||||
return calc100to256_16Bits(x);
|
||||
}
|
||||
|
||||
int16_t calc100toRESX_16Bits(int16_t x) // return x*10.24
|
||||
{
|
||||
#ifdef CORRECT_NEGATIVE_SHIFTS
|
||||
int16_t res= ((int16_t)x*41)>>2;
|
||||
int8_t sign=(x<0?1:0);
|
||||
//int8_t sign=(uint8_t) x>>7;
|
||||
x-=sign;
|
||||
res-=(x>>6);
|
||||
res-=sign;
|
||||
return res;
|
||||
#else
|
||||
// return (int16_t)x*10 + x/4 - x/64;
|
||||
return ((x*41)>>2) - (x>>6);
|
||||
#endif
|
||||
}
|
||||
|
||||
int16_t calc100toRESX(int8_t x) // return x*10.24
|
||||
{
|
||||
return calc100toRESX_16Bits(x);
|
||||
}
|
||||
|
||||
// return x*1.024
|
||||
int16_t calc1000toRESX(int16_t x) // improve calc time by Pat MacKenzie
|
||||
{
|
||||
// return x + x/32 - x/128 + x/512;
|
||||
int16_t y = x>>5;
|
||||
x+=y;
|
||||
y=y>>2;
|
||||
x-=y;
|
||||
return x+(y>>2);
|
||||
}
|
||||
|
||||
int16_t calcRESXto1000(int16_t x) // return x/1.024
|
||||
{
|
||||
// *1000/1024 = x - x/32 + x/128
|
||||
return (x - (x>>5) + (x>>7));
|
||||
}
|
||||
|
||||
int8_t calcRESXto100(int16_t x)
|
||||
{
|
||||
return (x*25) >> 8;
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined(HELI) || defined(FRSKY_HUB) || defined(CPUARM)
|
||||
uint16_t isqrt32(uint32_t n)
|
||||
{
|
||||
uint16_t c = 0x8000;
|
||||
uint16_t g = 0x8000;
|
||||
|
||||
for (;;) {
|
||||
if ((uint32_t)g*g > n)
|
||||
g ^= c;
|
||||
c >>= 1;
|
||||
if(c == 0)
|
||||
return g;
|
||||
g |= c;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
/*
|
||||
Division by 10 and rounding or fixed point arithmetic values
|
||||
|
||||
Examples:
|
||||
value -> result
|
||||
105 -> 11
|
||||
104 -> 10
|
||||
-205 -> -21
|
||||
-204 -> -20
|
||||
*/
|
||||
|
||||
#if defined(PCBTARANIS)
|
||||
double gpsToDouble(bool neg, int16_t bp, int16_t ap)
|
||||
{
|
||||
double result = ap;
|
||||
result /= 10000;
|
||||
result += (bp % 100);
|
||||
result /= 60;
|
||||
result += (bp / 100);
|
||||
return neg?-result:result;
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined(FRSKY_HUB) && !defined(CPUARM)
|
||||
void extractLatitudeLongitude(uint32_t * latitude, uint32_t * longitude)
|
||||
{
|
||||
div_t qr = div(frskyData.hub.gpsLatitude_bp, 100);
|
||||
*latitude = ((uint32_t)(qr.quot) * 1000000) + (((uint32_t)(qr.rem) * 10000 + frskyData.hub.gpsLatitude_ap) * 5) / 3;
|
||||
|
||||
qr = div(frskyData.hub.gpsLongitude_bp, 100);
|
||||
*longitude = ((uint32_t)(qr.quot) * 1000000) + (((uint32_t)(qr.rem) * 10000 + frskyData.hub.gpsLongitude_ap) * 5) / 3;
|
||||
}
|
||||
|
||||
void getGpsPilotPosition()
|
||||
{
|
||||
extractLatitudeLongitude(&frskyData.hub.pilotLatitude, &frskyData.hub.pilotLongitude);
|
||||
uint32_t lat = frskyData.hub.pilotLatitude / 10000;
|
||||
uint32_t angle2 = (lat*lat) / 10000;
|
||||
uint32_t angle4 = angle2 * angle2;
|
||||
frskyData.hub.distFromEarthAxis = 139*(((uint32_t)10000000-((angle2*(uint32_t)123370)/81)+(angle4/25))/12500);
|
||||
// TRACE("frskyData.hub.distFromEarthAxis=%d", frskyData.hub.distFromEarthAxis);
|
||||
}
|
||||
|
||||
void getGpsDistance()
|
||||
{
|
||||
uint32_t lat, lng;
|
||||
|
||||
extractLatitudeLongitude(&lat, &lng);
|
||||
|
||||
// printf("lat=%d (%d), long=%d (%d)\n", lat, abs(lat - frskyData.hub.pilotLatitude), lng, abs(lng - frskyData.hub.pilotLongitude));
|
||||
|
||||
uint32_t angle = (lat > frskyData.hub.pilotLatitude) ? lat - frskyData.hub.pilotLatitude : frskyData.hub.pilotLatitude - lat;
|
||||
uint32_t dist = EARTH_RADIUS * angle / 1000000;
|
||||
uint32_t result = dist*dist;
|
||||
|
||||
angle = (lng > frskyData.hub.pilotLongitude) ? lng - frskyData.hub.pilotLongitude : frskyData.hub.pilotLongitude - lng;
|
||||
dist = frskyData.hub.distFromEarthAxis * angle / 1000000;
|
||||
result += dist*dist;
|
||||
|
||||
dist = abs(TELEMETRY_BARO_ALT_AVAILABLE() ? TELEMETRY_RELATIVE_BARO_ALT_BP : TELEMETRY_RELATIVE_GPS_ALT_BP);
|
||||
result += dist*dist;
|
||||
|
||||
frskyData.hub.gpsDistance = isqrt32(result);
|
||||
if (frskyData.hub.gpsDistance > frskyData.hub.maxGpsDistance)
|
||||
frskyData.hub.maxGpsDistance = frskyData.hub.gpsDistance;
|
||||
}
|
||||
#endif
|
||||
|
|
5702
radio/src/opentx.cpp
5702
radio/src/opentx.cpp
File diff suppressed because it is too large
Load diff
|
@ -18,471 +18,471 @@
|
|||
* GNU General Public License for more details.
|
||||
*/
|
||||
|
||||
#include <limits.h>
|
||||
#include "opentx.h"
|
||||
|
||||
extern void rtcdriver_settime(struct gtm * t);
|
||||
|
||||
#define LEAP_SECONDS_POSSIBLE 0
|
||||
|
||||
/* Shift A right by B bits portably, by dividing A by 2**B and
|
||||
truncating towards minus infinity. A and B should be free of side
|
||||
effects, and B should be in the range 0 <= B <= INT_BITS - 2, where
|
||||
INT_BITS is the number of useful bits in an int. GNU code can
|
||||
assume that INT_BITS is at least 32.
|
||||
|
||||
ISO C99 says that A >> B is implementation-defined if A < 0. Some
|
||||
implementations (e.g., UNICOS 9.0 on a Cray Y-MP EL) don't shift
|
||||
right in the usual way when A < 0, so SHR falls back on division if
|
||||
ordinary A >> B doesn't seem to be the usual signed shift. */
|
||||
#define SHR(a, b) (-1 >> 1 == -1 ? (a) >> (b) : (a) / (1 << (b)) - ((a) % (1 << (b)) < 0))
|
||||
|
||||
/* The extra casts in the following macros work around compiler bugs,
|
||||
e.g., in Cray C 5.0.3.0. */
|
||||
|
||||
/* True if the arithmetic type T is an integer type. bool counts as
|
||||
an integer. */
|
||||
#define TYPE_IS_INTEGER(t) ((t) 1.5 == 1)
|
||||
|
||||
/* True if negative values of the signed integer type T use two's
|
||||
complement, ones' complement, or signed magnitude representation,
|
||||
respectively. Much GNU code assumes two's complement, but some
|
||||
people like to be portable to all possible C hosts. */
|
||||
#define TYPE_TWOS_COMPLEMENT(t) ((t) ~ (t) 0 == (t) -1)
|
||||
#define TYPE_ONES_COMPLEMENT(t) ((t) ~ (t) 0 == 0)
|
||||
#define TYPE_SIGNED_MAGNITUDE(t) ((t) ~ (t) 0 < (t) -1)
|
||||
|
||||
/* True if the arithmetic type T is signed. */
|
||||
#define TYPE_SIGNED(t) (! ((t) 0 < (t) -1))
|
||||
|
||||
/* The maximum and minimum values for the integer type T. These
|
||||
macros have undefined behavior if T is signed and has padding bits.
|
||||
If this is a problem for you, please let us know how to fix it for
|
||||
your host. */
|
||||
#define TYPE_MINIMUM(t) \
|
||||
((t) (! TYPE_SIGNED (t) \
|
||||
? (t) 0 \
|
||||
: TYPE_SIGNED_MAGNITUDE (t) \
|
||||
? ~ (t) 0 \
|
||||
: ~ (t) 0 << (sizeof (t) * CHAR_BIT - 1)))
|
||||
#define TYPE_MAXIMUM(t) \
|
||||
((t) (! TYPE_SIGNED (t) \
|
||||
? (t) -1 \
|
||||
: ~ (~ (t) 0 << (sizeof (t) * CHAR_BIT - 1))))
|
||||
|
||||
#ifndef TIME_T_MIN
|
||||
# define TIME_T_MIN TYPE_MINIMUM (gtime_t)
|
||||
#endif
|
||||
#ifndef TIME_T_MAX
|
||||
# define TIME_T_MAX TYPE_MAXIMUM (gtime_t)
|
||||
#endif
|
||||
#define TIME_T_MIDPOINT (SHR (TIME_T_MIN + TIME_T_MAX, 1) + 1)
|
||||
|
||||
/* Verify a requirement at compile-time (unlike assert, which is runtime). */
|
||||
#define verify(name, assertion) struct name { char a[(assertion) ? 1 : -1]; }
|
||||
|
||||
verify (gtime_t_is_integer, TYPE_IS_INTEGER (gtime_t));
|
||||
verify (twos_complement_arithmetic, TYPE_TWOS_COMPLEMENT (int));
|
||||
/* The code also assumes that signed integer overflow silently wraps
|
||||
around, but this assumption can't be stated without causing a
|
||||
diagnostic on some hosts. */
|
||||
|
||||
#define EPOCH_YEAR 1970
|
||||
#define TM_YEAR_BASE 1900
|
||||
verify (base_year_is_a_multiple_of_100, TM_YEAR_BASE % 100 == 0);
|
||||
|
||||
/* Return 1 if YEAR + TM_YEAR_BASE is a leap year. */
|
||||
static inline int
|
||||
leapyear (long int year)
|
||||
{
|
||||
/* Don't add YEAR to TM_YEAR_BASE, as that might overflow.
|
||||
Also, work even if YEAR is negative. */
|
||||
return
|
||||
((year & 3) == 0
|
||||
&& (year % 100 != 0
|
||||
|| ((year / 100) & 3) == (- (TM_YEAR_BASE / 100) & 3)));
|
||||
}
|
||||
|
||||
const unsigned short int __mon_yday[2][13] =
|
||||
{
|
||||
/* Normal years. */
|
||||
{ 0, 31, 59, 90, 120, 151, 181, 212, 243, 273, 304, 334, 365 },
|
||||
/* Leap years. */
|
||||
{ 0, 31, 60, 91, 121, 152, 182, 213, 244, 274, 305, 335, 366 }
|
||||
};
|
||||
|
||||
/* Compute the `struct tm' representation of *T,
|
||||
offset OFFSET seconds east of UTC,
|
||||
and store year, yday, mon, mday, wday, hour, min, sec into *TP.
|
||||
Return nonzero if successful. */
|
||||
int
|
||||
__offtime (
|
||||
gtime_t *t,
|
||||
long int offset,
|
||||
struct gtm *tp)
|
||||
{
|
||||
long int days, rem, y;
|
||||
const unsigned short int *ip;
|
||||
|
||||
days = *t / SECS_PER_DAY;
|
||||
rem = *t % SECS_PER_DAY;
|
||||
rem += offset;
|
||||
while (rem < 0)
|
||||
{
|
||||
rem += SECS_PER_DAY;
|
||||
--days;
|
||||
}
|
||||
while (rem >= (long int)SECS_PER_DAY)
|
||||
{
|
||||
rem -= SECS_PER_DAY;
|
||||
++days;
|
||||
}
|
||||
tp->tm_hour = rem / SECS_PER_HOUR;
|
||||
rem %= SECS_PER_HOUR;
|
||||
tp->tm_min = rem / 60;
|
||||
tp->tm_sec = rem % 60;
|
||||
/* January 1, 1970 was a Thursday. */
|
||||
tp->tm_wday = (4 + days) % 7;
|
||||
if (tp->tm_wday < 0)
|
||||
tp->tm_wday += 7;
|
||||
y = 1970;
|
||||
|
||||
#define DIV(a, b) ((a) / (b) - ((a) % (b) < 0))
|
||||
#define LEAPS_THRU_END_OF(y) (DIV (y, 4) - DIV (y, 100) + DIV (y, 400))
|
||||
|
||||
while (days < 0 || days >= (leapyear (y) ? 366 : 365))
|
||||
{
|
||||
/* Guess a corrected year, assuming 365 days per year. */
|
||||
long int yg = y + days / 365 - (days % 365 < 0);
|
||||
|
||||
/* Adjust DAYS and Y to match the guessed year. */
|
||||
days -= ((yg - y) * 365
|
||||
+ LEAPS_THRU_END_OF (yg - 1)
|
||||
- LEAPS_THRU_END_OF (y - 1));
|
||||
y = yg;
|
||||
}
|
||||
tp->tm_year = y - 1900;
|
||||
if (tp->tm_year != y - 1900)
|
||||
{
|
||||
/* The year cannot be represented due to overflow. */
|
||||
// __set_errno (EOVERFLOW);
|
||||
return 0;
|
||||
}
|
||||
tp->tm_yday = days;
|
||||
ip = __mon_yday[leapyear(y)];
|
||||
for (y = 11; days < (long int) ip[y]; --y)
|
||||
continue;
|
||||
days -= ip[y];
|
||||
tp->tm_mon = y;
|
||||
tp->tm_mday = days + 1;
|
||||
return 1;
|
||||
}
|
||||
|
||||
/* time_r function implementations */
|
||||
// G: No time zones in our implementation so just do the converion from gtime_t to struct tm
|
||||
struct gtm *
|
||||
__localtime_r (gtime_t * t, struct gtm * tp)
|
||||
{
|
||||
__offtime(t, 0, tp);
|
||||
return tp;
|
||||
}
|
||||
|
||||
|
||||
/* Return an integer value measuring (YEAR1-YDAY1 HOUR1:MIN1:SEC1) -
|
||||
(YEAR0-YDAY0 HOUR0:MIN0:SEC0) in seconds, assuming that the clocks
|
||||
were not adjusted between the time stamps.
|
||||
|
||||
The YEAR values uses the same numbering as TP->tm_year. Values
|
||||
need not be in the usual range. However, YEAR1 must not be less
|
||||
than 2 * INT_MIN or greater than 2 * INT_MAX.
|
||||
|
||||
The result may overflow. It is the caller's responsibility to
|
||||
detect overflow. */
|
||||
|
||||
static inline gtime_t
|
||||
ydhms_diff (long int year1, long int yday1, int hour1, int min1, int sec1,
|
||||
int year0, int yday0, int hour0, int min0, int sec0)
|
||||
{
|
||||
verify (C99_integer_division, -1 / 2 == 0);
|
||||
verify (long_int_year_and_yday_are_wide_enough,
|
||||
INT_MAX <= LONG_MAX / 2 || TIME_T_MAX <= UINT_MAX);
|
||||
|
||||
/* Compute intervening leap days correctly even if year is negative.
|
||||
Take care to avoid integer overflow here. */
|
||||
int a4 = SHR (year1, 2) + SHR (TM_YEAR_BASE, 2) - ! (year1 & 3);
|
||||
int b4 = SHR (year0, 2) + SHR (TM_YEAR_BASE, 2) - ! (year0 & 3);
|
||||
int a100 = a4 / 25 - (a4 % 25 < 0);
|
||||
int b100 = b4 / 25 - (b4 % 25 < 0);
|
||||
int a400 = SHR (a100, 2);
|
||||
int b400 = SHR (b100, 2);
|
||||
int intervening_leap_days = (a4 - b4) - (a100 - b100) + (a400 - b400);
|
||||
|
||||
/* Compute the desired time in gtime_t precision. Overflow might
|
||||
occur here. */
|
||||
gtime_t tyear1 = year1;
|
||||
gtime_t years = tyear1 - year0;
|
||||
gtime_t days = 365 * years + yday1 - yday0 + intervening_leap_days;
|
||||
gtime_t hours = 24 * days + hour1 - hour0;
|
||||
gtime_t minutes = 60 * hours + min1 - min0;
|
||||
gtime_t seconds = 60 * minutes + sec1 - sec0;
|
||||
return seconds;
|
||||
}
|
||||
|
||||
/* Return a gtime_t value corresponding to (YEAR-YDAY HOUR:MIN:SEC),
|
||||
assuming that *T corresponds to *TP and that no clock adjustments
|
||||
occurred between *TP and the desired time.
|
||||
If TP is null, return a value not equal to *T; this avoids false matches.
|
||||
If overflow occurs, yield the minimal or maximal value, except do not
|
||||
yield a value equal to *T. */
|
||||
static gtime_t
|
||||
guess_time_tm (long int year, long int yday, int hour, int min, int sec,
|
||||
gtime_t *t, struct gtm *tp)
|
||||
{
|
||||
if (tp)
|
||||
{
|
||||
gtime_t d = ydhms_diff (year, yday, hour, min, sec,
|
||||
tp->tm_year, tp->tm_yday,
|
||||
tp->tm_hour, tp->tm_min, tp->tm_sec);
|
||||
gtime_t t1 = *t + d;
|
||||
if ((t1 < *t) == (TYPE_SIGNED (gtime_t) ? d < 0 : TIME_T_MAX / 2 < d))
|
||||
return t1;
|
||||
}
|
||||
|
||||
/* Overflow occurred one way or another. Return the nearest result
|
||||
that is actually in range, except don't report a zero difference
|
||||
if the actual difference is nonzero, as that would cause a false
|
||||
match; and don't oscillate between two values, as that would
|
||||
confuse the spring-forward gap detector. */
|
||||
return (*t < TIME_T_MIDPOINT
|
||||
? (*t <= TIME_T_MIN + 1 ? *t + 1 : TIME_T_MIN)
|
||||
: (TIME_T_MAX - 1 <= *t ? *t - 1 : TIME_T_MAX));
|
||||
}
|
||||
|
||||
/* Use CONVERT to convert *T to a broken down time in *TP.
|
||||
If *T is out of range for conversion, adjust it so that
|
||||
it is the nearest in-range value and then convert that. */
|
||||
static struct gtm *
|
||||
ranged_convert (struct gtm *(*convert) (gtime_t *, struct gtm *),
|
||||
gtime_t *t, struct gtm *tp)
|
||||
{
|
||||
struct gtm *r = convert (t, tp);
|
||||
|
||||
if (!r && *t)
|
||||
{
|
||||
gtime_t bad = *t;
|
||||
gtime_t ok = 0;
|
||||
|
||||
/* BAD is a known unconvertible gtime_t, and OK is a known good one.
|
||||
Use binary search to narrow the range between BAD and OK until
|
||||
they differ by 1. */
|
||||
while (bad != ok + (bad < 0 ? -1 : 1))
|
||||
{
|
||||
gtime_t mid = *t = (bad < 0
|
||||
? bad + ((ok - bad) >> 1)
|
||||
: ok + ((bad - ok) >> 1));
|
||||
r = convert (t, tp);
|
||||
if (r)
|
||||
ok = mid;
|
||||
else
|
||||
bad = mid;
|
||||
}
|
||||
|
||||
if (!r && ok)
|
||||
{
|
||||
/* The last conversion attempt failed;
|
||||
revert to the most recent successful attempt. */
|
||||
*t = ok;
|
||||
r = convert (t, tp);
|
||||
}
|
||||
}
|
||||
|
||||
return r;
|
||||
}
|
||||
|
||||
/* Convert *TP to a gtime_t value, inverting
|
||||
the monotonic and mostly-unit-linear conversion function CONVERT.
|
||||
Use *OFFSET to keep track of a guess at the offset of the result,
|
||||
compared to what the result would be for UTC without leap seconds.
|
||||
If *OFFSET's guess is correct, only one CONVERT call is needed.
|
||||
This function is external because it is used also by timegm.c. */
|
||||
gtime_t
|
||||
__mktime_internal (struct gtm *tp,
|
||||
struct gtm *(*convert) (gtime_t *, struct gtm *),
|
||||
gtime_t *offset)
|
||||
{
|
||||
gtime_t t, gt, t0, t1, t2;
|
||||
struct gtm tm;
|
||||
|
||||
/* The maximum number of probes (calls to CONVERT) should be enough
|
||||
to handle any combinations of time zone rule changes, solar time,
|
||||
leap seconds, and oscillations around a spring-forward gap.
|
||||
POSIX.1 prohibits leap seconds, but some hosts have them anyway. */
|
||||
int remaining_probes = 6;
|
||||
|
||||
/* Time requested. Copy it in case CONVERT modifies *TP; this can
|
||||
occur if TP is localtime's returned value and CONVERT is localtime. */
|
||||
int sec = tp->tm_sec;
|
||||
int min = tp->tm_min;
|
||||
int hour = tp->tm_hour;
|
||||
int mday = tp->tm_mday;
|
||||
int mon = tp->tm_mon;
|
||||
int year_requested = tp->tm_year;
|
||||
|
||||
/* Ensure that mon is in range, and set year accordingly. */
|
||||
int mon_remainder = mon % 12;
|
||||
int negative_mon_remainder = mon_remainder < 0;
|
||||
int mon_years = mon / 12 - negative_mon_remainder;
|
||||
long int lyear_requested = year_requested;
|
||||
long int year = lyear_requested + mon_years;
|
||||
|
||||
/* The other values need not be in range:
|
||||
the remaining code handles minor overflows correctly,
|
||||
assuming int and gtime_t arithmetic wraps around.
|
||||
Major overflows are caught at the end. */
|
||||
|
||||
/* Calculate day of year from year, month, and day of month.
|
||||
The result need not be in range. */
|
||||
int mon_yday = ((__mon_yday[leapyear (year)]
|
||||
[mon_remainder + 12 * negative_mon_remainder])
|
||||
- 1);
|
||||
long int lmday = mday;
|
||||
long int yday = mon_yday + lmday;
|
||||
|
||||
gtime_t guessed_offset = *offset;
|
||||
|
||||
int sec_requested = sec;
|
||||
|
||||
/*
|
||||
if (LEAP_SECONDS_POSSIBLE)
|
||||
{
|
||||
// Handle out-of-range seconds specially,
|
||||
// since ydhms_tm_diff assumes every minute has 60 seconds.
|
||||
if (sec < 0)
|
||||
sec = 0;
|
||||
if (59 < sec)
|
||||
sec = 59;
|
||||
}
|
||||
*/
|
||||
|
||||
/* Invert CONVERT by probing. First assume the same offset as last
|
||||
time. */
|
||||
|
||||
t0 = ydhms_diff (year, yday, hour, min, sec,
|
||||
EPOCH_YEAR - TM_YEAR_BASE, 0, 0, 0, - guessed_offset);
|
||||
|
||||
if (TIME_T_MAX / INT_MAX / 366 / 24 / 60 / 60 < 3)
|
||||
{
|
||||
/* gtime_t isn't large enough to rule out overflows, so check
|
||||
for major overflows. A gross check suffices, since if t0
|
||||
has overflowed, it is off by a multiple of TIME_T_MAX -
|
||||
TIME_T_MIN + 1. So ignore any component of the difference
|
||||
that is bounded by a small value. */
|
||||
|
||||
/* Approximate log base 2 of the number of time units per
|
||||
biennium. A biennium is 2 years; use this unit instead of
|
||||
years to avoid integer overflow. For example, 2 average
|
||||
Gregorian years are 2 * 365.2425 * 24 * 60 * 60 seconds,
|
||||
which is 63113904 seconds, and rint (log2 (63113904)) is
|
||||
26. */
|
||||
int ALOG2_SECONDS_PER_BIENNIUM = 26;
|
||||
int ALOG2_MINUTES_PER_BIENNIUM = 20;
|
||||
int ALOG2_HOURS_PER_BIENNIUM = 14;
|
||||
int ALOG2_DAYS_PER_BIENNIUM = 10;
|
||||
int LOG2_YEARS_PER_BIENNIUM = 1;
|
||||
|
||||
int approx_requested_biennia =
|
||||
(SHR (year_requested, LOG2_YEARS_PER_BIENNIUM)
|
||||
- SHR (EPOCH_YEAR - TM_YEAR_BASE, LOG2_YEARS_PER_BIENNIUM)
|
||||
+ SHR (mday, ALOG2_DAYS_PER_BIENNIUM)
|
||||
+ SHR (hour, ALOG2_HOURS_PER_BIENNIUM)
|
||||
+ SHR (min, ALOG2_MINUTES_PER_BIENNIUM)
|
||||
+ (LEAP_SECONDS_POSSIBLE
|
||||
? 0
|
||||
: SHR (sec, ALOG2_SECONDS_PER_BIENNIUM)));
|
||||
|
||||
int approx_biennia = SHR (t0, ALOG2_SECONDS_PER_BIENNIUM);
|
||||
int diff = approx_biennia - approx_requested_biennia;
|
||||
int abs_diff = diff < 0 ? - diff : diff;
|
||||
|
||||
/* IRIX 4.0.5 cc miscalculates TIME_T_MIN / 3: it erroneously
|
||||
gives a positive value of 715827882. Setting a variable
|
||||
first then doing math on it seems to work.
|
||||
(ghazi@caip.rutgers.edu) */
|
||||
gtime_t time_t_max = TIME_T_MAX;
|
||||
gtime_t time_t_min = TIME_T_MIN;
|
||||
gtime_t overflow_threshold =
|
||||
(time_t_max / 3 - time_t_min / 3) >> ALOG2_SECONDS_PER_BIENNIUM;
|
||||
|
||||
if (overflow_threshold < abs_diff)
|
||||
{
|
||||
/* Overflow occurred. Try repairing it; this might work if
|
||||
the time zone offset is enough to undo the overflow. */
|
||||
gtime_t repaired_t0 = -1 - t0;
|
||||
approx_biennia = SHR (repaired_t0, ALOG2_SECONDS_PER_BIENNIUM);
|
||||
diff = approx_biennia - approx_requested_biennia;
|
||||
abs_diff = diff < 0 ? - diff : diff;
|
||||
if (overflow_threshold < abs_diff)
|
||||
return -1;
|
||||
guessed_offset += repaired_t0 - t0;
|
||||
t0 = repaired_t0;
|
||||
}
|
||||
}
|
||||
|
||||
/* Repeatedly use the error to improve the guess. */
|
||||
|
||||
for (t = t1 = t2 = t0;
|
||||
(gt = guess_time_tm (year, yday, hour, min, sec, &t,
|
||||
ranged_convert (convert, &t, &tm)),
|
||||
t != gt);
|
||||
t1 = t2, t2 = t, t = gt)
|
||||
if (t == t1 && t != t2)
|
||||
goto offset_found;
|
||||
else if (--remaining_probes == 0)
|
||||
return -1;
|
||||
|
||||
offset_found:
|
||||
*offset = guessed_offset + t - t0;
|
||||
|
||||
if (LEAP_SECONDS_POSSIBLE && sec_requested != tm.tm_sec)
|
||||
{
|
||||
/* Adjust time to reflect the tm_sec requested, not the normalized value.
|
||||
Also, repair any damage from a false match due to a leap second. */
|
||||
int sec_adjustment = (sec == 0 && tm.tm_sec == 60) - sec;
|
||||
t1 = t + sec_requested;
|
||||
t2 = t1 + sec_adjustment;
|
||||
if (((t1 < t) != (sec_requested < 0))
|
||||
| ((t2 < t1) != (sec_adjustment < 0))
|
||||
| ! convert (&t2, &tm))
|
||||
return -1;
|
||||
t = t2;
|
||||
}
|
||||
|
||||
*tp = tm;
|
||||
return t;
|
||||
}
|
||||
|
||||
/* Convert *TP to a gtime_t value. */
|
||||
gtime_t
|
||||
gmktime (struct gtm *tp)
|
||||
{
|
||||
// no time zone stuff. Just do the math ;)
|
||||
static gtime_t localtime_offset;
|
||||
return __mktime_internal (tp, __localtime_r, &localtime_offset);
|
||||
}
|
||||
|
||||
/* Fill a (struct tm) TP* from a given gtime_t time stamp */
|
||||
gtime_t
|
||||
filltm(gtime_t *t, struct gtm *tp)
|
||||
{
|
||||
return __offtime(t, 0, tp);
|
||||
}
|
||||
|
||||
gtime_t g_rtcTime;
|
||||
uint8_t g_ms100 = 0; // global to allow time set function to reset to zero
|
||||
|
||||
void gettime(struct gtm * tm)
|
||||
{
|
||||
filltm(&g_rtcTime, tm); // create a struct tm date/time structure from global unix time stamp
|
||||
}
|
||||
|
||||
|
||||
#include <limits.h>
|
||||
#include "opentx.h"
|
||||
|
||||
extern void rtcdriver_settime(struct gtm * t);
|
||||
|
||||
#define LEAP_SECONDS_POSSIBLE 0
|
||||
|
||||
/* Shift A right by B bits portably, by dividing A by 2**B and
|
||||
truncating towards minus infinity. A and B should be free of side
|
||||
effects, and B should be in the range 0 <= B <= INT_BITS - 2, where
|
||||
INT_BITS is the number of useful bits in an int. GNU code can
|
||||
assume that INT_BITS is at least 32.
|
||||
|
||||
ISO C99 says that A >> B is implementation-defined if A < 0. Some
|
||||
implementations (e.g., UNICOS 9.0 on a Cray Y-MP EL) don't shift
|
||||
right in the usual way when A < 0, so SHR falls back on division if
|
||||
ordinary A >> B doesn't seem to be the usual signed shift. */
|
||||
#define SHR(a, b) (-1 >> 1 == -1 ? (a) >> (b) : (a) / (1 << (b)) - ((a) % (1 << (b)) < 0))
|
||||
|
||||
/* The extra casts in the following macros work around compiler bugs,
|
||||
e.g., in Cray C 5.0.3.0. */
|
||||
|
||||
/* True if the arithmetic type T is an integer type. bool counts as
|
||||
an integer. */
|
||||
#define TYPE_IS_INTEGER(t) ((t) 1.5 == 1)
|
||||
|
||||
/* True if negative values of the signed integer type T use two's
|
||||
complement, ones' complement, or signed magnitude representation,
|
||||
respectively. Much GNU code assumes two's complement, but some
|
||||
people like to be portable to all possible C hosts. */
|
||||
#define TYPE_TWOS_COMPLEMENT(t) ((t) ~ (t) 0 == (t) -1)
|
||||
#define TYPE_ONES_COMPLEMENT(t) ((t) ~ (t) 0 == 0)
|
||||
#define TYPE_SIGNED_MAGNITUDE(t) ((t) ~ (t) 0 < (t) -1)
|
||||
|
||||
/* True if the arithmetic type T is signed. */
|
||||
#define TYPE_SIGNED(t) (! ((t) 0 < (t) -1))
|
||||
|
||||
/* The maximum and minimum values for the integer type T. These
|
||||
macros have undefined behavior if T is signed and has padding bits.
|
||||
If this is a problem for you, please let us know how to fix it for
|
||||
your host. */
|
||||
#define TYPE_MINIMUM(t) \
|
||||
((t) (! TYPE_SIGNED (t) \
|
||||
? (t) 0 \
|
||||
: TYPE_SIGNED_MAGNITUDE (t) \
|
||||
? ~ (t) 0 \
|
||||
: ~ (t) 0 << (sizeof (t) * CHAR_BIT - 1)))
|
||||
#define TYPE_MAXIMUM(t) \
|
||||
((t) (! TYPE_SIGNED (t) \
|
||||
? (t) -1 \
|
||||
: ~ (~ (t) 0 << (sizeof (t) * CHAR_BIT - 1))))
|
||||
|
||||
#ifndef TIME_T_MIN
|
||||
# define TIME_T_MIN TYPE_MINIMUM (gtime_t)
|
||||
#endif
|
||||
#ifndef TIME_T_MAX
|
||||
# define TIME_T_MAX TYPE_MAXIMUM (gtime_t)
|
||||
#endif
|
||||
#define TIME_T_MIDPOINT (SHR (TIME_T_MIN + TIME_T_MAX, 1) + 1)
|
||||
|
||||
/* Verify a requirement at compile-time (unlike assert, which is runtime). */
|
||||
#define verify(name, assertion) struct name { char a[(assertion) ? 1 : -1]; }
|
||||
|
||||
verify (gtime_t_is_integer, TYPE_IS_INTEGER (gtime_t));
|
||||
verify (twos_complement_arithmetic, TYPE_TWOS_COMPLEMENT (int));
|
||||
/* The code also assumes that signed integer overflow silently wraps
|
||||
around, but this assumption can't be stated without causing a
|
||||
diagnostic on some hosts. */
|
||||
|
||||
#define EPOCH_YEAR 1970
|
||||
#define TM_YEAR_BASE 1900
|
||||
verify (base_year_is_a_multiple_of_100, TM_YEAR_BASE % 100 == 0);
|
||||
|
||||
/* Return 1 if YEAR + TM_YEAR_BASE is a leap year. */
|
||||
static inline int
|
||||
leapyear (long int year)
|
||||
{
|
||||
/* Don't add YEAR to TM_YEAR_BASE, as that might overflow.
|
||||
Also, work even if YEAR is negative. */
|
||||
return
|
||||
((year & 3) == 0
|
||||
&& (year % 100 != 0
|
||||
|| ((year / 100) & 3) == (- (TM_YEAR_BASE / 100) & 3)));
|
||||
}
|
||||
|
||||
const unsigned short int __mon_yday[2][13] =
|
||||
{
|
||||
/* Normal years. */
|
||||
{ 0, 31, 59, 90, 120, 151, 181, 212, 243, 273, 304, 334, 365 },
|
||||
/* Leap years. */
|
||||
{ 0, 31, 60, 91, 121, 152, 182, 213, 244, 274, 305, 335, 366 }
|
||||
};
|
||||
|
||||
/* Compute the `struct tm' representation of *T,
|
||||
offset OFFSET seconds east of UTC,
|
||||
and store year, yday, mon, mday, wday, hour, min, sec into *TP.
|
||||
Return nonzero if successful. */
|
||||
int
|
||||
__offtime (
|
||||
gtime_t *t,
|
||||
long int offset,
|
||||
struct gtm *tp)
|
||||
{
|
||||
long int days, rem, y;
|
||||
const unsigned short int *ip;
|
||||
|
||||
days = *t / SECS_PER_DAY;
|
||||
rem = *t % SECS_PER_DAY;
|
||||
rem += offset;
|
||||
while (rem < 0)
|
||||
{
|
||||
rem += SECS_PER_DAY;
|
||||
--days;
|
||||
}
|
||||
while (rem >= (long int)SECS_PER_DAY)
|
||||
{
|
||||
rem -= SECS_PER_DAY;
|
||||
++days;
|
||||
}
|
||||
tp->tm_hour = rem / SECS_PER_HOUR;
|
||||
rem %= SECS_PER_HOUR;
|
||||
tp->tm_min = rem / 60;
|
||||
tp->tm_sec = rem % 60;
|
||||
/* January 1, 1970 was a Thursday. */
|
||||
tp->tm_wday = (4 + days) % 7;
|
||||
if (tp->tm_wday < 0)
|
||||
tp->tm_wday += 7;
|
||||
y = 1970;
|
||||
|
||||
#define DIV(a, b) ((a) / (b) - ((a) % (b) < 0))
|
||||
#define LEAPS_THRU_END_OF(y) (DIV (y, 4) - DIV (y, 100) + DIV (y, 400))
|
||||
|
||||
while (days < 0 || days >= (leapyear (y) ? 366 : 365))
|
||||
{
|
||||
/* Guess a corrected year, assuming 365 days per year. */
|
||||
long int yg = y + days / 365 - (days % 365 < 0);
|
||||
|
||||
/* Adjust DAYS and Y to match the guessed year. */
|
||||
days -= ((yg - y) * 365
|
||||
+ LEAPS_THRU_END_OF (yg - 1)
|
||||
- LEAPS_THRU_END_OF (y - 1));
|
||||
y = yg;
|
||||
}
|
||||
tp->tm_year = y - 1900;
|
||||
if (tp->tm_year != y - 1900)
|
||||
{
|
||||
/* The year cannot be represented due to overflow. */
|
||||
// __set_errno (EOVERFLOW);
|
||||
return 0;
|
||||
}
|
||||
tp->tm_yday = days;
|
||||
ip = __mon_yday[leapyear(y)];
|
||||
for (y = 11; days < (long int) ip[y]; --y)
|
||||
continue;
|
||||
days -= ip[y];
|
||||
tp->tm_mon = y;
|
||||
tp->tm_mday = days + 1;
|
||||
return 1;
|
||||
}
|
||||
|
||||
/* time_r function implementations */
|
||||
// G: No time zones in our implementation so just do the converion from gtime_t to struct tm
|
||||
struct gtm *
|
||||
__localtime_r (gtime_t * t, struct gtm * tp)
|
||||
{
|
||||
__offtime(t, 0, tp);
|
||||
return tp;
|
||||
}
|
||||
|
||||
|
||||
/* Return an integer value measuring (YEAR1-YDAY1 HOUR1:MIN1:SEC1) -
|
||||
(YEAR0-YDAY0 HOUR0:MIN0:SEC0) in seconds, assuming that the clocks
|
||||
were not adjusted between the time stamps.
|
||||
|
||||
The YEAR values uses the same numbering as TP->tm_year. Values
|
||||
need not be in the usual range. However, YEAR1 must not be less
|
||||
than 2 * INT_MIN or greater than 2 * INT_MAX.
|
||||
|
||||
The result may overflow. It is the caller's responsibility to
|
||||
detect overflow. */
|
||||
|
||||
static inline gtime_t
|
||||
ydhms_diff (long int year1, long int yday1, int hour1, int min1, int sec1,
|
||||
int year0, int yday0, int hour0, int min0, int sec0)
|
||||
{
|
||||
verify (C99_integer_division, -1 / 2 == 0);
|
||||
verify (long_int_year_and_yday_are_wide_enough,
|
||||
INT_MAX <= LONG_MAX / 2 || TIME_T_MAX <= UINT_MAX);
|
||||
|
||||
/* Compute intervening leap days correctly even if year is negative.
|
||||
Take care to avoid integer overflow here. */
|
||||
int a4 = SHR (year1, 2) + SHR (TM_YEAR_BASE, 2) - ! (year1 & 3);
|
||||
int b4 = SHR (year0, 2) + SHR (TM_YEAR_BASE, 2) - ! (year0 & 3);
|
||||
int a100 = a4 / 25 - (a4 % 25 < 0);
|
||||
int b100 = b4 / 25 - (b4 % 25 < 0);
|
||||
int a400 = SHR (a100, 2);
|
||||
int b400 = SHR (b100, 2);
|
||||
int intervening_leap_days = (a4 - b4) - (a100 - b100) + (a400 - b400);
|
||||
|
||||
/* Compute the desired time in gtime_t precision. Overflow might
|
||||
occur here. */
|
||||
gtime_t tyear1 = year1;
|
||||
gtime_t years = tyear1 - year0;
|
||||
gtime_t days = 365 * years + yday1 - yday0 + intervening_leap_days;
|
||||
gtime_t hours = 24 * days + hour1 - hour0;
|
||||
gtime_t minutes = 60 * hours + min1 - min0;
|
||||
gtime_t seconds = 60 * minutes + sec1 - sec0;
|
||||
return seconds;
|
||||
}
|
||||
|
||||
/* Return a gtime_t value corresponding to (YEAR-YDAY HOUR:MIN:SEC),
|
||||
assuming that *T corresponds to *TP and that no clock adjustments
|
||||
occurred between *TP and the desired time.
|
||||
If TP is null, return a value not equal to *T; this avoids false matches.
|
||||
If overflow occurs, yield the minimal or maximal value, except do not
|
||||
yield a value equal to *T. */
|
||||
static gtime_t
|
||||
guess_time_tm (long int year, long int yday, int hour, int min, int sec,
|
||||
gtime_t *t, struct gtm *tp)
|
||||
{
|
||||
if (tp)
|
||||
{
|
||||
gtime_t d = ydhms_diff (year, yday, hour, min, sec,
|
||||
tp->tm_year, tp->tm_yday,
|
||||
tp->tm_hour, tp->tm_min, tp->tm_sec);
|
||||
gtime_t t1 = *t + d;
|
||||
if ((t1 < *t) == (TYPE_SIGNED (gtime_t) ? d < 0 : TIME_T_MAX / 2 < d))
|
||||
return t1;
|
||||
}
|
||||
|
||||
/* Overflow occurred one way or another. Return the nearest result
|
||||
that is actually in range, except don't report a zero difference
|
||||
if the actual difference is nonzero, as that would cause a false
|
||||
match; and don't oscillate between two values, as that would
|
||||
confuse the spring-forward gap detector. */
|
||||
return (*t < TIME_T_MIDPOINT
|
||||
? (*t <= TIME_T_MIN + 1 ? *t + 1 : TIME_T_MIN)
|
||||
: (TIME_T_MAX - 1 <= *t ? *t - 1 : TIME_T_MAX));
|
||||
}
|
||||
|
||||
/* Use CONVERT to convert *T to a broken down time in *TP.
|
||||
If *T is out of range for conversion, adjust it so that
|
||||
it is the nearest in-range value and then convert that. */
|
||||
static struct gtm *
|
||||
ranged_convert (struct gtm *(*convert) (gtime_t *, struct gtm *),
|
||||
gtime_t *t, struct gtm *tp)
|
||||
{
|
||||
struct gtm *r = convert (t, tp);
|
||||
|
||||
if (!r && *t)
|
||||
{
|
||||
gtime_t bad = *t;
|
||||
gtime_t ok = 0;
|
||||
|
||||
/* BAD is a known unconvertible gtime_t, and OK is a known good one.
|
||||
Use binary search to narrow the range between BAD and OK until
|
||||
they differ by 1. */
|
||||
while (bad != ok + (bad < 0 ? -1 : 1))
|
||||
{
|
||||
gtime_t mid = *t = (bad < 0
|
||||
? bad + ((ok - bad) >> 1)
|
||||
: ok + ((bad - ok) >> 1));
|
||||
r = convert (t, tp);
|
||||
if (r)
|
||||
ok = mid;
|
||||
else
|
||||
bad = mid;
|
||||
}
|
||||
|
||||
if (!r && ok)
|
||||
{
|
||||
/* The last conversion attempt failed;
|
||||
revert to the most recent successful attempt. */
|
||||
*t = ok;
|
||||
r = convert (t, tp);
|
||||
}
|
||||
}
|
||||
|
||||
return r;
|
||||
}
|
||||
|
||||
/* Convert *TP to a gtime_t value, inverting
|
||||
the monotonic and mostly-unit-linear conversion function CONVERT.
|
||||
Use *OFFSET to keep track of a guess at the offset of the result,
|
||||
compared to what the result would be for UTC without leap seconds.
|
||||
If *OFFSET's guess is correct, only one CONVERT call is needed.
|
||||
This function is external because it is used also by timegm.c. */
|
||||
gtime_t
|
||||
__mktime_internal (struct gtm *tp,
|
||||
struct gtm *(*convert) (gtime_t *, struct gtm *),
|
||||
gtime_t *offset)
|
||||
{
|
||||
gtime_t t, gt, t0, t1, t2;
|
||||
struct gtm tm;
|
||||
|
||||
/* The maximum number of probes (calls to CONVERT) should be enough
|
||||
to handle any combinations of time zone rule changes, solar time,
|
||||
leap seconds, and oscillations around a spring-forward gap.
|
||||
POSIX.1 prohibits leap seconds, but some hosts have them anyway. */
|
||||
int remaining_probes = 6;
|
||||
|
||||
/* Time requested. Copy it in case CONVERT modifies *TP; this can
|
||||
occur if TP is localtime's returned value and CONVERT is localtime. */
|
||||
int sec = tp->tm_sec;
|
||||
int min = tp->tm_min;
|
||||
int hour = tp->tm_hour;
|
||||
int mday = tp->tm_mday;
|
||||
int mon = tp->tm_mon;
|
||||
int year_requested = tp->tm_year;
|
||||
|
||||
/* Ensure that mon is in range, and set year accordingly. */
|
||||
int mon_remainder = mon % 12;
|
||||
int negative_mon_remainder = mon_remainder < 0;
|
||||
int mon_years = mon / 12 - negative_mon_remainder;
|
||||
long int lyear_requested = year_requested;
|
||||
long int year = lyear_requested + mon_years;
|
||||
|
||||
/* The other values need not be in range:
|
||||
the remaining code handles minor overflows correctly,
|
||||
assuming int and gtime_t arithmetic wraps around.
|
||||
Major overflows are caught at the end. */
|
||||
|
||||
/* Calculate day of year from year, month, and day of month.
|
||||
The result need not be in range. */
|
||||
int mon_yday = ((__mon_yday[leapyear (year)]
|
||||
[mon_remainder + 12 * negative_mon_remainder])
|
||||
- 1);
|
||||
long int lmday = mday;
|
||||
long int yday = mon_yday + lmday;
|
||||
|
||||
gtime_t guessed_offset = *offset;
|
||||
|
||||
int sec_requested = sec;
|
||||
|
||||
/*
|
||||
if (LEAP_SECONDS_POSSIBLE)
|
||||
{
|
||||
// Handle out-of-range seconds specially,
|
||||
// since ydhms_tm_diff assumes every minute has 60 seconds.
|
||||
if (sec < 0)
|
||||
sec = 0;
|
||||
if (59 < sec)
|
||||
sec = 59;
|
||||
}
|
||||
*/
|
||||
|
||||
/* Invert CONVERT by probing. First assume the same offset as last
|
||||
time. */
|
||||
|
||||
t0 = ydhms_diff (year, yday, hour, min, sec,
|
||||
EPOCH_YEAR - TM_YEAR_BASE, 0, 0, 0, - guessed_offset);
|
||||
|
||||
if (TIME_T_MAX / INT_MAX / 366 / 24 / 60 / 60 < 3)
|
||||
{
|
||||
/* gtime_t isn't large enough to rule out overflows, so check
|
||||
for major overflows. A gross check suffices, since if t0
|
||||
has overflowed, it is off by a multiple of TIME_T_MAX -
|
||||
TIME_T_MIN + 1. So ignore any component of the difference
|
||||
that is bounded by a small value. */
|
||||
|
||||
/* Approximate log base 2 of the number of time units per
|
||||
biennium. A biennium is 2 years; use this unit instead of
|
||||
years to avoid integer overflow. For example, 2 average
|
||||
Gregorian years are 2 * 365.2425 * 24 * 60 * 60 seconds,
|
||||
which is 63113904 seconds, and rint (log2 (63113904)) is
|
||||
26. */
|
||||
int ALOG2_SECONDS_PER_BIENNIUM = 26;
|
||||
int ALOG2_MINUTES_PER_BIENNIUM = 20;
|
||||
int ALOG2_HOURS_PER_BIENNIUM = 14;
|
||||
int ALOG2_DAYS_PER_BIENNIUM = 10;
|
||||
int LOG2_YEARS_PER_BIENNIUM = 1;
|
||||
|
||||
int approx_requested_biennia =
|
||||
(SHR (year_requested, LOG2_YEARS_PER_BIENNIUM)
|
||||
- SHR (EPOCH_YEAR - TM_YEAR_BASE, LOG2_YEARS_PER_BIENNIUM)
|
||||
+ SHR (mday, ALOG2_DAYS_PER_BIENNIUM)
|
||||
+ SHR (hour, ALOG2_HOURS_PER_BIENNIUM)
|
||||
+ SHR (min, ALOG2_MINUTES_PER_BIENNIUM)
|
||||
+ (LEAP_SECONDS_POSSIBLE
|
||||
? 0
|
||||
: SHR (sec, ALOG2_SECONDS_PER_BIENNIUM)));
|
||||
|
||||
int approx_biennia = SHR (t0, ALOG2_SECONDS_PER_BIENNIUM);
|
||||
int diff = approx_biennia - approx_requested_biennia;
|
||||
int abs_diff = diff < 0 ? - diff : diff;
|
||||
|
||||
/* IRIX 4.0.5 cc miscalculates TIME_T_MIN / 3: it erroneously
|
||||
gives a positive value of 715827882. Setting a variable
|
||||
first then doing math on it seems to work.
|
||||
(ghazi@caip.rutgers.edu) */
|
||||
gtime_t time_t_max = TIME_T_MAX;
|
||||
gtime_t time_t_min = TIME_T_MIN;
|
||||
gtime_t overflow_threshold =
|
||||
(time_t_max / 3 - time_t_min / 3) >> ALOG2_SECONDS_PER_BIENNIUM;
|
||||
|
||||
if (overflow_threshold < abs_diff)
|
||||
{
|
||||
/* Overflow occurred. Try repairing it; this might work if
|
||||
the time zone offset is enough to undo the overflow. */
|
||||
gtime_t repaired_t0 = -1 - t0;
|
||||
approx_biennia = SHR (repaired_t0, ALOG2_SECONDS_PER_BIENNIUM);
|
||||
diff = approx_biennia - approx_requested_biennia;
|
||||
abs_diff = diff < 0 ? - diff : diff;
|
||||
if (overflow_threshold < abs_diff)
|
||||
return -1;
|
||||
guessed_offset += repaired_t0 - t0;
|
||||
t0 = repaired_t0;
|
||||
}
|
||||
}
|
||||
|
||||
/* Repeatedly use the error to improve the guess. */
|
||||
|
||||
for (t = t1 = t2 = t0;
|
||||
(gt = guess_time_tm (year, yday, hour, min, sec, &t,
|
||||
ranged_convert (convert, &t, &tm)),
|
||||
t != gt);
|
||||
t1 = t2, t2 = t, t = gt)
|
||||
if (t == t1 && t != t2)
|
||||
goto offset_found;
|
||||
else if (--remaining_probes == 0)
|
||||
return -1;
|
||||
|
||||
offset_found:
|
||||
*offset = guessed_offset + t - t0;
|
||||
|
||||
if (LEAP_SECONDS_POSSIBLE && sec_requested != tm.tm_sec)
|
||||
{
|
||||
/* Adjust time to reflect the tm_sec requested, not the normalized value.
|
||||
Also, repair any damage from a false match due to a leap second. */
|
||||
int sec_adjustment = (sec == 0 && tm.tm_sec == 60) - sec;
|
||||
t1 = t + sec_requested;
|
||||
t2 = t1 + sec_adjustment;
|
||||
if (((t1 < t) != (sec_requested < 0))
|
||||
| ((t2 < t1) != (sec_adjustment < 0))
|
||||
| ! convert (&t2, &tm))
|
||||
return -1;
|
||||
t = t2;
|
||||
}
|
||||
|
||||
*tp = tm;
|
||||
return t;
|
||||
}
|
||||
|
||||
/* Convert *TP to a gtime_t value. */
|
||||
gtime_t
|
||||
gmktime (struct gtm *tp)
|
||||
{
|
||||
// no time zone stuff. Just do the math ;)
|
||||
static gtime_t localtime_offset;
|
||||
return __mktime_internal (tp, __localtime_r, &localtime_offset);
|
||||
}
|
||||
|
||||
/* Fill a (struct tm) TP* from a given gtime_t time stamp */
|
||||
gtime_t
|
||||
filltm(gtime_t *t, struct gtm *tp)
|
||||
{
|
||||
return __offtime(t, 0, tp);
|
||||
}
|
||||
|
||||
gtime_t g_rtcTime;
|
||||
uint8_t g_ms100 = 0; // global to allow time set function to reset to zero
|
||||
|
||||
void gettime(struct gtm * tm)
|
||||
{
|
||||
filltm(&g_rtcTime, tm); // create a struct tm date/time structure from global unix time stamp
|
||||
}
|
||||
|
||||
|
||||
|
|
|
@ -18,40 +18,40 @@
|
|||
* GNU General Public License for more details.
|
||||
*/
|
||||
|
||||
#include "opentx.h"
|
||||
#include "serial.h"
|
||||
#include <stdarg.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#define PRINTF_BUFFER_SIZE 128
|
||||
|
||||
void serialPutc(char c)
|
||||
{
|
||||
#if defined(USB_SERIAL)
|
||||
usbSerialPutc(c);
|
||||
#else
|
||||
serial2Putc(c);
|
||||
#endif
|
||||
}
|
||||
|
||||
void serialPrintf(const char * format, ...)
|
||||
{
|
||||
va_list arglist;
|
||||
char tmp[PRINTF_BUFFER_SIZE+1];
|
||||
|
||||
va_start(arglist, format);
|
||||
vsnprintf(tmp, PRINTF_BUFFER_SIZE, format, arglist);
|
||||
tmp[PRINTF_BUFFER_SIZE] = '\0';
|
||||
va_end(arglist);
|
||||
|
||||
const char *t = tmp;
|
||||
while (*t) {
|
||||
serialPutc(*t++);
|
||||
}
|
||||
}
|
||||
|
||||
void serialCrlf()
|
||||
{
|
||||
serialPutc('\r');
|
||||
serialPutc('\n');
|
||||
}
|
||||
#include "opentx.h"
|
||||
#include "serial.h"
|
||||
#include <stdarg.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#define PRINTF_BUFFER_SIZE 128
|
||||
|
||||
void serialPutc(char c)
|
||||
{
|
||||
#if defined(USB_SERIAL)
|
||||
usbSerialPutc(c);
|
||||
#else
|
||||
serial2Putc(c);
|
||||
#endif
|
||||
}
|
||||
|
||||
void serialPrintf(const char * format, ...)
|
||||
{
|
||||
va_list arglist;
|
||||
char tmp[PRINTF_BUFFER_SIZE+1];
|
||||
|
||||
va_start(arglist, format);
|
||||
vsnprintf(tmp, PRINTF_BUFFER_SIZE, format, arglist);
|
||||
tmp[PRINTF_BUFFER_SIZE] = '\0';
|
||||
va_end(arglist);
|
||||
|
||||
const char *t = tmp;
|
||||
while (*t) {
|
||||
serialPutc(*t++);
|
||||
}
|
||||
}
|
||||
|
||||
void serialCrlf()
|
||||
{
|
||||
serialPutc('\r');
|
||||
serialPutc('\n');
|
||||
}
|
||||
|
|
File diff suppressed because it is too large
Load diff
|
@ -1,209 +1,209 @@
|
|||
/*
|
||||
* Copyright (C) OpenTX
|
||||
*
|
||||
* Based on code named
|
||||
* th9x - http://code.google.com/p/th9x
|
||||
* er9x - http://code.google.com/p/er9x
|
||||
* gruvin9x - http://code.google.com/p/gruvin9x
|
||||
*
|
||||
* License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
|
||||
*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License version 2 as
|
||||
* published by the Free Software Foundation.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*/
|
||||
|
||||
#include "opentx.h"
|
||||
|
||||
OS_TID menusTaskId;
|
||||
// menus stack must be aligned to 8 bytes otherwise printf for %f does not work!
|
||||
TaskStack<MENUS_STACK_SIZE> _ALIGNED(8) menusStack;
|
||||
|
||||
OS_TID mixerTaskId;
|
||||
TaskStack<MIXER_STACK_SIZE> mixerStack;
|
||||
|
||||
OS_TID audioTaskId;
|
||||
TaskStack<AUDIO_STACK_SIZE> audioStack;
|
||||
|
||||
#if defined(BLUETOOTH)
|
||||
OS_TID btTaskId;
|
||||
TaskStack<BLUETOOTH_STACK_SIZE> bluetoothStack;
|
||||
#endif
|
||||
|
||||
OS_MutexID audioMutex;
|
||||
OS_MutexID mixerMutex;
|
||||
|
||||
enum TaskIndex {
|
||||
MENU_TASK_INDEX,
|
||||
MIXER_TASK_INDEX,
|
||||
AUDIO_TASK_INDEX,
|
||||
CLI_TASK_INDEX,
|
||||
BLUETOOTH_TASK_INDEX,
|
||||
TASK_INDEX_COUNT,
|
||||
MAIN_TASK_INDEX = 255
|
||||
};
|
||||
|
||||
template<int SIZE>
|
||||
void TaskStack<SIZE>::paint()
|
||||
{
|
||||
for (uint32_t i=0; i<SIZE; i++) {
|
||||
stack[i] = 0x55555555;
|
||||
}
|
||||
}
|
||||
|
||||
uint16_t getStackAvailable(void * address, uint16_t size)
|
||||
{
|
||||
uint32_t * array = (uint32_t *)address;
|
||||
uint16_t i = 0;
|
||||
while (i < size && array[i] == 0x55555555) {
|
||||
i++;
|
||||
}
|
||||
return i*4;
|
||||
#if defined(CLI)
|
||||
cliStackPaint();
|
||||
#endif
|
||||
}
|
||||
|
||||
void stackPaint()
|
||||
{
|
||||
menusStack.paint();
|
||||
mixerStack.paint();
|
||||
audioStack.paint();
|
||||
#if defined(CLI)
|
||||
cliStack.paint();
|
||||
#endif
|
||||
}
|
||||
|
||||
#if defined(CPUSTM32) && !defined(SIMU)
|
||||
uint16_t stackSize()
|
||||
{
|
||||
return ((unsigned char *)&_estack - (unsigned char *)&_main_stack_start) / 4;
|
||||
}
|
||||
|
||||
uint16_t stackAvailable()
|
||||
{
|
||||
return getStackAvailable(&_main_stack_start, stackSize());
|
||||
}
|
||||
#endif
|
||||
|
||||
void mixerTask(void * pdata)
|
||||
{
|
||||
s_pulses_paused = true;
|
||||
|
||||
while(1) {
|
||||
|
||||
#if defined(SIMU)
|
||||
if (main_thread_running == 0)
|
||||
return;
|
||||
#endif
|
||||
|
||||
if (!s_pulses_paused) {
|
||||
uint16_t t0 = getTmr2MHz();
|
||||
|
||||
CoEnterMutexSection(mixerMutex);
|
||||
doMixerCalculations();
|
||||
CoLeaveMutexSection(mixerMutex);
|
||||
|
||||
#if defined(FRSKY) || defined(MAVLINK)
|
||||
telemetryWakeup();
|
||||
#endif
|
||||
|
||||
if (heartbeat == HEART_WDT_CHECK) {
|
||||
wdt_reset();
|
||||
heartbeat = 0;
|
||||
}
|
||||
|
||||
t0 = getTmr2MHz() - t0;
|
||||
if (t0 > maxMixerDuration) maxMixerDuration = t0 ;
|
||||
}
|
||||
|
||||
CoTickDelay(1); // 2ms for now
|
||||
}
|
||||
}
|
||||
|
||||
#define MENU_TASK_PERIOD_TICKS 10 // 20ms
|
||||
|
||||
void menusTask(void * pdata)
|
||||
{
|
||||
opentxInit();
|
||||
|
||||
#if defined(PWR_BUTTON_DELAY)
|
||||
while (1) {
|
||||
uint32_t pwr_check = pwrCheck();
|
||||
if (pwr_check == e_power_off) {
|
||||
break;
|
||||
}
|
||||
else if (pwr_check == e_power_press) {
|
||||
CoTickDelay(MENU_TASK_PERIOD_TICKS);
|
||||
continue;
|
||||
}
|
||||
#else
|
||||
while (pwrCheck() != e_power_off) {
|
||||
#endif
|
||||
U64 start = CoGetOSTime();
|
||||
perMain();
|
||||
// TODO remove completely massstorage from sky9x firmware
|
||||
U32 runtime = (U32)(CoGetOSTime() - start);
|
||||
// deduct the thread run-time from the wait, if run-time was more than
|
||||
// desired period, then skip the wait all together
|
||||
if (runtime < MENU_TASK_PERIOD_TICKS) {
|
||||
CoTickDelay(MENU_TASK_PERIOD_TICKS - runtime);
|
||||
}
|
||||
|
||||
#if defined(SIMU)
|
||||
if (main_thread_running == 0)
|
||||
return;
|
||||
#endif
|
||||
}
|
||||
|
||||
#if defined(PCBTARANIS) && defined(REV9E)
|
||||
toplcdOff();
|
||||
#endif
|
||||
|
||||
BACKLIGHT_OFF();
|
||||
|
||||
#if defined(PCBHORUS)
|
||||
ledOff();
|
||||
#endif
|
||||
|
||||
#if defined(COLORLCD) || defined(PCBTARANIS)
|
||||
drawSleepBitmap();
|
||||
#else
|
||||
lcdClear();
|
||||
displayPopup(STR_SHUTDOWN);
|
||||
#endif
|
||||
|
||||
opentxClose();
|
||||
boardOff(); // Only turn power off if necessary
|
||||
}
|
||||
|
||||
extern void audioTask(void* pdata);
|
||||
|
||||
void tasksStart()
|
||||
{
|
||||
CoInitOS();
|
||||
|
||||
#if defined(CLI)
|
||||
cliStart();
|
||||
#endif
|
||||
|
||||
#if defined(BLUETOOTH)
|
||||
btTaskId = CoCreateTask(btTask, NULL, 15, &bluetoothStack.stack[BLUETOOTH_STACK_SIZE-1], BLUETOOTH_STACK_SIZE);
|
||||
#endif
|
||||
|
||||
mixerTaskId = CoCreateTask(mixerTask, NULL, 5, &mixerStack.stack[MIXER_STACK_SIZE-1], MIXER_STACK_SIZE);
|
||||
menusTaskId = CoCreateTask(menusTask, NULL, 10, &menusStack.stack[MENUS_STACK_SIZE-1], MENUS_STACK_SIZE);
|
||||
#if !defined(SIMU)
|
||||
// TODO move the SIMU audio in this task
|
||||
audioTaskId = CoCreateTask(audioTask, NULL, 7, &audioStack.stack[AUDIO_STACK_SIZE-1], AUDIO_STACK_SIZE);
|
||||
#endif
|
||||
audioMutex = CoCreateMutex();
|
||||
mixerMutex = CoCreateMutex();
|
||||
|
||||
CoStartOS();
|
||||
}
|
||||
/*
|
||||
* Copyright (C) OpenTX
|
||||
*
|
||||
* Based on code named
|
||||
* th9x - http://code.google.com/p/th9x
|
||||
* er9x - http://code.google.com/p/er9x
|
||||
* gruvin9x - http://code.google.com/p/gruvin9x
|
||||
*
|
||||
* License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
|
||||
*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License version 2 as
|
||||
* published by the Free Software Foundation.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*/
|
||||
|
||||
#include "opentx.h"
|
||||
|
||||
OS_TID menusTaskId;
|
||||
// menus stack must be aligned to 8 bytes otherwise printf for %f does not work!
|
||||
TaskStack<MENUS_STACK_SIZE> _ALIGNED(8) menusStack;
|
||||
|
||||
OS_TID mixerTaskId;
|
||||
TaskStack<MIXER_STACK_SIZE> mixerStack;
|
||||
|
||||
OS_TID audioTaskId;
|
||||
TaskStack<AUDIO_STACK_SIZE> audioStack;
|
||||
|
||||
#if defined(BLUETOOTH)
|
||||
OS_TID btTaskId;
|
||||
TaskStack<BLUETOOTH_STACK_SIZE> bluetoothStack;
|
||||
#endif
|
||||
|
||||
OS_MutexID audioMutex;
|
||||
OS_MutexID mixerMutex;
|
||||
|
||||
enum TaskIndex {
|
||||
MENU_TASK_INDEX,
|
||||
MIXER_TASK_INDEX,
|
||||
AUDIO_TASK_INDEX,
|
||||
CLI_TASK_INDEX,
|
||||
BLUETOOTH_TASK_INDEX,
|
||||
TASK_INDEX_COUNT,
|
||||
MAIN_TASK_INDEX = 255
|
||||
};
|
||||
|
||||
template<int SIZE>
|
||||
void TaskStack<SIZE>::paint()
|
||||
{
|
||||
for (uint32_t i=0; i<SIZE; i++) {
|
||||
stack[i] = 0x55555555;
|
||||
}
|
||||
}
|
||||
|
||||
uint16_t getStackAvailable(void * address, uint16_t size)
|
||||
{
|
||||
uint32_t * array = (uint32_t *)address;
|
||||
uint16_t i = 0;
|
||||
while (i < size && array[i] == 0x55555555) {
|
||||
i++;
|
||||
}
|
||||
return i*4;
|
||||
#if defined(CLI)
|
||||
cliStackPaint();
|
||||
#endif
|
||||
}
|
||||
|
||||
void stackPaint()
|
||||
{
|
||||
menusStack.paint();
|
||||
mixerStack.paint();
|
||||
audioStack.paint();
|
||||
#if defined(CLI)
|
||||
cliStack.paint();
|
||||
#endif
|
||||
}
|
||||
|
||||
#if defined(CPUSTM32) && !defined(SIMU)
|
||||
uint16_t stackSize()
|
||||
{
|
||||
return ((unsigned char *)&_estack - (unsigned char *)&_main_stack_start) / 4;
|
||||
}
|
||||
|
||||
uint16_t stackAvailable()
|
||||
{
|
||||
return getStackAvailable(&_main_stack_start, stackSize());
|
||||
}
|
||||
#endif
|
||||
|
||||
void mixerTask(void * pdata)
|
||||
{
|
||||
s_pulses_paused = true;
|
||||
|
||||
while(1) {
|
||||
|
||||
#if defined(SIMU)
|
||||
if (main_thread_running == 0)
|
||||
return;
|
||||
#endif
|
||||
|
||||
if (!s_pulses_paused) {
|
||||
uint16_t t0 = getTmr2MHz();
|
||||
|
||||
CoEnterMutexSection(mixerMutex);
|
||||
doMixerCalculations();
|
||||
CoLeaveMutexSection(mixerMutex);
|
||||
|
||||
#if defined(FRSKY) || defined(MAVLINK)
|
||||
telemetryWakeup();
|
||||
#endif
|
||||
|
||||
if (heartbeat == HEART_WDT_CHECK) {
|
||||
wdt_reset();
|
||||
heartbeat = 0;
|
||||
}
|
||||
|
||||
t0 = getTmr2MHz() - t0;
|
||||
if (t0 > maxMixerDuration) maxMixerDuration = t0 ;
|
||||
}
|
||||
|
||||
CoTickDelay(1); // 2ms for now
|
||||
}
|
||||
}
|
||||
|
||||
#define MENU_TASK_PERIOD_TICKS 10 // 20ms
|
||||
|
||||
void menusTask(void * pdata)
|
||||
{
|
||||
opentxInit();
|
||||
|
||||
#if defined(PWR_BUTTON_DELAY)
|
||||
while (1) {
|
||||
uint32_t pwr_check = pwrCheck();
|
||||
if (pwr_check == e_power_off) {
|
||||
break;
|
||||
}
|
||||
else if (pwr_check == e_power_press) {
|
||||
CoTickDelay(MENU_TASK_PERIOD_TICKS);
|
||||
continue;
|
||||
}
|
||||
#else
|
||||
while (pwrCheck() != e_power_off) {
|
||||
#endif
|
||||
U64 start = CoGetOSTime();
|
||||
perMain();
|
||||
// TODO remove completely massstorage from sky9x firmware
|
||||
U32 runtime = (U32)(CoGetOSTime() - start);
|
||||
// deduct the thread run-time from the wait, if run-time was more than
|
||||
// desired period, then skip the wait all together
|
||||
if (runtime < MENU_TASK_PERIOD_TICKS) {
|
||||
CoTickDelay(MENU_TASK_PERIOD_TICKS - runtime);
|
||||
}
|
||||
|
||||
#if defined(SIMU)
|
||||
if (main_thread_running == 0)
|
||||
return;
|
||||
#endif
|
||||
}
|
||||
|
||||
#if defined(PCBTARANIS) && defined(REV9E)
|
||||
toplcdOff();
|
||||
#endif
|
||||
|
||||
BACKLIGHT_OFF();
|
||||
|
||||
#if defined(PCBHORUS)
|
||||
ledOff();
|
||||
#endif
|
||||
|
||||
#if defined(COLORLCD) || defined(PCBTARANIS)
|
||||
drawSleepBitmap();
|
||||
#else
|
||||
lcdClear();
|
||||
displayPopup(STR_SHUTDOWN);
|
||||
#endif
|
||||
|
||||
opentxClose();
|
||||
boardOff(); // Only turn power off if necessary
|
||||
}
|
||||
|
||||
extern void audioTask(void* pdata);
|
||||
|
||||
void tasksStart()
|
||||
{
|
||||
CoInitOS();
|
||||
|
||||
#if defined(CLI)
|
||||
cliStart();
|
||||
#endif
|
||||
|
||||
#if defined(BLUETOOTH)
|
||||
btTaskId = CoCreateTask(btTask, NULL, 15, &bluetoothStack.stack[BLUETOOTH_STACK_SIZE-1], BLUETOOTH_STACK_SIZE);
|
||||
#endif
|
||||
|
||||
mixerTaskId = CoCreateTask(mixerTask, NULL, 5, &mixerStack.stack[MIXER_STACK_SIZE-1], MIXER_STACK_SIZE);
|
||||
menusTaskId = CoCreateTask(menusTask, NULL, 10, &menusStack.stack[MENUS_STACK_SIZE-1], MENUS_STACK_SIZE);
|
||||
#if !defined(SIMU)
|
||||
// TODO move the SIMU audio in this task
|
||||
audioTaskId = CoCreateTask(audioTask, NULL, 7, &audioStack.stack[AUDIO_STACK_SIZE-1], AUDIO_STACK_SIZE);
|
||||
#endif
|
||||
audioMutex = CoCreateMutex();
|
||||
mixerMutex = CoCreateMutex();
|
||||
|
||||
CoStartOS();
|
||||
}
|
||||
|
|
|
@ -18,241 +18,241 @@
|
|||
* GNU General Public License for more details.
|
||||
*/
|
||||
|
||||
/*
|
||||
*
|
||||
* ============================================================
|
||||
* Templates file
|
||||
*
|
||||
* eccpm
|
||||
* crow
|
||||
* throttle cut
|
||||
* flaperon
|
||||
* elevon
|
||||
* v-tail
|
||||
* throttle hold
|
||||
* Aileron Differential
|
||||
* Spoilers
|
||||
* Snap Roll
|
||||
* ELE->Flap
|
||||
* Flap->ELE
|
||||
*
|
||||
*/
|
||||
|
||||
#include "opentx.h"
|
||||
|
||||
#if defined(PCBTARANIS)
|
||||
#pragma message("Templates with virtual inputs (FrSky Taranis) are not implemented!")
|
||||
#endif
|
||||
|
||||
MixData* setDest(uint8_t dch, uint8_t src, bool clear=false)
|
||||
{
|
||||
uint8_t i = 0;
|
||||
MixData * mix;
|
||||
|
||||
while (1) {
|
||||
mix = mixAddress(i);
|
||||
if (mix->srcRaw && mix->destCh <= dch) {
|
||||
if (clear && mix->destCh == dch) {
|
||||
deleteExpoMix(0, i);
|
||||
}
|
||||
else {
|
||||
if (++i==MAX_MIXERS) {
|
||||
// TODO should return null pointer but needs to be tested then
|
||||
mix = mixAddress(0);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
else {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
memmove(mix+1, mix, (MAX_MIXERS-(i+1))*sizeof(MixData) );
|
||||
memclear(mix, sizeof(MixData));
|
||||
mix->destCh = dch;
|
||||
mix->srcRaw = src;
|
||||
mix->weight = 100;
|
||||
return mix;
|
||||
}
|
||||
|
||||
void mixSetWeight(MixData* md, int8_t weight)
|
||||
{
|
||||
u_int8int16_t tmp;
|
||||
tmp.word=weight;
|
||||
MD_UNION_TO_WEIGHT(tmp,md);
|
||||
// MD_SETWEIGHT(md,weight); doesn't matter here in code cost compiler optimizes this anyway
|
||||
}
|
||||
|
||||
#if defined(PCBTARANIS)
|
||||
#define TMPL_INPUT(x) (MIXSRC_FIRST_INPUT+channel_order(x)-1)
|
||||
#else
|
||||
#define clearInputs()
|
||||
#define defaultInputs()
|
||||
#define TMPL_INPUT(x) (MIXSRC_Rud+x-1)
|
||||
#endif
|
||||
|
||||
void clearMixes()
|
||||
{
|
||||
memset(g_model.mixData, 0, sizeof(g_model.mixData)); // clear all mixes
|
||||
}
|
||||
|
||||
void clearCurves()
|
||||
{
|
||||
memclear(g_model.curves, sizeof(g_model.curves) + sizeof(g_model.points)); // clear all curves
|
||||
}
|
||||
|
||||
void setCurve(uint8_t c, const pm_int8_t ar[])
|
||||
{
|
||||
int8_t * cv = curveAddress(c);
|
||||
for (uint8_t i=0; i<5; i++) {
|
||||
cv[i] = pgm_read_byte(&ar[i]);
|
||||
}
|
||||
}
|
||||
|
||||
void setLogicalSwitch(uint8_t idx, uint8_t func, int8_t v1, int8_t v2)
|
||||
{
|
||||
LogicalSwitchData *cs = lswAddress(idx-1);
|
||||
cs->func = func;
|
||||
cs->v1 = v1;
|
||||
cs->v2 = v2;
|
||||
}
|
||||
|
||||
const pm_int8_t heli_ar1[] PROGMEM = {-100, 20, 30, 70, 90};
|
||||
const pm_int8_t heli_ar2[] PROGMEM = {80, 70, 60, 70, 100};
|
||||
const pm_int8_t heli_ar3[] PROGMEM = {100, 90, 80, 90, 100};
|
||||
const pm_int8_t heli_ar4[] PROGMEM = {-30, -15, 0, 50, 100};
|
||||
const pm_int8_t heli_ar5[] PROGMEM = {-100, -50, 0, 50, 100};
|
||||
|
||||
void applyTemplate(uint8_t idx)
|
||||
{
|
||||
MixData *md;
|
||||
|
||||
//CC(STK) -> vSTK
|
||||
//ICC(vSTK) -> STK
|
||||
#define ICC(x) icc[(x)-1]
|
||||
uint8_t icc[4] = {0};
|
||||
for (uint8_t i=0; i<4; i++) { //generate inverse array
|
||||
for(uint8_t j=0; j<4; j++)
|
||||
if(CC(i+1)==j+MIXSRC_Rud) icc[j]=i;
|
||||
}
|
||||
|
||||
switch (idx) {
|
||||
case TMPL_CLEAR_MIXES:
|
||||
case TMPL_SIMPLE_4CH:
|
||||
case TMPL_HELI_SETUP:
|
||||
clearMixes();
|
||||
break;
|
||||
}
|
||||
|
||||
switch (idx) {
|
||||
// Simple 4-Ch
|
||||
case TMPL_SIMPLE_4CH:
|
||||
defaultInputs();
|
||||
setDest(ICC(STK_RUD), TMPL_INPUT(STK_RUD));
|
||||
setDest(ICC(STK_ELE), TMPL_INPUT(STK_ELE));
|
||||
setDest(ICC(STK_THR), TMPL_INPUT(STK_THR));
|
||||
setDest(ICC(STK_AIL), TMPL_INPUT(STK_AIL));
|
||||
break;
|
||||
|
||||
// Sticky-T-Cut
|
||||
case TMPL_STI_THR_CUT:
|
||||
md=setDest(ICC(STK_THR), MIXSRC_MAX); mixSetWeight(md, -100); md->swtch=SWSRC_SWC; md->mltpx=MLTPX_REP;
|
||||
md=setDest(13, MIXSRC_CH14); // md->weight= 100; done by setDest anyway
|
||||
md=setDest(13, MIXSRC_MAX); mixSetWeight(md, -100); md->swtch=SWSRC_SWB; md->mltpx=MLTPX_REP;
|
||||
md=setDest(13, MIXSRC_MAX); /* md->weight= 100;*/ md->swtch=SWSRC_THR; md->mltpx=MLTPX_REP;
|
||||
setLogicalSwitch(11, LS_FUNC_VNEG, STK_THR, -99);
|
||||
setLogicalSwitch(12, LS_FUNC_VPOS, MIXSRC_CH14, 0);
|
||||
break;
|
||||
|
||||
// V-Tail
|
||||
case TMPL_V_TAIL:
|
||||
defaultInputs();
|
||||
setDest(ICC(STK_RUD), TMPL_INPUT(STK_RUD), true);
|
||||
md=setDest(ICC(STK_RUD), TMPL_INPUT(STK_ELE)); mixSetWeight(md, -100);
|
||||
setDest(ICC(STK_ELE), TMPL_INPUT(STK_RUD), true);
|
||||
setDest(ICC(STK_ELE), TMPL_INPUT(STK_ELE));
|
||||
break;
|
||||
|
||||
// Elevon\\Delta
|
||||
case TMPL_ELEVON_DELTA:
|
||||
defaultInputs();
|
||||
setDest(ICC(STK_ELE), MIXSRC_Ele, true);
|
||||
setDest(ICC(STK_ELE), MIXSRC_Ail);
|
||||
setDest(ICC(STK_AIL), MIXSRC_Ele, true);
|
||||
md=setDest(ICC(STK_AIL), MIXSRC_Ail); mixSetWeight(md, -100);
|
||||
break;
|
||||
|
||||
// eCCPM
|
||||
case TMPL_ECCPM:
|
||||
md=setDest(ICC(STK_ELE), MIXSRC_Ele, true); md->weight= 72;
|
||||
md=setDest(ICC(STK_ELE), MIXSRC_Thr); md->weight= 55;
|
||||
md=setDest(ICC(STK_AIL), MIXSRC_Ele, true); mixSetWeight(md, -36);
|
||||
md=setDest(ICC(STK_AIL), MIXSRC_Ail); md->weight= 62;
|
||||
md=setDest(ICC(STK_AIL), MIXSRC_Thr); md->weight= 55;
|
||||
md=setDest(5, MIXSRC_Ele, true); mixSetWeight(md, -36);
|
||||
md=setDest(5, MIXSRC_Ail); mixSetWeight(md, -62);
|
||||
md=setDest(5, MIXSRC_Thr); md->weight= 55;
|
||||
break;
|
||||
|
||||
// Heli Setup
|
||||
case TMPL_HELI_SETUP:
|
||||
clearCurves();
|
||||
|
||||
//Set up Mixes
|
||||
// 3 cyclic channels
|
||||
md=setDest(0, MIXSRC_CYC1); // md->weight=100;
|
||||
md=setDest(1, MIXSRC_CYC2); // md->weight=100;
|
||||
md=setDest(2, MIXSRC_CYC3); // md->weight=100;
|
||||
|
||||
// rudder
|
||||
md=setDest(3, MIXSRC_Rud); // md->weight=100;
|
||||
|
||||
// throttle
|
||||
#if defined(PCBTARANIS)
|
||||
// TODO
|
||||
#else
|
||||
md=setDest(4, MIXSRC_Thr); md->swtch=SWSRC_ID0; mixSetCurve(md, 0); md->carryTrim=TRIM_OFF;
|
||||
md=setDest(4, MIXSRC_Thr); md->swtch=SWSRC_ID1; mixSetCurve(md, 1); md->carryTrim=TRIM_OFF;
|
||||
md=setDest(4, MIXSRC_Thr); md->swtch=SWSRC_ID2; mixSetCurve(md, 2); md->carryTrim=TRIM_OFF;
|
||||
#endif
|
||||
md=setDest(4, MIXSRC_MAX); mixSetWeight(md, -100); md->swtch=SWSRC_THR; md->mltpx=MLTPX_REP;
|
||||
|
||||
// gyro gain
|
||||
md=setDest(5, MIXSRC_MAX); md->weight= 30; md->swtch=-SWSRC_GEA;
|
||||
md=setDest(5, MIXSRC_MAX); mixSetWeight(md, -30); md->swtch= SWSRC_GEA;
|
||||
|
||||
// collective
|
||||
#if defined(PCBTARANIS)
|
||||
// TODO
|
||||
#else
|
||||
md=setDest(10, MIXSRC_Thr); /*md->weight= 100;*/ md->swtch=SWSRC_ID0; mixSetCurve(md, 3); md->carryTrim=TRIM_OFF;
|
||||
md=setDest(10, MIXSRC_Thr); /*md->weight= 100;*/ md->swtch=SWSRC_ID1; mixSetCurve(md, 4); md->carryTrim=TRIM_OFF;
|
||||
md=setDest(10, MIXSRC_Thr); /*md->weight= 100;*/ md->swtch=SWSRC_ID2; mixSetCurve(md, 5); md->carryTrim=TRIM_OFF;
|
||||
#endif
|
||||
|
||||
g_model.swashR.collectiveSource = MIXSRC_CH11;
|
||||
g_model.swashR.type = SWASH_TYPE_120;
|
||||
|
||||
// curves
|
||||
setCurve(0, heli_ar1);
|
||||
setCurve(1, heli_ar2);
|
||||
setCurve(2, heli_ar3);
|
||||
setCurve(3, heli_ar4);
|
||||
setCurve(4, heli_ar5);
|
||||
setCurve(5, heli_ar5);
|
||||
break;
|
||||
|
||||
// Servo Test
|
||||
case TMPL_SERVO_TEST:
|
||||
md=setDest(NUM_CHNOUT-1, MIXSRC_SW1, true); md->weight=110; md->mltpx=MLTPX_ADD; md->delayUp = 6; md->delayDown = 6; md->speedUp = 8; md->speedDown = 8;
|
||||
setLogicalSwitch(1, LS_FUNC_VNEG, MIXSRC_LAST_CH, 0);
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
|
||||
}
|
||||
|
||||
storageDirty(EE_MODEL);
|
||||
}
|
||||
/*
|
||||
*
|
||||
* ============================================================
|
||||
* Templates file
|
||||
*
|
||||
* eccpm
|
||||
* crow
|
||||
* throttle cut
|
||||
* flaperon
|
||||
* elevon
|
||||
* v-tail
|
||||
* throttle hold
|
||||
* Aileron Differential
|
||||
* Spoilers
|
||||
* Snap Roll
|
||||
* ELE->Flap
|
||||
* Flap->ELE
|
||||
*
|
||||
*/
|
||||
|
||||
#include "opentx.h"
|
||||
|
||||
#if defined(PCBTARANIS)
|
||||
#pragma message("Templates with virtual inputs (FrSky Taranis) are not implemented!")
|
||||
#endif
|
||||
|
||||
MixData* setDest(uint8_t dch, uint8_t src, bool clear=false)
|
||||
{
|
||||
uint8_t i = 0;
|
||||
MixData * mix;
|
||||
|
||||
while (1) {
|
||||
mix = mixAddress(i);
|
||||
if (mix->srcRaw && mix->destCh <= dch) {
|
||||
if (clear && mix->destCh == dch) {
|
||||
deleteExpoMix(0, i);
|
||||
}
|
||||
else {
|
||||
if (++i==MAX_MIXERS) {
|
||||
// TODO should return null pointer but needs to be tested then
|
||||
mix = mixAddress(0);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
else {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
memmove(mix+1, mix, (MAX_MIXERS-(i+1))*sizeof(MixData) );
|
||||
memclear(mix, sizeof(MixData));
|
||||
mix->destCh = dch;
|
||||
mix->srcRaw = src;
|
||||
mix->weight = 100;
|
||||
return mix;
|
||||
}
|
||||
|
||||
void mixSetWeight(MixData* md, int8_t weight)
|
||||
{
|
||||
u_int8int16_t tmp;
|
||||
tmp.word=weight;
|
||||
MD_UNION_TO_WEIGHT(tmp,md);
|
||||
// MD_SETWEIGHT(md,weight); doesn't matter here in code cost compiler optimizes this anyway
|
||||
}
|
||||
|
||||
#if defined(PCBTARANIS)
|
||||
#define TMPL_INPUT(x) (MIXSRC_FIRST_INPUT+channel_order(x)-1)
|
||||
#else
|
||||
#define clearInputs()
|
||||
#define defaultInputs()
|
||||
#define TMPL_INPUT(x) (MIXSRC_Rud+x-1)
|
||||
#endif
|
||||
|
||||
void clearMixes()
|
||||
{
|
||||
memset(g_model.mixData, 0, sizeof(g_model.mixData)); // clear all mixes
|
||||
}
|
||||
|
||||
void clearCurves()
|
||||
{
|
||||
memclear(g_model.curves, sizeof(g_model.curves) + sizeof(g_model.points)); // clear all curves
|
||||
}
|
||||
|
||||
void setCurve(uint8_t c, const pm_int8_t ar[])
|
||||
{
|
||||
int8_t * cv = curveAddress(c);
|
||||
for (uint8_t i=0; i<5; i++) {
|
||||
cv[i] = pgm_read_byte(&ar[i]);
|
||||
}
|
||||
}
|
||||
|
||||
void setLogicalSwitch(uint8_t idx, uint8_t func, int8_t v1, int8_t v2)
|
||||
{
|
||||
LogicalSwitchData *cs = lswAddress(idx-1);
|
||||
cs->func = func;
|
||||
cs->v1 = v1;
|
||||
cs->v2 = v2;
|
||||
}
|
||||
|
||||
const pm_int8_t heli_ar1[] PROGMEM = {-100, 20, 30, 70, 90};
|
||||
const pm_int8_t heli_ar2[] PROGMEM = {80, 70, 60, 70, 100};
|
||||
const pm_int8_t heli_ar3[] PROGMEM = {100, 90, 80, 90, 100};
|
||||
const pm_int8_t heli_ar4[] PROGMEM = {-30, -15, 0, 50, 100};
|
||||
const pm_int8_t heli_ar5[] PROGMEM = {-100, -50, 0, 50, 100};
|
||||
|
||||
void applyTemplate(uint8_t idx)
|
||||
{
|
||||
MixData *md;
|
||||
|
||||
//CC(STK) -> vSTK
|
||||
//ICC(vSTK) -> STK
|
||||
#define ICC(x) icc[(x)-1]
|
||||
uint8_t icc[4] = {0};
|
||||
for (uint8_t i=0; i<4; i++) { //generate inverse array
|
||||
for(uint8_t j=0; j<4; j++)
|
||||
if(CC(i+1)==j+MIXSRC_Rud) icc[j]=i;
|
||||
}
|
||||
|
||||
switch (idx) {
|
||||
case TMPL_CLEAR_MIXES:
|
||||
case TMPL_SIMPLE_4CH:
|
||||
case TMPL_HELI_SETUP:
|
||||
clearMixes();
|
||||
break;
|
||||
}
|
||||
|
||||
switch (idx) {
|
||||
// Simple 4-Ch
|
||||
case TMPL_SIMPLE_4CH:
|
||||
defaultInputs();
|
||||
setDest(ICC(STK_RUD), TMPL_INPUT(STK_RUD));
|
||||
setDest(ICC(STK_ELE), TMPL_INPUT(STK_ELE));
|
||||
setDest(ICC(STK_THR), TMPL_INPUT(STK_THR));
|
||||
setDest(ICC(STK_AIL), TMPL_INPUT(STK_AIL));
|
||||
break;
|
||||
|
||||
// Sticky-T-Cut
|
||||
case TMPL_STI_THR_CUT:
|
||||
md=setDest(ICC(STK_THR), MIXSRC_MAX); mixSetWeight(md, -100); md->swtch=SWSRC_SWC; md->mltpx=MLTPX_REP;
|
||||
md=setDest(13, MIXSRC_CH14); // md->weight= 100; done by setDest anyway
|
||||
md=setDest(13, MIXSRC_MAX); mixSetWeight(md, -100); md->swtch=SWSRC_SWB; md->mltpx=MLTPX_REP;
|
||||
md=setDest(13, MIXSRC_MAX); /* md->weight= 100;*/ md->swtch=SWSRC_THR; md->mltpx=MLTPX_REP;
|
||||
setLogicalSwitch(11, LS_FUNC_VNEG, STK_THR, -99);
|
||||
setLogicalSwitch(12, LS_FUNC_VPOS, MIXSRC_CH14, 0);
|
||||
break;
|
||||
|
||||
// V-Tail
|
||||
case TMPL_V_TAIL:
|
||||
defaultInputs();
|
||||
setDest(ICC(STK_RUD), TMPL_INPUT(STK_RUD), true);
|
||||
md=setDest(ICC(STK_RUD), TMPL_INPUT(STK_ELE)); mixSetWeight(md, -100);
|
||||
setDest(ICC(STK_ELE), TMPL_INPUT(STK_RUD), true);
|
||||
setDest(ICC(STK_ELE), TMPL_INPUT(STK_ELE));
|
||||
break;
|
||||
|
||||
// Elevon\\Delta
|
||||
case TMPL_ELEVON_DELTA:
|
||||
defaultInputs();
|
||||
setDest(ICC(STK_ELE), MIXSRC_Ele, true);
|
||||
setDest(ICC(STK_ELE), MIXSRC_Ail);
|
||||
setDest(ICC(STK_AIL), MIXSRC_Ele, true);
|
||||
md=setDest(ICC(STK_AIL), MIXSRC_Ail); mixSetWeight(md, -100);
|
||||
break;
|
||||
|
||||
// eCCPM
|
||||
case TMPL_ECCPM:
|
||||
md=setDest(ICC(STK_ELE), MIXSRC_Ele, true); md->weight= 72;
|
||||
md=setDest(ICC(STK_ELE), MIXSRC_Thr); md->weight= 55;
|
||||
md=setDest(ICC(STK_AIL), MIXSRC_Ele, true); mixSetWeight(md, -36);
|
||||
md=setDest(ICC(STK_AIL), MIXSRC_Ail); md->weight= 62;
|
||||
md=setDest(ICC(STK_AIL), MIXSRC_Thr); md->weight= 55;
|
||||
md=setDest(5, MIXSRC_Ele, true); mixSetWeight(md, -36);
|
||||
md=setDest(5, MIXSRC_Ail); mixSetWeight(md, -62);
|
||||
md=setDest(5, MIXSRC_Thr); md->weight= 55;
|
||||
break;
|
||||
|
||||
// Heli Setup
|
||||
case TMPL_HELI_SETUP:
|
||||
clearCurves();
|
||||
|
||||
//Set up Mixes
|
||||
// 3 cyclic channels
|
||||
md=setDest(0, MIXSRC_CYC1); // md->weight=100;
|
||||
md=setDest(1, MIXSRC_CYC2); // md->weight=100;
|
||||
md=setDest(2, MIXSRC_CYC3); // md->weight=100;
|
||||
|
||||
// rudder
|
||||
md=setDest(3, MIXSRC_Rud); // md->weight=100;
|
||||
|
||||
// throttle
|
||||
#if defined(PCBTARANIS)
|
||||
// TODO
|
||||
#else
|
||||
md=setDest(4, MIXSRC_Thr); md->swtch=SWSRC_ID0; mixSetCurve(md, 0); md->carryTrim=TRIM_OFF;
|
||||
md=setDest(4, MIXSRC_Thr); md->swtch=SWSRC_ID1; mixSetCurve(md, 1); md->carryTrim=TRIM_OFF;
|
||||
md=setDest(4, MIXSRC_Thr); md->swtch=SWSRC_ID2; mixSetCurve(md, 2); md->carryTrim=TRIM_OFF;
|
||||
#endif
|
||||
md=setDest(4, MIXSRC_MAX); mixSetWeight(md, -100); md->swtch=SWSRC_THR; md->mltpx=MLTPX_REP;
|
||||
|
||||
// gyro gain
|
||||
md=setDest(5, MIXSRC_MAX); md->weight= 30; md->swtch=-SWSRC_GEA;
|
||||
md=setDest(5, MIXSRC_MAX); mixSetWeight(md, -30); md->swtch= SWSRC_GEA;
|
||||
|
||||
// collective
|
||||
#if defined(PCBTARANIS)
|
||||
// TODO
|
||||
#else
|
||||
md=setDest(10, MIXSRC_Thr); /*md->weight= 100;*/ md->swtch=SWSRC_ID0; mixSetCurve(md, 3); md->carryTrim=TRIM_OFF;
|
||||
md=setDest(10, MIXSRC_Thr); /*md->weight= 100;*/ md->swtch=SWSRC_ID1; mixSetCurve(md, 4); md->carryTrim=TRIM_OFF;
|
||||
md=setDest(10, MIXSRC_Thr); /*md->weight= 100;*/ md->swtch=SWSRC_ID2; mixSetCurve(md, 5); md->carryTrim=TRIM_OFF;
|
||||
#endif
|
||||
|
||||
g_model.swashR.collectiveSource = MIXSRC_CH11;
|
||||
g_model.swashR.type = SWASH_TYPE_120;
|
||||
|
||||
// curves
|
||||
setCurve(0, heli_ar1);
|
||||
setCurve(1, heli_ar2);
|
||||
setCurve(2, heli_ar3);
|
||||
setCurve(3, heli_ar4);
|
||||
setCurve(4, heli_ar5);
|
||||
setCurve(5, heli_ar5);
|
||||
break;
|
||||
|
||||
// Servo Test
|
||||
case TMPL_SERVO_TEST:
|
||||
md=setDest(NUM_CHNOUT-1, MIXSRC_SW1, true); md->weight=110; md->mltpx=MLTPX_ADD; md->delayUp = 6; md->delayDown = 6; md->speedUp = 8; md->speedDown = 8;
|
||||
setLogicalSwitch(1, LS_FUNC_VNEG, MIXSRC_LAST_CH, 0);
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
|
||||
}
|
||||
|
||||
storageDirty(EE_MODEL);
|
||||
}
|
||||
|
|
File diff suppressed because it is too large
Load diff
|
@ -18,139 +18,139 @@
|
|||
* GNU General Public License for more details.
|
||||
*/
|
||||
|
||||
#include "opentx.h"
|
||||
|
||||
|
||||
#if defined(CPUARM)
|
||||
|
||||
void varioWakeup()
|
||||
{
|
||||
if (isFunctionActive(FUNCTION_VARIO)) {
|
||||
int varioFreq, varioDuration, varioPause=0;
|
||||
uint8_t varioFlags;
|
||||
|
||||
int verticalSpeed = 0;
|
||||
if (g_model.frsky.varioSource) {
|
||||
uint8_t item = g_model.frsky.varioSource-1;
|
||||
if (item < MAX_SENSORS) {
|
||||
verticalSpeed = telemetryItems[item].value * g_model.telemetrySensors[item].getPrecMultiplier();
|
||||
}
|
||||
}
|
||||
|
||||
int varioCenterMin = (int)g_model.frsky.varioCenterMin * 10 - 50;
|
||||
int varioCenterMax = (int)g_model.frsky.varioCenterMax * 10 + 50;
|
||||
int varioMax = (10+(int)g_model.frsky.varioMax) * 100;
|
||||
int varioMin = (-10+(int)g_model.frsky.varioMin) * 100;
|
||||
|
||||
if (verticalSpeed > varioMax)
|
||||
verticalSpeed = varioMax;
|
||||
else if (verticalSpeed < varioMin)
|
||||
verticalSpeed = varioMin;
|
||||
|
||||
if (verticalSpeed <= varioCenterMin) {
|
||||
varioFreq = VARIO_FREQUENCY_ZERO + (g_eeGeneral.varioPitch*10) - (((VARIO_FREQUENCY_ZERO+(g_eeGeneral.varioPitch*10)-((VARIO_FREQUENCY_ZERO + (g_eeGeneral.varioPitch*10))/2)) * (verticalSpeed-varioCenterMin)) / varioMin);
|
||||
varioDuration = 80; // continuous beep: we will enter again here before the tone ends
|
||||
varioFlags = PLAY_BACKGROUND|PLAY_NOW;
|
||||
}
|
||||
else if (verticalSpeed >= varioCenterMax || !g_model.frsky.varioCenterSilent) {
|
||||
varioFreq = VARIO_FREQUENCY_ZERO + (g_eeGeneral.varioPitch*10) + (((VARIO_FREQUENCY_RANGE+(g_eeGeneral.varioRange*10)) * (verticalSpeed-varioCenterMin)) / varioMax);
|
||||
int varioPeriod = VARIO_REPEAT_MAX + ((VARIO_REPEAT_ZERO+(g_eeGeneral.varioRepeat*10)-VARIO_REPEAT_MAX) * (varioMax-verticalSpeed) * (varioMax-verticalSpeed)) / ((varioMax-varioCenterMin) * (varioMax-varioCenterMin));
|
||||
if (verticalSpeed >= varioCenterMax || varioCenterMin == varioCenterMax)
|
||||
varioDuration = varioPeriod / 5;
|
||||
else
|
||||
varioDuration = varioPeriod * (85 - (((verticalSpeed-varioCenterMin) * 25) / (varioCenterMax-varioCenterMin))) / 100;
|
||||
varioPause = varioPeriod - varioDuration;
|
||||
varioFlags = PLAY_BACKGROUND;
|
||||
}
|
||||
else {
|
||||
return;
|
||||
}
|
||||
|
||||
AUDIO_VARIO(varioFreq, varioDuration, varioPause, varioFlags);
|
||||
}
|
||||
}
|
||||
|
||||
#elif defined(FRSKY)
|
||||
|
||||
void varioWakeup()
|
||||
{
|
||||
static tmr10ms_t s_varioTmr;
|
||||
tmr10ms_t tmr10ms = get_tmr10ms();
|
||||
|
||||
if (isFunctionActive(FUNCTION_VARIO)) {
|
||||
#if defined(AUDIO)
|
||||
cli();
|
||||
int16_t verticalSpeed = frskyData.hub.varioSpeed;
|
||||
sei();
|
||||
|
||||
#if defined(PCBSTD)
|
||||
int16_t varioCenterMax = (int16_t)g_model.frsky.varioCenterMax * 10 + 50;
|
||||
if (verticalSpeed >= varioCenterMax) {
|
||||
verticalSpeed = verticalSpeed - varioCenterMax;
|
||||
int16_t varioMax = (10+(int16_t)g_model.frsky.varioMax) * 100;
|
||||
if (verticalSpeed > varioMax) verticalSpeed = varioMax;
|
||||
verticalSpeed = (verticalSpeed * 10) / ((varioMax-varioCenterMax) / 100);
|
||||
|
||||
if ((int16_t)(s_varioTmr-tmr10ms) < 0) {
|
||||
uint8_t varioFreq = (verticalSpeed * 10 + 16000) >> 8;
|
||||
uint8_t varioDuration = (1600 - verticalSpeed) / 100;
|
||||
s_varioTmr = tmr10ms + (varioDuration*2);
|
||||
AUDIO_VARIO(varioFreq, varioDuration);
|
||||
}
|
||||
}
|
||||
#else
|
||||
int varioCenterMin = (int)g_model.frsky.varioCenterMin * 10 - 50;
|
||||
int varioCenterMax = (int)g_model.frsky.varioCenterMax * 10 + 50;
|
||||
int varioMax = (10+(int)g_model.frsky.varioMax) * 100;
|
||||
int varioMin = (-10+(int)g_model.frsky.varioMin) * 100;
|
||||
|
||||
if (verticalSpeed < varioCenterMin || (verticalSpeed > varioCenterMax && (int16_t)(s_varioTmr - tmr10ms) < 0)) {
|
||||
if (verticalSpeed > varioMax)
|
||||
verticalSpeed = varioMax;
|
||||
else if (verticalSpeed < varioMin)
|
||||
verticalSpeed = varioMin;
|
||||
|
||||
uint8_t varioFreq, varioDuration;
|
||||
if (verticalSpeed > 0) {
|
||||
varioFreq = (verticalSpeed * 4 + 8000) >> 7;
|
||||
varioDuration = (8000 - verticalSpeed * 5) / 100;
|
||||
}
|
||||
else {
|
||||
varioFreq = (verticalSpeed * 3 + 8000) >> 7;
|
||||
varioDuration = 20;
|
||||
}
|
||||
s_varioTmr = tmr10ms + (varioDuration/2);
|
||||
AUDIO_VARIO(varioFreq, varioDuration);
|
||||
}
|
||||
#endif
|
||||
|
||||
#elif defined(BUZZER) // && !defined(AUDIO)
|
||||
|
||||
int8_t verticalSpeed = limit((int16_t)-100, (int16_t)(frskyData.hub.varioSpeed/10), (int16_t)+100);
|
||||
|
||||
uint16_t interval;
|
||||
if (verticalSpeed == 0) {
|
||||
interval = 300;
|
||||
}
|
||||
else {
|
||||
if (verticalSpeed < 0) {
|
||||
verticalSpeed = -verticalSpeed;
|
||||
warble = 1;
|
||||
}
|
||||
interval = (uint8_t)200 / verticalSpeed;
|
||||
}
|
||||
if (g_tmr10ms - s_varioTmr > interval) {
|
||||
s_varioTmr = g_tmr10ms;
|
||||
if (warble)
|
||||
AUDIO_VARIO_DOWN();
|
||||
else
|
||||
AUDIO_VARIO_UP();
|
||||
}
|
||||
#endif
|
||||
}
|
||||
else {
|
||||
s_varioTmr = tmr10ms;
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
#include "opentx.h"
|
||||
|
||||
|
||||
#if defined(CPUARM)
|
||||
|
||||
void varioWakeup()
|
||||
{
|
||||
if (isFunctionActive(FUNCTION_VARIO)) {
|
||||
int varioFreq, varioDuration, varioPause=0;
|
||||
uint8_t varioFlags;
|
||||
|
||||
int verticalSpeed = 0;
|
||||
if (g_model.frsky.varioSource) {
|
||||
uint8_t item = g_model.frsky.varioSource-1;
|
||||
if (item < MAX_SENSORS) {
|
||||
verticalSpeed = telemetryItems[item].value * g_model.telemetrySensors[item].getPrecMultiplier();
|
||||
}
|
||||
}
|
||||
|
||||
int varioCenterMin = (int)g_model.frsky.varioCenterMin * 10 - 50;
|
||||
int varioCenterMax = (int)g_model.frsky.varioCenterMax * 10 + 50;
|
||||
int varioMax = (10+(int)g_model.frsky.varioMax) * 100;
|
||||
int varioMin = (-10+(int)g_model.frsky.varioMin) * 100;
|
||||
|
||||
if (verticalSpeed > varioMax)
|
||||
verticalSpeed = varioMax;
|
||||
else if (verticalSpeed < varioMin)
|
||||
verticalSpeed = varioMin;
|
||||
|
||||
if (verticalSpeed <= varioCenterMin) {
|
||||
varioFreq = VARIO_FREQUENCY_ZERO + (g_eeGeneral.varioPitch*10) - (((VARIO_FREQUENCY_ZERO+(g_eeGeneral.varioPitch*10)-((VARIO_FREQUENCY_ZERO + (g_eeGeneral.varioPitch*10))/2)) * (verticalSpeed-varioCenterMin)) / varioMin);
|
||||
varioDuration = 80; // continuous beep: we will enter again here before the tone ends
|
||||
varioFlags = PLAY_BACKGROUND|PLAY_NOW;
|
||||
}
|
||||
else if (verticalSpeed >= varioCenterMax || !g_model.frsky.varioCenterSilent) {
|
||||
varioFreq = VARIO_FREQUENCY_ZERO + (g_eeGeneral.varioPitch*10) + (((VARIO_FREQUENCY_RANGE+(g_eeGeneral.varioRange*10)) * (verticalSpeed-varioCenterMin)) / varioMax);
|
||||
int varioPeriod = VARIO_REPEAT_MAX + ((VARIO_REPEAT_ZERO+(g_eeGeneral.varioRepeat*10)-VARIO_REPEAT_MAX) * (varioMax-verticalSpeed) * (varioMax-verticalSpeed)) / ((varioMax-varioCenterMin) * (varioMax-varioCenterMin));
|
||||
if (verticalSpeed >= varioCenterMax || varioCenterMin == varioCenterMax)
|
||||
varioDuration = varioPeriod / 5;
|
||||
else
|
||||
varioDuration = varioPeriod * (85 - (((verticalSpeed-varioCenterMin) * 25) / (varioCenterMax-varioCenterMin))) / 100;
|
||||
varioPause = varioPeriod - varioDuration;
|
||||
varioFlags = PLAY_BACKGROUND;
|
||||
}
|
||||
else {
|
||||
return;
|
||||
}
|
||||
|
||||
AUDIO_VARIO(varioFreq, varioDuration, varioPause, varioFlags);
|
||||
}
|
||||
}
|
||||
|
||||
#elif defined(FRSKY)
|
||||
|
||||
void varioWakeup()
|
||||
{
|
||||
static tmr10ms_t s_varioTmr;
|
||||
tmr10ms_t tmr10ms = get_tmr10ms();
|
||||
|
||||
if (isFunctionActive(FUNCTION_VARIO)) {
|
||||
#if defined(AUDIO)
|
||||
cli();
|
||||
int16_t verticalSpeed = frskyData.hub.varioSpeed;
|
||||
sei();
|
||||
|
||||
#if defined(PCBSTD)
|
||||
int16_t varioCenterMax = (int16_t)g_model.frsky.varioCenterMax * 10 + 50;
|
||||
if (verticalSpeed >= varioCenterMax) {
|
||||
verticalSpeed = verticalSpeed - varioCenterMax;
|
||||
int16_t varioMax = (10+(int16_t)g_model.frsky.varioMax) * 100;
|
||||
if (verticalSpeed > varioMax) verticalSpeed = varioMax;
|
||||
verticalSpeed = (verticalSpeed * 10) / ((varioMax-varioCenterMax) / 100);
|
||||
|
||||
if ((int16_t)(s_varioTmr-tmr10ms) < 0) {
|
||||
uint8_t varioFreq = (verticalSpeed * 10 + 16000) >> 8;
|
||||
uint8_t varioDuration = (1600 - verticalSpeed) / 100;
|
||||
s_varioTmr = tmr10ms + (varioDuration*2);
|
||||
AUDIO_VARIO(varioFreq, varioDuration);
|
||||
}
|
||||
}
|
||||
#else
|
||||
int varioCenterMin = (int)g_model.frsky.varioCenterMin * 10 - 50;
|
||||
int varioCenterMax = (int)g_model.frsky.varioCenterMax * 10 + 50;
|
||||
int varioMax = (10+(int)g_model.frsky.varioMax) * 100;
|
||||
int varioMin = (-10+(int)g_model.frsky.varioMin) * 100;
|
||||
|
||||
if (verticalSpeed < varioCenterMin || (verticalSpeed > varioCenterMax && (int16_t)(s_varioTmr - tmr10ms) < 0)) {
|
||||
if (verticalSpeed > varioMax)
|
||||
verticalSpeed = varioMax;
|
||||
else if (verticalSpeed < varioMin)
|
||||
verticalSpeed = varioMin;
|
||||
|
||||
uint8_t varioFreq, varioDuration;
|
||||
if (verticalSpeed > 0) {
|
||||
varioFreq = (verticalSpeed * 4 + 8000) >> 7;
|
||||
varioDuration = (8000 - verticalSpeed * 5) / 100;
|
||||
}
|
||||
else {
|
||||
varioFreq = (verticalSpeed * 3 + 8000) >> 7;
|
||||
varioDuration = 20;
|
||||
}
|
||||
s_varioTmr = tmr10ms + (varioDuration/2);
|
||||
AUDIO_VARIO(varioFreq, varioDuration);
|
||||
}
|
||||
#endif
|
||||
|
||||
#elif defined(BUZZER) // && !defined(AUDIO)
|
||||
|
||||
int8_t verticalSpeed = limit((int16_t)-100, (int16_t)(frskyData.hub.varioSpeed/10), (int16_t)+100);
|
||||
|
||||
uint16_t interval;
|
||||
if (verticalSpeed == 0) {
|
||||
interval = 300;
|
||||
}
|
||||
else {
|
||||
if (verticalSpeed < 0) {
|
||||
verticalSpeed = -verticalSpeed;
|
||||
warble = 1;
|
||||
}
|
||||
interval = (uint8_t)200 / verticalSpeed;
|
||||
}
|
||||
if (g_tmr10ms - s_varioTmr > interval) {
|
||||
s_varioTmr = g_tmr10ms;
|
||||
if (warble)
|
||||
AUDIO_VARIO_DOWN();
|
||||
else
|
||||
AUDIO_VARIO_UP();
|
||||
}
|
||||
#endif
|
||||
}
|
||||
else {
|
||||
s_varioTmr = tmr10ms;
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue