Mirror/opentx
1
0
Fork 0
mirror of https://github.com/opentx/opentx.git synced 2025-07-25 09:15:38 +03:00
Code Issues Wiki Activity

CRLF removed

Browse source
This commit is contained in:
Bertrand Songis 2016-03-14 22:30:46 +01:00
parent 691f4aa0ae
commit e9bcb05a93
14 changed files with 7545 additions and 7545 deletions
Download patch file Download diff file Expand all files Collapse all files

68
radio/src/buzzer.cpp
View file

@ -18,37 +18,37 @@
* GNU General Public License for more details. * GNU General Public License for more details.
*/ */
#include "opentx.h" #include "opentx.h"
uint8_t g_beepCnt; uint8_t g_beepCnt;
uint8_t beepAgain = 0; uint8_t beepAgain = 0;
uint8_t beepAgainOrig = 0; uint8_t beepAgainOrig = 0;
uint8_t beepOn = false; uint8_t beepOn = false;
bool warble = false; bool warble = false;
bool warbleC; bool warbleC;
// The various "beep" tone lengths // The various "beep" tone lengths
static const pm_uint8_t beepTab[] PROGMEM = { static const pm_uint8_t beepTab[] PROGMEM = {
// key, trim, warn2, warn1, error // key, trim, warn2, warn1, error
1, 1, 2, 10, 60, //xShort 1, 1, 2, 10, 60, //xShort
1, 1, 4, 20, 80, //short 1, 1, 4, 20, 80, //short
1, 1, 8, 30, 100, //normal 1, 1, 8, 30, 100, //normal
2, 2, 15, 40, 120, //long 2, 2, 15, 40, 120, //long
5, 5, 30, 50, 150, //xLong 5, 5, 30, 50, 150, //xLong
}; };
void beep(uint8_t val) void beep(uint8_t val)
{ {
#if defined(HAPTIC) && !defined(AUDIO) #if defined(HAPTIC) && !defined(AUDIO)
haptic.event(val==0 ? AU_KEYPAD_UP : (val==4 ? AU_ERROR : AU_TIMER_LT10+beepAgain)); haptic.event(val==0 ? AU_KEYPAD_UP : (val==4 ? AU_ERROR : AU_TIMER_LT10+beepAgain));
#endif #endif
#if !defined(AUDIO) #if !defined(AUDIO)
if (g_eeGeneral.alarmsFlash && val>1) flashCounter = FLASH_DURATION; if (g_eeGeneral.alarmsFlash && val>1) flashCounter = FLASH_DURATION;
#endif #endif
if (g_eeGeneral.beepMode>0 || (g_eeGeneral.beepMode==0 && val!=0) || (g_eeGeneral.beepMode==-1 && val>=3)) { 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)); _beep(pgm_read_byte(beepTab+5*(2+g_eeGeneral.beepLength)+val));
} }
} }

1334
radio/src/cli.cpp
View file

File diff suppressed because it is too large Load diff

874
radio/src/curves.cpp
View file

@ -1,437 +1,437 @@
/* /*
* Copyright (C) OpenTX * Copyright (C) OpenTX
* *
* Based on code named * Based on code named
* th9x - http://code.google.com/p/th9x * th9x - http://code.google.com/p/th9x
* er9x - http://code.google.com/p/er9x * er9x - http://code.google.com/p/er9x
* gruvin9x - http://code.google.com/p/gruvin9x * gruvin9x - http://code.google.com/p/gruvin9x
* *
* License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
* *
* This program is free software; you can redistribute it and/or modify * 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 * it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation. * published by the Free Software Foundation.
* *
* This program is distributed in the hope that it will be useful, * This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of * but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details. * GNU General Public License for more details.
*/ */
#include "opentx.h" #include "opentx.h"
#if defined(XCURVES) #if defined(XCURVES)
int8_t *curveEnd[MAX_CURVES]; int8_t *curveEnd[MAX_CURVES];
void loadCurves() void loadCurves()
{ {
int8_t * tmp = g_model.points; int8_t * tmp = g_model.points;
for (int i=0; i<MAX_CURVES; i++) { for (int i=0; i<MAX_CURVES; i++) {
switch (g_model.curves[i].type) { switch (g_model.curves[i].type) {
case CURVE_TYPE_STANDARD: case CURVE_TYPE_STANDARD:
tmp += 5+g_model.curves[i].points; tmp += 5+g_model.curves[i].points;
break; break;
case CURVE_TYPE_CUSTOM: case CURVE_TYPE_CUSTOM:
tmp += 8+2*g_model.curves[i].points; tmp += 8+2*g_model.curves[i].points;
break; break;
default: default:
TRACE("Wrong curve type! Fixing..."); TRACE("Wrong curve type! Fixing...");
g_model.curves[i].type = CURVE_TYPE_STANDARD; g_model.curves[i].type = CURVE_TYPE_STANDARD;
tmp += 5+g_model.curves[i].points; tmp += 5+g_model.curves[i].points;
break; break;
} }
curveEnd[i] = tmp; curveEnd[i] = tmp;
} }
} }
int8_t *curveAddress(uint8_t idx) int8_t *curveAddress(uint8_t idx)
{ {
return idx==0 ? g_model.points : curveEnd[idx-1]; return idx==0 ? g_model.points : curveEnd[idx-1];
} }
#else #else
int8_t *curveAddress(uint8_t idx) int8_t *curveAddress(uint8_t idx)
{ {
return &g_model.points[idx==0 ? 0 : 5*idx+g_model.curves[idx-1]]; return &g_model.points[idx==0 ? 0 : 5*idx+g_model.curves[idx-1]];
} }
CurveInfo curveInfo(uint8_t idx) CurveInfo curveInfo(uint8_t idx)
{ {
CurveInfo result; CurveInfo result;
result.crv = curveAddress(idx); result.crv = curveAddress(idx);
int8_t *next = curveAddress(idx+1); int8_t *next = curveAddress(idx+1);
uint8_t size = next - result.crv; uint8_t size = next - result.crv;
if ((size & 1) == 0) { if ((size & 1) == 0) {
result.points = (size / 2) + 1; result.points = (size / 2) + 1;
result.custom = true; result.custom = true;
} }
else { else {
result.points = size; result.points = size;
result.custom = false; result.custom = false;
} }
return result; return result;
} }
#endif #endif
#if defined(XCURVES) #if defined(XCURVES)
#define CUSTOM_POINT_X(points, count, idx) ((idx)==0 ? -100 : (((idx)==(count)-1) ? 100 : points[(count)+(idx)-1])) #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 compute_tangent(CurveInfo * crv, int8_t * points, int i)
{ {
s32 m=0; s32 m=0;
uint8_t num_points = crv->points + 5; uint8_t num_points = crv->points + 5;
#define MMULT 1024 #define MMULT 1024
if (i == 0) { if (i == 0) {
//linear interpolation between 1st 2 points //linear interpolation between 1st 2 points
//keep 3 decimal-places for m //keep 3 decimal-places for m
if (crv->type == CURVE_TYPE_CUSTOM) { if (crv->type == CURVE_TYPE_CUSTOM) {
int8_t x0 = CUSTOM_POINT_X(points, num_points, 0); int8_t x0 = CUSTOM_POINT_X(points, num_points, 0);
int8_t x1 = CUSTOM_POINT_X(points, num_points, 1); int8_t x1 = CUSTOM_POINT_X(points, num_points, 1);
if (x1 > x0) m = (MMULT * (points[1] - points[0])) / (x1 - x0); if (x1 > x0) m = (MMULT * (points[1] - points[0])) / (x1 - x0);
} }
else { else {
s32 delta = (2 * 100) / (num_points - 1); s32 delta = (2 * 100) / (num_points - 1);
m = (MMULT * (points[1] - points[0])) / delta; m = (MMULT * (points[1] - points[0])) / delta;
} }
} }
else if (i == num_points - 1) { else if (i == num_points - 1) {
//linear interpolation between last 2 points //linear interpolation between last 2 points
//keep 3 decimal-places for m //keep 3 decimal-places for m
if (crv->type == CURVE_TYPE_CUSTOM) { if (crv->type == CURVE_TYPE_CUSTOM) {
int8_t x0 = CUSTOM_POINT_X(points, num_points, num_points-2); int8_t x0 = CUSTOM_POINT_X(points, num_points, num_points-2);
int8_t x1 = CUSTOM_POINT_X(points, num_points, num_points-1); 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); if (x1 > x0) m = (MMULT * (points[num_points-1] - points[num_points-2])) / (x1 - x0);
} }
else { else {
s32 delta = (2 * 100) / (num_points - 1); s32 delta = (2 * 100) / (num_points - 1);
m = (MMULT * (points[num_points-1] - points[num_points-2])) / delta; m = (MMULT * (points[num_points-1] - points[num_points-2])) / delta;
} }
} }
else { else {
//apply monotone rules from //apply monotone rules from
//http://en.wikipedia.org/wiki/Monotone_cubic_interpolation //http://en.wikipedia.org/wiki/Monotone_cubic_interpolation
//1) compute slopes of secant lines //1) compute slopes of secant lines
s32 d0=0, d1=0; s32 d0=0, d1=0;
if (crv->type == CURVE_TYPE_CUSTOM) { if (crv->type == CURVE_TYPE_CUSTOM) {
int8_t x0 = CUSTOM_POINT_X(points, num_points, i-1); int8_t x0 = CUSTOM_POINT_X(points, num_points, i-1);
int8_t x1 = CUSTOM_POINT_X(points, num_points, i); int8_t x1 = CUSTOM_POINT_X(points, num_points, i);
int8_t x2 = CUSTOM_POINT_X(points, num_points, i+1); int8_t x2 = CUSTOM_POINT_X(points, num_points, i+1);
if (x1 > x0) d0 = (MMULT * (points[i] - points[i-1])) / (x1 - x0); if (x1 > x0) d0 = (MMULT * (points[i] - points[i-1])) / (x1 - x0);
if (x2 > x1) d1 = (MMULT * (points[i+1] - points[i])) / (x2 - x1); if (x2 > x1) d1 = (MMULT * (points[i+1] - points[i])) / (x2 - x1);
} }
else { else {
s32 delta = (2 * 100) / (num_points - 1); s32 delta = (2 * 100) / (num_points - 1);
d0 = (MMULT * (points[i] - points[i-1])) / (delta); d0 = (MMULT * (points[i] - points[i-1])) / (delta);
d1 = (MMULT * (points[i+1] - points[i])) / (delta); d1 = (MMULT * (points[i+1] - points[i])) / (delta);
} }
//2) compute initial average tangent //2) compute initial average tangent
m = (d0 + d1) / 2; m = (d0 + d1) / 2;
//3 check for horizontal lines //3 check for horizontal lines
if (d0 == 0 || d1 == 0 || (d0 > 0 && d1 < 0) || (d0 < 0 && d1 > 0)) { if (d0 == 0 || d1 == 0 || (d0 > 0 && d1 < 0) || (d0 < 0 && d1 > 0)) {
m = 0; m = 0;
} }
else if (MMULT * m / d0 > 3 * MMULT) { else if (MMULT * m / d0 > 3 * MMULT) {
m = 3 * d0; m = 3 * d0;
} }
else if (MMULT * m / d1 > 3 * MMULT) { else if (MMULT * m / d1 > 3 * MMULT) {
m = 3 * d1; m = 3 * d1;
} }
} }
return m; return m;
} }
/* The following is a hermite cubic spline. /* The following is a hermite cubic spline.
The basis functions can be found here: The basis functions can be found here:
http://en.wikipedia.org/wiki/Cubic_Hermite_spline http://en.wikipedia.org/wiki/Cubic_Hermite_spline
The tangents are computed via the 'cubic monotone' rules (allowing for local-maxima) The tangents are computed via the 'cubic monotone' rules (allowing for local-maxima)
*/ */
int16_t hermite_spline(int16_t x, uint8_t idx) int16_t hermite_spline(int16_t x, uint8_t idx)
{ {
CurveInfo &crv = g_model.curves[idx]; CurveInfo &crv = g_model.curves[idx];
int8_t *points = curveAddress(idx); int8_t *points = curveAddress(idx);
uint8_t count = crv.points+5; uint8_t count = crv.points+5;
bool custom = (crv.type == CURVE_TYPE_CUSTOM); bool custom = (crv.type == CURVE_TYPE_CUSTOM);
if (x < -RESX) if (x < -RESX)
x = -RESX; x = -RESX;
else if (x > RESX) else if (x > RESX)
x = RESX; x = RESX;
for (int i=0; i<count-1; i++) { for (int i=0; i<count-1; i++) {
s32 p0x, p3x; s32 p0x, p3x;
if (custom) { if (custom) {
p0x = (i>0 ? calc100toRESX(points[count+i-1]) : -RESX); p0x = (i>0 ? calc100toRESX(points[count+i-1]) : -RESX);
p3x = (i<count-2 ? calc100toRESX(points[count+i]) : RESX); p3x = (i<count-2 ? calc100toRESX(points[count+i]) : RESX);
} }
else { else {
p0x = -RESX + (i*2*RESX)/(count-1); p0x = -RESX + (i*2*RESX)/(count-1);
p3x = -RESX + ((i+1)*2*RESX)/(count-1); p3x = -RESX + ((i+1)*2*RESX)/(count-1);
} }
if (x >= p0x && x <= p3x) { if (x >= p0x && x <= p3x) {
s32 p0y = calc100toRESX(points[i]); s32 p0y = calc100toRESX(points[i]);
s32 p3y = calc100toRESX(points[i+1]); s32 p3y = calc100toRESX(points[i+1]);
s32 m0 = compute_tangent(&crv, points, i); s32 m0 = compute_tangent(&crv, points, i);
s32 m3 = compute_tangent(&crv, points, i+1); s32 m3 = compute_tangent(&crv, points, i+1);
s32 y; s32 y;
s32 h = p3x - p0x; s32 h = p3x - p0x;
s32 t = (h > 0 ? (MMULT * (x - p0x)) / h : 0); s32 t = (h > 0 ? (MMULT * (x - p0x)) / h : 0);
s32 t2 = t * t / MMULT; s32 t2 = t * t / MMULT;
s32 t3 = t2 * t / MMULT; s32 t3 = t2 * t / MMULT;
s32 h00 = 2*t3 - 3*t2 + MMULT; s32 h00 = 2*t3 - 3*t2 + MMULT;
s32 h10 = t3 - 2*t2 + t; s32 h10 = t3 - 2*t2 + t;
s32 h01 = -2*t3 + 3*t2; s32 h01 = -2*t3 + 3*t2;
s32 h11 = t3 - t2; s32 h11 = t3 - t2;
y = p0y * h00 + h * (m0 * h10 / MMULT) + p3y * h01 + h * (m3 * h11 / MMULT); y = p0y * h00 + h * (m0 * h10 / MMULT) + p3y * h01 + h * (m3 * h11 / MMULT);
y /= MMULT; y /= MMULT;
return y; return y;
} }
} }
return 0; return 0;
} }
#endif #endif
int intpol(int x, uint8_t idx) // -100, -75, -50, -25, 0 ,25 ,50, 75, 100 int intpol(int x, uint8_t idx) // -100, -75, -50, -25, 0 ,25 ,50, 75, 100
{ {
#if defined(XCURVES) #if defined(XCURVES)
CurveInfo &crv = g_model.curves[idx]; CurveInfo &crv = g_model.curves[idx];
int8_t *points = curveAddress(idx); int8_t *points = curveAddress(idx);
uint8_t count = crv.points+5; uint8_t count = crv.points+5;
bool custom = (crv.type == CURVE_TYPE_CUSTOM); bool custom = (crv.type == CURVE_TYPE_CUSTOM);
#else #else
CurveInfo crv = curveInfo(idx); CurveInfo crv = curveInfo(idx);
int8_t *points = crv.crv; int8_t *points = crv.crv;
uint8_t count = crv.points; uint8_t count = crv.points;
bool custom = crv.custom; bool custom = crv.custom;
#endif #endif
int16_t erg = 0; int16_t erg = 0;
x += RESXu; x += RESXu;
if (x <= 0) { if (x <= 0) {
erg = (int16_t)points[0] * (RESX/4); erg = (int16_t)points[0] * (RESX/4);
} }
else if (x >= (RESX*2)) { else if (x >= (RESX*2)) {
erg = (int16_t)points[count-1] * (RESX/4); erg = (int16_t)points[count-1] * (RESX/4);
} }
else { else {
uint16_t a=0, b=0; uint16_t a=0, b=0;
uint8_t i; uint8_t i;
if (custom) { if (custom) {
for (i=0; i<count-1; i++) { for (i=0; i<count-1; i++) {
a = b; a = b;
b = (i==count-2 ? 2*RESX : RESX + calc100toRESX(points[count+i])); b = (i==count-2 ? 2*RESX : RESX + calc100toRESX(points[count+i]));
if ((uint16_t)x<=b) break; if ((uint16_t)x<=b) break;
} }
} }
else { else {
uint16_t d = (RESX * 2) / (count-1); uint16_t d = (RESX * 2) / (count-1);
i = (uint16_t)x / d; i = (uint16_t)x / d;
a = i * d; a = i * d;
b = a + d; b = a + d;
} }
erg = (int16_t)points[i]*(RESX/4) + ((int32_t)(x-a) * (points[i+1]-points[i]) * (RESX/4)) / ((b-a)); 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; return erg / 25; // 100*D5/RESX;
} }
#if defined(CURVES) && defined(XCURVES) #if defined(CURVES) && defined(XCURVES)
int applyCurve(int x, CurveRef & curve) int applyCurve(int x, CurveRef & curve)
{ {
switch (curve.type) { switch (curve.type) {
case CURVE_REF_DIFF: case CURVE_REF_DIFF:
{ {
#if defined(CPUARM) #if defined(CPUARM)
int curveParam = GET_GVAR_PREC1(curve.value, -100, 100, mixerCurrentFlightMode); int curveParam = GET_GVAR_PREC1(curve.value, -100, 100, mixerCurrentFlightMode);
if (curveParam > 0 && x < 0) if (curveParam > 0 && x < 0)
x = (x * (1000 - curveParam)) / 1000; x = (x * (1000 - curveParam)) / 1000;
else if (curveParam < 0 && x > 0) else if (curveParam < 0 && x > 0)
x = (x * (1000 + curveParam)) / 1000; x = (x * (1000 + curveParam)) / 1000;
return x; return x;
#else #else
int curveParam = calc100to256(GET_GVAR(curve.value, -100, 100, mixerCurrentFlightMode)); int curveParam = calc100to256(GET_GVAR(curve.value, -100, 100, mixerCurrentFlightMode));
if (curveParam > 0 && x < 0) if (curveParam > 0 && x < 0)
x = (x * (256 - curveParam)) >> 8; x = (x * (256 - curveParam)) >> 8;
else if (curveParam < 0 && x > 0) else if (curveParam < 0 && x > 0)
x = (x * (256 + curveParam)) >> 8; x = (x * (256 + curveParam)) >> 8;
return x; return x;
#endif #endif
} }
case CURVE_REF_EXPO: case CURVE_REF_EXPO:
{ {
#if defined(CPUARM) #if defined(CPUARM)
int curveParam = GET_GVAR_PREC1(curve.value, -100, 100, mixerCurrentFlightMode) / 10; int curveParam = GET_GVAR_PREC1(curve.value, -100, 100, mixerCurrentFlightMode) / 10;
#else #else
int curveParam = GET_GVAR(curve.value, -100, 100, mixerCurrentFlightMode); int curveParam = GET_GVAR(curve.value, -100, 100, mixerCurrentFlightMode);
#endif #endif
return expo(x, curveParam); return expo(x, curveParam);
} }
case CURVE_REF_FUNC: case CURVE_REF_FUNC:
switch (curve.value) { switch (curve.value) {
case CURVE_X_GT0: case CURVE_X_GT0:
if (x < 0) x = 0; //x|x>0 if (x < 0) x = 0; //x|x>0
return x; return x;
case CURVE_X_LT0: case CURVE_X_LT0:
if (x > 0) x = 0; //x|x<0 if (x > 0) x = 0; //x|x<0
return x; return x;
case CURVE_ABS_X: // x|abs(x) case CURVE_ABS_X: // x|abs(x)
return abs(x); return abs(x);
case CURVE_F_GT0: //f|f>0 case CURVE_F_GT0: //f|f>0
return x > 0 ? RESX : 0; return x > 0 ? RESX : 0;
case CURVE_F_LT0: //f|f<0 case CURVE_F_LT0: //f|f<0
return x < 0 ? -RESX : 0; return x < 0 ? -RESX : 0;
case CURVE_ABS_F: //f|abs(f) case CURVE_ABS_F: //f|abs(f)
return x > 0 ? RESX : -RESX; return x > 0 ? RESX : -RESX;
} }
break; break;
case CURVE_REF_CUSTOM: case CURVE_REF_CUSTOM:
{ {
int curveParam = curve.value; int curveParam = curve.value;
if (curveParam < 0) { if (curveParam < 0) {
x = -x; x = -x;
curveParam = -curveParam; curveParam = -curveParam;
} }
if (curveParam > 0 && curveParam <= MAX_CURVES) { if (curveParam > 0 && curveParam <= MAX_CURVES) {
return applyCustomCurve(x, curveParam - 1); return applyCustomCurve(x, curveParam - 1);
} }
break; break;
} }
} }
return x; return x;
} }
int applyCustomCurve(int x, uint8_t idx) int applyCustomCurve(int x, uint8_t idx)
{ {
if (idx >= MAX_CURVES) if (idx >= MAX_CURVES)
return 0; return 0;
CurveInfo &crv = g_model.curves[idx]; CurveInfo &crv = g_model.curves[idx];
if (crv.smooth) if (crv.smooth)
return hermite_spline(x, idx); return hermite_spline(x, idx);
else else
return intpol(x, idx); return intpol(x, idx);
} }
#elif defined(CURVES) #elif defined(CURVES)
int applyCurve(int x, int8_t idx) int applyCurve(int x, int8_t idx)
{ {
/* already tried to have only one return at the end */ /* already tried to have only one return at the end */
switch(idx) { switch(idx) {
case CURVE_NONE: case CURVE_NONE:
return x; return x;
case CURVE_X_GT0: case CURVE_X_GT0:
if (x < 0) x = 0; //x|x>0 if (x < 0) x = 0; //x|x>0
return x; return x;
case CURVE_X_LT0: case CURVE_X_LT0:
if (x > 0) x = 0; //x|x<0 if (x > 0) x = 0; //x|x<0
return x; return x;
case CURVE_ABS_X: // x|abs(x) case CURVE_ABS_X: // x|abs(x)
return abs(x); return abs(x);
case CURVE_F_GT0: //f|f>0 case CURVE_F_GT0: //f|f>0
return x > 0 ? RESX : 0; return x > 0 ? RESX : 0;
case CURVE_F_LT0: //f|f<0 case CURVE_F_LT0: //f|f<0
return x < 0 ? -RESX : 0; return x < 0 ? -RESX : 0;
case CURVE_ABS_F: //f|abs(f) case CURVE_ABS_F: //f|abs(f)
return x > 0 ? RESX : -RESX; return x > 0 ? RESX : -RESX;
} }
if (idx < 0) { if (idx < 0) {
x = -x; x = -x;
idx = -idx + CURVE_BASE - 1; idx = -idx + CURVE_BASE - 1;
} }
return applyCustomCurve(x, idx - CURVE_BASE); return applyCustomCurve(x, idx - CURVE_BASE);
} }
#endif #endif
// #define EXTENDED_EXPO // #define EXTENDED_EXPO
// increases range of expo curve but costs about 82 bytes flash // increases range of expo curve but costs about 82 bytes flash
// expo-funktion: // expo-funktion:
// --------------- // ---------------
// kmplot // 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)=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*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)=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] // 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, // don't know what this above should be, just confusing in my opinion,
// here is the real explanation // here is the real explanation
// actually the real formula is // actually the real formula is
/* /*
f(x) = exp( ln(x) * 10^k) 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 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 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 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) ) ) f(x) = 1024 * ( e^( ln(x/1024) * 10^(k/100) ) )
This would be really hard to be calculated by such a microcontroller 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 Therefore Thomas Husterer compared a few usual function something like x^3, x^4*something, which look similar
Actually the formula Actually the formula
f(x) = k*x^3+x*(1-k) f(x) = k*x^3+x*(1-k)
gives a similar form and should have even advantages compared to a original exp curve. 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 This function again expect x from 0 to 1 and k only from 0 to 1
Therefore rescaling is needed like before: Therefore rescaling is needed like before:
f(x) = 1024* ((k/100)*(x/1024)^3 + (x/1024)*(100-k)/100) f(x) = 1024* ((k/100)*(x/1024)^3 + (x/1024)*(100-k)/100)
some mathematical tricks some mathematical tricks
f(x) = (k*x*x*x/(1024*1024) + x*(100-k)) / 100 f(x) = (k*x*x*x/(1024*1024) + x*(100-k)) / 100
for better rounding results we add the 50 for better rounding results we add the 50
f(x) = (k*x*x*x/(1024*1024) + x*(100-k) + 50) / 100 f(x) = (k*x*x*x/(1024*1024) + x*(100-k) + 50) / 100
because we now understand the formula, we can optimize it further 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 --> calc100to256(k) --> eliminates /100 by replacing with /256 which is just a simple shift right 8
k is now between 0 and 256 k is now between 0 and 256
f(x) = (k*x*x*x/(1024*1024) + x*(256-k) + 128) / 256 f(x) = (k*x*x*x/(1024*1024) + x*(256-k) + 128) / 256
*/ */
// input parameters; // input parameters;
// x 0 to 1024; // x 0 to 1024;
// k 0 to 100; // k 0 to 100;
// output between 0 and 1024 // output between 0 and 1024
unsigned int expou(unsigned int x, unsigned int k) unsigned int expou(unsigned int x, unsigned int k)
{ {
#if defined(EXTENDED_EXPO) #if defined(EXTENDED_EXPO)
bool extended; bool extended;
if (k>80) { if (k>80) {
extended=true; extended=true;
} }
else { else {
k += (k>>2); // use bigger values before extend, because the effect is anyway very very low k += (k>>2); // use bigger values before extend, because the effect is anyway very very low
extended=false; extended=false;
} }
#endif #endif
k = calc100to256(k); k = calc100to256(k);
uint32_t value = (uint32_t) x*x; uint32_t value = (uint32_t) x*x;
value *= (uint32_t)k; value *= (uint32_t)k;
value >>= 8; value >>= 8;
value *= (uint32_t)x; value *= (uint32_t)x;
#if defined(EXTENDED_EXPO) #if defined(EXTENDED_EXPO)
if (extended) { // for higher values do more multiplications to get a stronger expo curve if (extended) { // for higher values do more multiplications to get a stronger expo curve
value >>= 16; value >>= 16;
value *= (uint32_t)x; value *= (uint32_t)x;
value >>= 4; value >>= 4;
value *= (uint32_t)x; value *= (uint32_t)x;
} }
#endif #endif
value >>= 12; value >>= 12;
value += (uint32_t)(256-k)*x+128; value += (uint32_t)(256-k)*x+128;
return value>>8; return value>>8;
} }
int expo(int x, int k) int expo(int x, int k)
{ {
if (k == 0) return x; if (k == 0) return x;
int y; int y;
bool neg = (x < 0); bool neg = (x < 0);
if (neg) x = -x; if (neg) x = -x;
if (k<0) { if (k<0) {
y = RESXu-expou(RESXu-x, -k); y = RESXu-expou(RESXu-x, -k);
} }
else { else {
y = expou(x, k); y = expou(x, k);
} }
return neg? -y : y; return neg? -y : y;
} }

166
radio/src/debug.cpp
View file

@ -18,86 +18,86 @@
* GNU General Public License for more details. * GNU General Public License for more details.
*/ */
#include "opentx.h" #include "opentx.h"
#include "stamp.h" #include "stamp.h"
#include <stdarg.h> #include <stdarg.h>
#if defined(SIMU) #if defined(SIMU)
traceCallbackFunc traceCallback = 0; traceCallbackFunc traceCallback = 0;
#endif #endif
#if defined(SIMU) #if defined(SIMU)
#define PRINTF_BUFFER_SIZE 1024 #define PRINTF_BUFFER_SIZE 1024
void debugPrintf(const char * format, ...) void debugPrintf(const char * format, ...)
{ {
va_list arglist; va_list arglist;
char tmp[PRINTF_BUFFER_SIZE]; char tmp[PRINTF_BUFFER_SIZE];
va_start(arglist, format); va_start(arglist, format);
vsnprintf(tmp, PRINTF_BUFFER_SIZE, format, arglist); vsnprintf(tmp, PRINTF_BUFFER_SIZE, format, arglist);
va_end(arglist); va_end(arglist);
fputs(tmp, stdout); fputs(tmp, stdout);
fflush(stdout); fflush(stdout);
if (traceCallback) { if (traceCallback) {
traceCallback(tmp); traceCallback(tmp);
} }
} }
#endif #endif
#if defined(DEBUG_TRACE_BUFFER) #if defined(DEBUG_TRACE_BUFFER)
static struct TraceElement traceBuffer[TRACE_BUFFER_LEN]; static struct TraceElement traceBuffer[TRACE_BUFFER_LEN];
static uint8_t traceBufferPos; static uint8_t traceBufferPos;
gtime_t filltm(gtime_t *t, struct gtm *tp); gtime_t filltm(gtime_t *t, struct gtm *tp);
void trace_event(enum TraceEvent event, uint32_t data) void trace_event(enum TraceEvent event, uint32_t data)
{ {
if (traceBufferPos >= TRACE_BUFFER_LEN) return; if (traceBufferPos >= TRACE_BUFFER_LEN) return;
__disable_irq(); __disable_irq();
struct TraceElement * p = &traceBuffer[traceBufferPos++]; struct TraceElement * p = &traceBuffer[traceBufferPos++];
__enable_irq(); __enable_irq();
p->time = g_rtcTime; p->time = g_rtcTime;
p->time_ms = g_ms100; p->time_ms = g_ms100;
p->event = event; p->event = event;
p->data = data; p->data = data;
} }
void trace_event_i(enum TraceEvent event, uint32_t data) void trace_event_i(enum TraceEvent event, uint32_t data)
{ {
if (traceBufferPos >= TRACE_BUFFER_LEN) return; if (traceBufferPos >= TRACE_BUFFER_LEN) return;
struct TraceElement * p = &traceBuffer[traceBufferPos++]; struct TraceElement * p = &traceBuffer[traceBufferPos++];
p->time = g_rtcTime; p->time = g_rtcTime;
p->time_ms = g_ms100; p->time_ms = g_ms100;
p->event = event; p->event = event;
p->data = data; p->data = data;
} }
const struct TraceElement * getTraceElement(uint16_t idx) const struct TraceElement * getTraceElement(uint16_t idx)
{ {
if (idx < TRACE_BUFFER_LEN) return &traceBuffer[idx]; if (idx < TRACE_BUFFER_LEN) return &traceBuffer[idx];
return 0; return 0;
} }
void dumpTraceBuffer() void dumpTraceBuffer()
{ {
TRACE("Dump of Trace Buffer (" VERSION " " DATE " " TIME "):"); TRACE("Dump of Trace Buffer (" VERSION " " DATE " " TIME "):");
TRACE("# Time Event Data"); TRACE("# Time Event Data");
for(int n = 0; n < TRACE_BUFFER_LEN; ++n) { for(int n = 0; n < TRACE_BUFFER_LEN; ++n) {
struct gtm tp; struct gtm tp;
filltm(&traceBuffer[n].time, &tp); filltm(&traceBuffer[n].time, &tp);
TRACE_INFO_WP("%02d ", n); 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_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); TRACE(" %03d 0x%08x", traceBuffer[n].event, traceBuffer[n].data);
if (traceBuffer[n].time == 0 && traceBuffer[n].time_ms == 0) break; if (traceBuffer[n].time == 0 && traceBuffer[n].time_ms == 0) break;
#if !defined(SIMU) #if !defined(SIMU)
if ((n % 5) == 0) { if ((n % 5) == 0) {
while (!serial2TxFifo.empty()) { while (!serial2TxFifo.empty()) {
CoTickDelay(1); CoTickDelay(1);
} }
} }
#endif #endif
} }
TRACE("End of Trace Buffer dump"); TRACE("End of Trace Buffer dump");
} }
#endif #endif

746
radio/src/logs.cpp
View file

@ -1,373 +1,373 @@
/* /*
* Copyright (C) OpenTX * Copyright (C) OpenTX
* *
* Based on code named * Based on code named
* th9x - http://code.google.com/p/th9x * th9x - http://code.google.com/p/th9x
* er9x - http://code.google.com/p/er9x * er9x - http://code.google.com/p/er9x
* gruvin9x - http://code.google.com/p/gruvin9x * gruvin9x - http://code.google.com/p/gruvin9x
* *
* License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
* *
* This program is free software; you can redistribute it and/or modify * 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 * it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation. * published by the Free Software Foundation.
* *
* This program is distributed in the hope that it will be useful, * This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of * but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details. * GNU General Public License for more details.
*/ */
#include "opentx.h" #include "opentx.h"
#include "ff.h" #include "ff.h"
FIL g_oLogFile = {0}; FIL g_oLogFile = {0};
const pm_char * g_logError = NULL; const pm_char * g_logError = NULL;
uint8_t logDelay; uint8_t logDelay;
#if defined(PCBTARANIS) || defined(PCBFLAMENCO) || defined(PCBHORUS) #if defined(PCBTARANIS) || defined(PCBFLAMENCO) || defined(PCBHORUS)
#define get2PosState(sw) (switchState(SW_ ## sw ## 0) ? -1 : 1) #define get2PosState(sw) (switchState(SW_ ## sw ## 0) ? -1 : 1)
#else #else
#define get2PosState(sw) (switchState(SW_ ## sw) ? -1 : 1) #define get2PosState(sw) (switchState(SW_ ## sw) ? -1 : 1)
#endif #endif
#define get3PosState(sw) (switchState(SW_ ## sw ## 0) ? -1 : (switchState(SW_ ## sw ## 2) ? 1 : 0)) #define get3PosState(sw) (switchState(SW_ ## sw ## 0) ? -1 : (switchState(SW_ ## sw ## 2) ? 1 : 0))
const pm_char * openLogs() const pm_char * openLogs()
{ {
// Determine and set log file filename // Determine and set log file filename
FRESULT result; FRESULT result;
char filename[34]; // /LOGS/modelnamexxx-2013-01-01.log char filename[34]; // /LOGS/modelnamexxx-2013-01-01.log
if (!sdMounted()) if (!sdMounted())
return STR_NO_SDCARD; return STR_NO_SDCARD;
if (sdGetFreeSectors() == 0) if (sdGetFreeSectors() == 0)
return STR_SDCARD_FULL; return STR_SDCARD_FULL;
// check and create folder here // check and create folder here
strcpy_P(filename, STR_LOGS_PATH); strcpy_P(filename, STR_LOGS_PATH);
const char * error = sdCheckAndCreateDirectory(filename); const char * error = sdCheckAndCreateDirectory(filename);
if (error) { if (error) {
return error; return error;
} }
filename[sizeof(LOGS_PATH)-1] = '/'; filename[sizeof(LOGS_PATH)-1] = '/';
memcpy(&filename[sizeof(LOGS_PATH)], g_model.header.name, sizeof(g_model.header.name)); memcpy(&filename[sizeof(LOGS_PATH)], g_model.header.name, sizeof(g_model.header.name));
filename[sizeof(LOGS_PATH)+sizeof(g_model.header.name)] = '\0'; filename[sizeof(LOGS_PATH)+sizeof(g_model.header.name)] = '\0';
uint8_t i = sizeof(LOGS_PATH)+sizeof(g_model.header.name)-1; uint8_t i = sizeof(LOGS_PATH)+sizeof(g_model.header.name)-1;
uint8_t len = 0; uint8_t len = 0;
while (i>sizeof(LOGS_PATH)-1) { while (i>sizeof(LOGS_PATH)-1) {
if (!len && filename[i]) if (!len && filename[i])
len = i+1; len = i+1;
if (len) { if (len) {
if (filename[i]) if (filename[i])
filename[i] = idx2char(filename[i]); filename[i] = idx2char(filename[i]);
else else
filename[i] = '_'; filename[i] = '_';
} }
i--; i--;
} }
if (len == 0) { if (len == 0) {
#if defined(EEPROM) #if defined(EEPROM)
uint8_t num = g_eeGeneral.currModel + 1; uint8_t num = g_eeGeneral.currModel + 1;
#else #else
// TODO // TODO
uint8_t num = 1; uint8_t num = 1;
#endif #endif
strcpy_P(&filename[sizeof(LOGS_PATH)], STR_MODEL); 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)] = (char)((num / 10) + '0');
filename[sizeof(LOGS_PATH) + PSIZE(TR_MODEL) + 1] = (char)((num % 10) + '0'); filename[sizeof(LOGS_PATH) + PSIZE(TR_MODEL) + 1] = (char)((num % 10) + '0');
len = sizeof(LOGS_PATH) + PSIZE(TR_MODEL) + 2; len = sizeof(LOGS_PATH) + PSIZE(TR_MODEL) + 2;
} }
char * tmp = &filename[len]; char * tmp = &filename[len];
#if defined(RTCLOCK) #if defined(RTCLOCK)
tmp = strAppendDate(&filename[len]); tmp = strAppendDate(&filename[len]);
#endif #endif
strcpy_P(tmp, STR_LOGS_EXT); strcpy_P(tmp, STR_LOGS_EXT);
result = f_open(&g_oLogFile, filename, FA_OPEN_ALWAYS | FA_WRITE); result = f_open(&g_oLogFile, filename, FA_OPEN_ALWAYS | FA_WRITE);
if (result != FR_OK) { if (result != FR_OK) {
return SDCARD_ERROR(result); return SDCARD_ERROR(result);
} }
if (f_size(&g_oLogFile) == 0) { if (f_size(&g_oLogFile) == 0) {
writeHeader(); writeHeader();
} }
else { else {
result = f_lseek(&g_oLogFile, f_size(&g_oLogFile)); // append result = f_lseek(&g_oLogFile, f_size(&g_oLogFile)); // append
if (result != FR_OK) { if (result != FR_OK) {
return SDCARD_ERROR(result); return SDCARD_ERROR(result);
} }
} }
return NULL; return NULL;
} }
tmr10ms_t lastLogTime = 0; tmr10ms_t lastLogTime = 0;
void closeLogs() void closeLogs()
{ {
if (f_close(&g_oLogFile) != FR_OK) { if (f_close(&g_oLogFile) != FR_OK) {
// close failed, forget file // close failed, forget file
g_oLogFile.fs = 0; g_oLogFile.fs = 0;
} }
lastLogTime = 0; lastLogTime = 0;
} }
#if !defined(CPUARM) #if !defined(CPUARM)
getvalue_t getConvertedTelemetryValue(getvalue_t val, uint8_t unit) getvalue_t getConvertedTelemetryValue(getvalue_t val, uint8_t unit)
{ {
convertUnit(val, unit); convertUnit(val, unit);
return val; return val;
} }
#endif #endif
void writeHeader() void writeHeader()
{ {
#if defined(RTCLOCK) #if defined(RTCLOCK)
f_puts("Date,Time,", &g_oLogFile); f_puts("Date,Time,", &g_oLogFile);
#else #else
f_puts("Time,", &g_oLogFile); f_puts("Time,", &g_oLogFile);
#endif #endif
#if defined(FRSKY) #if defined(FRSKY)
#if !defined(CPUARM) #if !defined(CPUARM)
f_puts("Buffer,RX,TX,A1,A2,", &g_oLogFile); f_puts("Buffer,RX,TX,A1,A2,", &g_oLogFile);
#if defined(FRSKY_HUB) #if defined(FRSKY_HUB)
if (IS_USR_PROTO_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("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(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); f_puts("),Vertical Speed,Air Speed(kts),Temp1,Temp2,RPM,Fuel," TELEMETRY_CELLS_LABEL "Current,Consumption,Vfas,AccelX,AccelY,AccelZ,", &g_oLogFile);
} }
#endif #endif
#if defined(WS_HOW_HIGH) #if defined(WS_HOW_HIGH)
if (IS_USR_PROTO_WS_HOW_HIGH()) { if (IS_USR_PROTO_WS_HOW_HIGH()) {
f_puts("WSHH Alt,", &g_oLogFile); f_puts("WSHH Alt,", &g_oLogFile);
} }
#endif #endif
#endif #endif
#if defined(CPUARM) #if defined(CPUARM)
char label[TELEM_LABEL_LEN+7]; char label[TELEM_LABEL_LEN+7];
for (int i=0; i<MAX_SENSORS; i++) { for (int i=0; i<MAX_SENSORS; i++) {
TelemetrySensor & sensor = g_model.telemetrySensors[i]; TelemetrySensor & sensor = g_model.telemetrySensors[i];
if (sensor.logs) { if (sensor.logs) {
memset(label, 0, sizeof(label)); memset(label, 0, sizeof(label));
zchar2str(label, sensor.label, TELEM_LABEL_LEN); zchar2str(label, sensor.label, TELEM_LABEL_LEN);
if (sensor.unit != UNIT_RAW && sensor.unit != UNIT_GPS && sensor.unit != UNIT_DATETIME) { if (sensor.unit != UNIT_RAW && sensor.unit != UNIT_GPS && sensor.unit != UNIT_DATETIME) {
strcat(label, "("); strcat(label, "(");
strncat(label, STR_VTELEMUNIT+1+3*sensor.unit, 3); strncat(label, STR_VTELEMUNIT+1+3*sensor.unit, 3);
strcat(label, ")"); strcat(label, ")");
} }
strcat(label, ","); strcat(label, ",");
f_puts(label, &g_oLogFile); f_puts(label, &g_oLogFile);
} }
} }
#endif #endif
#endif #endif
#if defined(PCBTARANIS) || defined(PCBHORUS) #if defined(PCBTARANIS) || defined(PCBHORUS)
for (uint8_t i=1; i<NUM_STICKS+NUM_POTS+1; i++) { for (uint8_t i=1; i<NUM_STICKS+NUM_POTS+1; i++) {
const char * p = STR_VSRCRAW + i * STR_VSRCRAW[0] + 2; const char * p = STR_VSRCRAW + i * STR_VSRCRAW[0] + 2;
for (uint8_t j=0; j<STR_VSRCRAW[0]-1; ++j) { for (uint8_t j=0; j<STR_VSRCRAW[0]-1; ++j) {
if (!*p) break; if (!*p) break;
f_putc(*p, &g_oLogFile); f_putc(*p, &g_oLogFile);
++p; ++p;
} }
f_putc(',', &g_oLogFile); f_putc(',', &g_oLogFile);
} }
#define STR_SWITCHES_LOG_HEADER "SA,SB,SC,SD,SE,SF,SG,SH" #define STR_SWITCHES_LOG_HEADER "SA,SB,SC,SD,SE,SF,SG,SH"
f_puts(STR_SWITCHES_LOG_HEADER ",LS" "\n", &g_oLogFile); f_puts(STR_SWITCHES_LOG_HEADER ",LS" "\n", &g_oLogFile);
#else #else
f_puts("Rud,Ele,Thr,Ail,P1,P2,P3,THR,RUD,ELE,3POS,AIL,GEA,TRN\n", &g_oLogFile); f_puts("Rud,Ele,Thr,Ail,P1,P2,P3,THR,RUD,ELE,3POS,AIL,GEA,TRN\n", &g_oLogFile);
#endif #endif
} }
uint32_t getLogicalSwitchesStates(uint8_t first) uint32_t getLogicalSwitchesStates(uint8_t first)
{ {
uint32_t result = 0; uint32_t result = 0;
for (uint8_t i=0; i<32; i++) { for (uint8_t i=0; i<32; i++) {
result |= (getSwitch(SWSRC_FIRST_LOGICAL_SWITCH+first+i) << i); result |= (getSwitch(SWSRC_FIRST_LOGICAL_SWITCH+first+i) << i);
} }
return result; return result;
} }
void writeLogs() void writeLogs()
{ {
static const pm_char * error_displayed = NULL; static const pm_char * error_displayed = NULL;
if (isFunctionActive(FUNCTION_LOGS) && logDelay > 0) { if (isFunctionActive(FUNCTION_LOGS) && logDelay > 0) {
tmr10ms_t tmr10ms = get_tmr10ms(); tmr10ms_t tmr10ms = get_tmr10ms();
if (lastLogTime == 0 || (tmr10ms_t)(tmr10ms - lastLogTime) >= (tmr10ms_t)logDelay*10) { if (lastLogTime == 0 || (tmr10ms_t)(tmr10ms - lastLogTime) >= (tmr10ms_t)logDelay*10) {
lastLogTime = tmr10ms; lastLogTime = tmr10ms;
if (!g_oLogFile.fs) { if (!g_oLogFile.fs) {
const pm_char * result = openLogs(); const pm_char * result = openLogs();
if (result != NULL) { if (result != NULL) {
if (result != error_displayed) { if (result != error_displayed) {
error_displayed = result; error_displayed = result;
POPUP_WARNING(result); POPUP_WARNING(result);
} }
return; return;
} }
} }
#if defined(RTCLOCK) #if defined(RTCLOCK)
{ {
static struct gtm utm; static struct gtm utm;
static gtime_t lastRtcTime = 0; static gtime_t lastRtcTime = 0;
if (g_rtcTime != lastRtcTime) { if (g_rtcTime != lastRtcTime) {
lastRtcTime = g_rtcTime; lastRtcTime = g_rtcTime;
gettime(&utm); 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); 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 #else
f_printf(&g_oLogFile, "%d,", tmr10ms); f_printf(&g_oLogFile, "%d,", tmr10ms);
#endif #endif
#if defined(FRSKY) #if defined(FRSKY)
#if !defined(CPUARM) #if !defined(CPUARM)
f_printf(&g_oLogFile, "%d,%d,%d,", frskyStreaming, RAW_FRSKY_MINMAX(frskyData.rssi[0]), RAW_FRSKY_MINMAX(frskyData.rssi[1])); 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++) { for (uint8_t i=0; i<MAX_FRSKY_A_CHANNELS; i++) {
int16_t converted_value = applyChannelRatio(i, RAW_FRSKY_MINMAX(frskyData.analog[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); f_printf(&g_oLogFile, "%d.%02d,", converted_value/100, converted_value%100);
} }
#if defined(FRSKY_HUB) #if defined(FRSKY_HUB)
TELEMETRY_BARO_ALT_PREPARE(); TELEMETRY_BARO_ALT_PREPARE();
if (IS_USR_PROTO_FRSKY_HUB()) { 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,", 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.year+2000,
frskyData.hub.month, frskyData.hub.month,
frskyData.hub.day, frskyData.hub.day,
frskyData.hub.hour, frskyData.hub.hour,
frskyData.hub.min, frskyData.hub.min,
frskyData.hub.sec, frskyData.hub.sec,
frskyData.hub.gpsLongitude_bp, frskyData.hub.gpsLongitude_bp,
frskyData.hub.gpsLongitude_ap, frskyData.hub.gpsLongitude_ap,
frskyData.hub.gpsLongitudeEW ? frskyData.hub.gpsLongitudeEW : '-', frskyData.hub.gpsLongitudeEW ? frskyData.hub.gpsLongitudeEW : '-',
frskyData.hub.gpsLatitude_bp, frskyData.hub.gpsLatitude_bp,
frskyData.hub.gpsLatitude_ap, frskyData.hub.gpsLatitude_ap,
frskyData.hub.gpsLatitudeNS ? frskyData.hub.gpsLatitudeNS : '-', frskyData.hub.gpsLatitudeNS ? frskyData.hub.gpsLatitudeNS : '-',
frskyData.hub.gpsCourse_bp, frskyData.hub.gpsCourse_bp,
frskyData.hub.gpsCourse_ap, frskyData.hub.gpsCourse_ap,
TELEMETRY_GPS_SPEED_ARGS TELEMETRY_GPS_SPEED_ARGS
TELEMETRY_GPS_ALT_ARGS TELEMETRY_GPS_ALT_ARGS
TELEMETRY_BARO_ALT_ARGS TELEMETRY_BARO_ALT_ARGS
TELEMETRY_VSPEED_ARGS TELEMETRY_VSPEED_ARGS
TELEMETRY_ASPEED_ARGS TELEMETRY_ASPEED_ARGS
frskyData.hub.temperature1, frskyData.hub.temperature1,
frskyData.hub.temperature2, frskyData.hub.temperature2,
frskyData.hub.rpm, frskyData.hub.rpm,
frskyData.hub.fuelLevel, frskyData.hub.fuelLevel,
TELEMETRY_CELLS_ARGS TELEMETRY_CELLS_ARGS
TELEMETRY_CURRENT_ARGS TELEMETRY_CURRENT_ARGS
frskyData.hub.currentConsumption, frskyData.hub.currentConsumption,
TELEMETRY_VFAS_ARGS TELEMETRY_VFAS_ARGS
frskyData.hub.accelX, frskyData.hub.accelX,
frskyData.hub.accelY, frskyData.hub.accelY,
frskyData.hub.accelZ); frskyData.hub.accelZ);
} }
#endif #endif
#if defined(WS_HOW_HIGH) #if defined(WS_HOW_HIGH)
if (IS_USR_PROTO_WS_HOW_HIGH()) { if (IS_USR_PROTO_WS_HOW_HIGH()) {
f_printf(&g_oLogFile, "%d,", TELEMETRY_RELATIVE_BARO_ALT_BP); f_printf(&g_oLogFile, "%d,", TELEMETRY_RELATIVE_BARO_ALT_BP);
} }
#endif #endif
#endif #endif
#if defined(CPUARM) #if defined(CPUARM)
for (int i=0; i<MAX_SENSORS; i++) { for (int i=0; i<MAX_SENSORS; i++) {
TelemetrySensor & sensor = g_model.telemetrySensors[i]; TelemetrySensor & sensor = g_model.telemetrySensors[i];
TelemetryItem & telemetryItem = telemetryItems[i]; TelemetryItem & telemetryItem = telemetryItems[i];
if (sensor.logs) { if (sensor.logs) {
if (sensor.unit == UNIT_GPS) { if (sensor.unit == UNIT_GPS) {
if (telemetryItem.gps.longitudeEW && telemetryItem.gps.latitudeNS) 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); 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 else
f_printf(&g_oLogFile, ","); f_printf(&g_oLogFile, ",");
} }
else if (sensor.unit == UNIT_DATETIME) { else if (sensor.unit == UNIT_DATETIME) {
if (telemetryItem.datetime.datestate) 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); 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 else
f_printf(&g_oLogFile, ","); f_printf(&g_oLogFile, ",");
} }
else if (sensor.prec == 2) { else if (sensor.prec == 2) {
div_t qr = div(telemetryItem.value, 100); div_t qr = div(telemetryItem.value, 100);
if (telemetryItem.value < 0) f_printf(&g_oLogFile, "-"); if (telemetryItem.value < 0) f_printf(&g_oLogFile, "-");
f_printf(&g_oLogFile, "%d.%02d,", abs(qr.quot), abs(qr.rem)); f_printf(&g_oLogFile, "%d.%02d,", abs(qr.quot), abs(qr.rem));
} }
else if (sensor.prec == 1) { else if (sensor.prec == 1) {
div_t qr = div(telemetryItem.value, 10); div_t qr = div(telemetryItem.value, 10);
if (telemetryItem.value < 0) f_printf(&g_oLogFile, "-"); if (telemetryItem.value < 0) f_printf(&g_oLogFile, "-");
f_printf(&g_oLogFile, "%d.%d,", abs(qr.quot), abs(qr.rem)); f_printf(&g_oLogFile, "%d.%d,", abs(qr.quot), abs(qr.rem));
} }
else { else {
f_printf(&g_oLogFile, "%d,", telemetryItem.value); f_printf(&g_oLogFile, "%d,", telemetryItem.value);
} }
} }
} }
#endif #endif
#endif #endif
for (uint8_t i=0; i<NUM_STICKS+NUM_POTS; i++) { for (uint8_t i=0; i<NUM_STICKS+NUM_POTS; i++) {
f_printf(&g_oLogFile, "%d,", calibratedStick[i]); f_printf(&g_oLogFile, "%d,", calibratedStick[i]);
} }
#if defined(PCBFLAMENCO) #if defined(PCBFLAMENCO)
int result = f_printf(&g_oLogFile, "%d,%d,%d,%d\n", int result = f_printf(&g_oLogFile, "%d,%d,%d,%d\n",
get3PosState(SA), get3PosState(SA),
get3PosState(SB), get3PosState(SB),
// get3PosState(SC), // get3PosState(SC),
get2PosState(SE), get2PosState(SE),
get3PosState(SF)); get3PosState(SF));
#elif defined(PCBTARANIS) || defined(PCBHORUS) #elif defined(PCBTARANIS) || defined(PCBHORUS)
int result = f_printf(&g_oLogFile, "%d,%d,%d,%d,%d,%d,%d,%d,%lu%lu\n", int result = f_printf(&g_oLogFile, "%d,%d,%d,%d,%d,%d,%d,%d,%lu%lu\n",
get3PosState(SA), get3PosState(SA),
get3PosState(SB), get3PosState(SB),
get3PosState(SC), get3PosState(SC),
get3PosState(SD), get3PosState(SD),
get3PosState(SE), get3PosState(SE),
get2PosState(SF), get2PosState(SF),
get3PosState(SG), get3PosState(SG),
get2PosState(SH), get2PosState(SH),
getLogicalSwitchesStates(32), getLogicalSwitchesStates(32),
getLogicalSwitchesStates(0)); getLogicalSwitchesStates(0));
#else #else
int result = f_printf(&g_oLogFile, "%d,%d,%d,%d,%d,%d,%d\n", int result = f_printf(&g_oLogFile, "%d,%d,%d,%d,%d,%d,%d\n",
get2PosState(THR), get2PosState(THR),
get2PosState(RUD), get2PosState(RUD),
get2PosState(ELE), get2PosState(ELE),
get3PosState(ID), get3PosState(ID),
get2PosState(AIL), get2PosState(AIL),
get2PosState(GEA), get2PosState(GEA),
get2PosState(TRN)); get2PosState(TRN));
#endif #endif
if (result<0 && !error_displayed) { if (result<0 && !error_displayed) {
error_displayed = STR_SDCARD_ERROR; error_displayed = STR_SDCARD_ERROR;
POPUP_WARNING(STR_SDCARD_ERROR); POPUP_WARNING(STR_SDCARD_ERROR);
closeLogs(); closeLogs();
} }
} }
} }
else { else {
error_displayed = NULL; error_displayed = NULL;
if (g_oLogFile.fs) { if (g_oLogFile.fs) {
closeLogs(); closeLogs();
} }
} }
} }

372
radio/src/maths.cpp
View file

@ -1,186 +1,186 @@
/* /*
* Copyright (C) OpenTX * Copyright (C) OpenTX
* *
* Based on code named * Based on code named
* th9x - http://code.google.com/p/th9x * th9x - http://code.google.com/p/th9x
* er9x - http://code.google.com/p/er9x * er9x - http://code.google.com/p/er9x
* gruvin9x - http://code.google.com/p/gruvin9x * gruvin9x - http://code.google.com/p/gruvin9x
* *
* License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
* *
* This program is free software; you can redistribute it and/or modify * 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 * it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation. * published by the Free Software Foundation.
* *
* This program is distributed in the hope that it will be useful, * This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of * but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details. * GNU General Public License for more details.
*/ */
#include "opentx.h" #include "opentx.h"
#if !defined(CPUARM) #if !defined(CPUARM)
// #define CORRECT_NEGATIVE_SHIFTS // #define CORRECT_NEGATIVE_SHIFTS
// open.20.fsguruh; shift right operations do the rounding different for negative values compared to positive values // 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 // 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 // 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 // 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 // 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 // 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 // is done now in makefile
int16_t calc100to256_16Bits(int16_t x) // return x*2.56 int16_t calc100to256_16Bits(int16_t x) // return x*2.56
{ {
// y = 2*x + x/2 +x/16-x/512-x/2048 // y = 2*x + x/2 +x/16-x/512-x/2048
// 512 and 2048 are out of scope from int8 input --> forget it // 512 and 2048 are out of scope from int8 input --> forget it
#ifdef CORRECT_NEGATIVE_SHIFTS #ifdef CORRECT_NEGATIVE_SHIFTS
int16_t res=(int16_t)x<<1; int16_t res=(int16_t)x<<1;
//int8_t sign=(uint8_t) x>>7; //int8_t sign=(uint8_t) x>>7;
int8_t sign=(x<0?1:0); int8_t sign=(x<0?1:0);
x-=sign; x-=sign;
res+=(x>>1); res+=(x>>1);
res+=sign; res+=sign;
res+=(x>>4); res+=(x>>4);
res+=sign; res+=sign;
return res; return res;
#else #else
return ((int16_t)x<<1)+(x>>1)+(x>>4); return ((int16_t)x<<1)+(x>>1)+(x>>4);
#endif #endif
} }
int16_t calc100to256(int8_t x) // return x*2.56 int16_t calc100to256(int8_t x) // return x*2.56
{ {
return calc100to256_16Bits(x); return calc100to256_16Bits(x);
} }
int16_t calc100toRESX_16Bits(int16_t x) // return x*10.24 int16_t calc100toRESX_16Bits(int16_t x) // return x*10.24
{ {
#ifdef CORRECT_NEGATIVE_SHIFTS #ifdef CORRECT_NEGATIVE_SHIFTS
int16_t res= ((int16_t)x*41)>>2; int16_t res= ((int16_t)x*41)>>2;
int8_t sign=(x<0?1:0); int8_t sign=(x<0?1:0);
//int8_t sign=(uint8_t) x>>7; //int8_t sign=(uint8_t) x>>7;
x-=sign; x-=sign;
res-=(x>>6); res-=(x>>6);
res-=sign; res-=sign;
return res; return res;
#else #else
// return (int16_t)x*10 + x/4 - x/64; // return (int16_t)x*10 + x/4 - x/64;
return ((x*41)>>2) - (x>>6); return ((x*41)>>2) - (x>>6);
#endif #endif
} }
int16_t calc100toRESX(int8_t x) // return x*10.24 int16_t calc100toRESX(int8_t x) // return x*10.24
{ {
return calc100toRESX_16Bits(x); return calc100toRESX_16Bits(x);
} }
// return x*1.024 // return x*1.024
int16_t calc1000toRESX(int16_t x) // improve calc time by Pat MacKenzie int16_t calc1000toRESX(int16_t x) // improve calc time by Pat MacKenzie
{ {
// return x + x/32 - x/128 + x/512; // return x + x/32 - x/128 + x/512;
int16_t y = x>>5; int16_t y = x>>5;
x+=y; x+=y;
y=y>>2; y=y>>2;
x-=y; x-=y;
return x+(y>>2); return x+(y>>2);
} }
int16_t calcRESXto1000(int16_t x) // return x/1.024 int16_t calcRESXto1000(int16_t x) // return x/1.024
{ {
// *1000/1024 = x - x/32 + x/128 // *1000/1024 = x - x/32 + x/128
return (x - (x>>5) + (x>>7)); return (x - (x>>5) + (x>>7));
} }
int8_t calcRESXto100(int16_t x) int8_t calcRESXto100(int16_t x)
{ {
return (x*25) >> 8; return (x*25) >> 8;
} }
#endif #endif
#if defined(HELI) || defined(FRSKY_HUB) || defined(CPUARM) #if defined(HELI) || defined(FRSKY_HUB) || defined(CPUARM)
uint16_t isqrt32(uint32_t n) uint16_t isqrt32(uint32_t n)
{ {
uint16_t c = 0x8000; uint16_t c = 0x8000;
uint16_t g = 0x8000; uint16_t g = 0x8000;
for (;;) { for (;;) {
if ((uint32_t)g*g > n) if ((uint32_t)g*g > n)
g ^= c; g ^= c;
c >>= 1; c >>= 1;
if(c == 0) if(c == 0)
return g; return g;
g |= c; g |= c;
} }
} }
#endif #endif
/* /*
Division by 10 and rounding or fixed point arithmetic values Division by 10 and rounding or fixed point arithmetic values
Examples: Examples:
value -> result value -> result
105 -> 11 105 -> 11
104 -> 10 104 -> 10
-205 -> -21 -205 -> -21
-204 -> -20 -204 -> -20
*/ */
#if defined(PCBTARANIS) #if defined(PCBTARANIS)
double gpsToDouble(bool neg, int16_t bp, int16_t ap) double gpsToDouble(bool neg, int16_t bp, int16_t ap)
{ {
double result = ap; double result = ap;
result /= 10000; result /= 10000;
result += (bp % 100); result += (bp % 100);
result /= 60; result /= 60;
result += (bp / 100); result += (bp / 100);
return neg?-result:result; return neg?-result:result;
} }
#endif #endif
#if defined(FRSKY_HUB) && !defined(CPUARM) #if defined(FRSKY_HUB) && !defined(CPUARM)
void extractLatitudeLongitude(uint32_t * latitude, uint32_t * longitude) void extractLatitudeLongitude(uint32_t * latitude, uint32_t * longitude)
{ {
div_t qr = div(frskyData.hub.gpsLatitude_bp, 100); 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; *latitude = ((uint32_t)(qr.quot) * 1000000) + (((uint32_t)(qr.rem) * 10000 + frskyData.hub.gpsLatitude_ap) * 5) / 3;
qr = div(frskyData.hub.gpsLongitude_bp, 100); qr = div(frskyData.hub.gpsLongitude_bp, 100);
*longitude = ((uint32_t)(qr.quot) * 1000000) + (((uint32_t)(qr.rem) * 10000 + frskyData.hub.gpsLongitude_ap) * 5) / 3; *longitude = ((uint32_t)(qr.quot) * 1000000) + (((uint32_t)(qr.rem) * 10000 + frskyData.hub.gpsLongitude_ap) * 5) / 3;
} }
void getGpsPilotPosition() void getGpsPilotPosition()
{ {
extractLatitudeLongitude(&frskyData.hub.pilotLatitude, &frskyData.hub.pilotLongitude); extractLatitudeLongitude(&frskyData.hub.pilotLatitude, &frskyData.hub.pilotLongitude);
uint32_t lat = frskyData.hub.pilotLatitude / 10000; uint32_t lat = frskyData.hub.pilotLatitude / 10000;
uint32_t angle2 = (lat*lat) / 10000; uint32_t angle2 = (lat*lat) / 10000;
uint32_t angle4 = angle2 * angle2; uint32_t angle4 = angle2 * angle2;
frskyData.hub.distFromEarthAxis = 139*(((uint32_t)10000000-((angle2*(uint32_t)123370)/81)+(angle4/25))/12500); frskyData.hub.distFromEarthAxis = 139*(((uint32_t)10000000-((angle2*(uint32_t)123370)/81)+(angle4/25))/12500);
// TRACE("frskyData.hub.distFromEarthAxis=%d", frskyData.hub.distFromEarthAxis); // TRACE("frskyData.hub.distFromEarthAxis=%d", frskyData.hub.distFromEarthAxis);
} }
void getGpsDistance() void getGpsDistance()
{ {
uint32_t lat, lng; uint32_t lat, lng;
extractLatitudeLongitude(&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)); // 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 angle = (lat > frskyData.hub.pilotLatitude) ? lat - frskyData.hub.pilotLatitude : frskyData.hub.pilotLatitude - lat;
uint32_t dist = EARTH_RADIUS * angle / 1000000; uint32_t dist = EARTH_RADIUS * angle / 1000000;
uint32_t result = dist*dist; uint32_t result = dist*dist;
angle = (lng > frskyData.hub.pilotLongitude) ? lng - frskyData.hub.pilotLongitude : frskyData.hub.pilotLongitude - lng; angle = (lng > frskyData.hub.pilotLongitude) ? lng - frskyData.hub.pilotLongitude : frskyData.hub.pilotLongitude - lng;
dist = frskyData.hub.distFromEarthAxis * angle / 1000000; dist = frskyData.hub.distFromEarthAxis * angle / 1000000;
result += dist*dist; result += dist*dist;
dist = abs(TELEMETRY_BARO_ALT_AVAILABLE() ? TELEMETRY_RELATIVE_BARO_ALT_BP : TELEMETRY_RELATIVE_GPS_ALT_BP); dist = abs(TELEMETRY_BARO_ALT_AVAILABLE() ? TELEMETRY_RELATIVE_BARO_ALT_BP : TELEMETRY_RELATIVE_GPS_ALT_BP);
result += dist*dist; result += dist*dist;
frskyData.hub.gpsDistance = isqrt32(result); frskyData.hub.gpsDistance = isqrt32(result);
if (frskyData.hub.gpsDistance > frskyData.hub.maxGpsDistance) if (frskyData.hub.gpsDistance > frskyData.hub.maxGpsDistance)
frskyData.hub.maxGpsDistance = frskyData.hub.gpsDistance; frskyData.hub.maxGpsDistance = frskyData.hub.gpsDistance;
} }
#endif #endif

5702
radio/src/opentx.cpp
View file

File diff suppressed because it is too large Load diff

936
radio/src/rtc.cpp
View file

@ -18,471 +18,471 @@
* GNU General Public License for more details. * GNU General Public License for more details.
*/ */
#include <limits.h> #include <limits.h>
#include "opentx.h" #include "opentx.h"
extern void rtcdriver_settime(struct gtm * t); extern void rtcdriver_settime(struct gtm * t);
#define LEAP_SECONDS_POSSIBLE 0 #define LEAP_SECONDS_POSSIBLE 0
/* Shift A right by B bits portably, by dividing A by 2**B and /* 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 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 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 INT_BITS is the number of useful bits in an int. GNU code can
assume that INT_BITS is at least 32. assume that INT_BITS is at least 32.
ISO C99 says that A >> B is implementation-defined if A < 0. Some 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 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 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. */ 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)) #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, /* The extra casts in the following macros work around compiler bugs,
e.g., in Cray C 5.0.3.0. */ e.g., in Cray C 5.0.3.0. */
/* True if the arithmetic type T is an integer type. bool counts as /* True if the arithmetic type T is an integer type. bool counts as
an integer. */ an integer. */
#define TYPE_IS_INTEGER(t) ((t) 1.5 == 1) #define TYPE_IS_INTEGER(t) ((t) 1.5 == 1)
/* True if negative values of the signed integer type T use two's /* True if negative values of the signed integer type T use two's
complement, ones' complement, or signed magnitude representation, complement, ones' complement, or signed magnitude representation,
respectively. Much GNU code assumes two's complement, but some respectively. Much GNU code assumes two's complement, but some
people like to be portable to all possible C hosts. */ people like to be portable to all possible C hosts. */
#define TYPE_TWOS_COMPLEMENT(t) ((t) ~ (t) 0 == (t) -1) #define TYPE_TWOS_COMPLEMENT(t) ((t) ~ (t) 0 == (t) -1)
#define TYPE_ONES_COMPLEMENT(t) ((t) ~ (t) 0 == 0) #define TYPE_ONES_COMPLEMENT(t) ((t) ~ (t) 0 == 0)
#define TYPE_SIGNED_MAGNITUDE(t) ((t) ~ (t) 0 < (t) -1) #define TYPE_SIGNED_MAGNITUDE(t) ((t) ~ (t) 0 < (t) -1)
/* True if the arithmetic type T is signed. */ /* True if the arithmetic type T is signed. */
#define TYPE_SIGNED(t) (! ((t) 0 < (t) -1)) #define TYPE_SIGNED(t) (! ((t) 0 < (t) -1))
/* The maximum and minimum values for the integer type T. These /* The maximum and minimum values for the integer type T. These
macros have undefined behavior if T is signed and has padding bits. 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 If this is a problem for you, please let us know how to fix it for
your host. */ your host. */
#define TYPE_MINIMUM(t) \ #define TYPE_MINIMUM(t) \
((t) (! TYPE_SIGNED (t) \ ((t) (! TYPE_SIGNED (t) \
? (t) 0 \ ? (t) 0 \
: TYPE_SIGNED_MAGNITUDE (t) \ : TYPE_SIGNED_MAGNITUDE (t) \
? ~ (t) 0 \ ? ~ (t) 0 \
: ~ (t) 0 << (sizeof (t) * CHAR_BIT - 1))) : ~ (t) 0 << (sizeof (t) * CHAR_BIT - 1)))
#define TYPE_MAXIMUM(t) \ #define TYPE_MAXIMUM(t) \
((t) (! TYPE_SIGNED (t) \ ((t) (! TYPE_SIGNED (t) \
? (t) -1 \ ? (t) -1 \
: ~ (~ (t) 0 << (sizeof (t) * CHAR_BIT - 1)))) : ~ (~ (t) 0 << (sizeof (t) * CHAR_BIT - 1))))
#ifndef TIME_T_MIN #ifndef TIME_T_MIN
# define TIME_T_MIN TYPE_MINIMUM (gtime_t) # define TIME_T_MIN TYPE_MINIMUM (gtime_t)
#endif #endif
#ifndef TIME_T_MAX #ifndef TIME_T_MAX
# define TIME_T_MAX TYPE_MAXIMUM (gtime_t) # define TIME_T_MAX TYPE_MAXIMUM (gtime_t)
#endif #endif
#define TIME_T_MIDPOINT (SHR (TIME_T_MIN + TIME_T_MAX, 1) + 1) #define TIME_T_MIDPOINT (SHR (TIME_T_MIN + TIME_T_MAX, 1) + 1)
/* Verify a requirement at compile-time (unlike assert, which is runtime). */ /* Verify a requirement at compile-time (unlike assert, which is runtime). */
#define verify(name, assertion) struct name { char a[(assertion) ? 1 : -1]; } #define verify(name, assertion) struct name { char a[(assertion) ? 1 : -1]; }
verify (gtime_t_is_integer, TYPE_IS_INTEGER (gtime_t)); verify (gtime_t_is_integer, TYPE_IS_INTEGER (gtime_t));
verify (twos_complement_arithmetic, TYPE_TWOS_COMPLEMENT (int)); verify (twos_complement_arithmetic, TYPE_TWOS_COMPLEMENT (int));
/* The code also assumes that signed integer overflow silently wraps /* The code also assumes that signed integer overflow silently wraps
around, but this assumption can't be stated without causing a around, but this assumption can't be stated without causing a
diagnostic on some hosts. */ diagnostic on some hosts. */
#define EPOCH_YEAR 1970 #define EPOCH_YEAR 1970
#define TM_YEAR_BASE 1900 #define TM_YEAR_BASE 1900
verify (base_year_is_a_multiple_of_100, TM_YEAR_BASE % 100 == 0); verify (base_year_is_a_multiple_of_100, TM_YEAR_BASE % 100 == 0);
/* Return 1 if YEAR + TM_YEAR_BASE is a leap year. */ /* Return 1 if YEAR + TM_YEAR_BASE is a leap year. */
static inline int static inline int
leapyear (long int year) leapyear (long int year)
{ {
/* Don't add YEAR to TM_YEAR_BASE, as that might overflow. /* Don't add YEAR to TM_YEAR_BASE, as that might overflow.
Also, work even if YEAR is negative. */ Also, work even if YEAR is negative. */
return return
((year & 3) == 0 ((year & 3) == 0
&& (year % 100 != 0 && (year % 100 != 0
|| ((year / 100) & 3) == (- (TM_YEAR_BASE / 100) & 3))); || ((year / 100) & 3) == (- (TM_YEAR_BASE / 100) & 3)));
} }
const unsigned short int __mon_yday[2][13] = const unsigned short int __mon_yday[2][13] =
{ {
/* Normal years. */ /* Normal years. */
{ 0, 31, 59, 90, 120, 151, 181, 212, 243, 273, 304, 334, 365 }, { 0, 31, 59, 90, 120, 151, 181, 212, 243, 273, 304, 334, 365 },
/* Leap years. */ /* Leap years. */
{ 0, 31, 60, 91, 121, 152, 182, 213, 244, 274, 305, 335, 366 } { 0, 31, 60, 91, 121, 152, 182, 213, 244, 274, 305, 335, 366 }
}; };
/* Compute the `struct tm' representation of *T, /* Compute the `struct tm' representation of *T,
offset OFFSET seconds east of UTC, offset OFFSET seconds east of UTC,
and store year, yday, mon, mday, wday, hour, min, sec into *TP. and store year, yday, mon, mday, wday, hour, min, sec into *TP.
Return nonzero if successful. */ Return nonzero if successful. */
int int
__offtime ( __offtime (
gtime_t *t, gtime_t *t,
long int offset, long int offset,
struct gtm *tp) struct gtm *tp)
{ {
long int days, rem, y; long int days, rem, y;
const unsigned short int *ip; const unsigned short int *ip;
days = *t / SECS_PER_DAY; days = *t / SECS_PER_DAY;
rem = *t % SECS_PER_DAY; rem = *t % SECS_PER_DAY;
rem += offset; rem += offset;
while (rem < 0) while (rem < 0)
{ {
rem += SECS_PER_DAY; rem += SECS_PER_DAY;
--days; --days;
} }
while (rem >= (long int)SECS_PER_DAY) while (rem >= (long int)SECS_PER_DAY)
{ {
rem -= SECS_PER_DAY; rem -= SECS_PER_DAY;
++days; ++days;
} }
tp->tm_hour = rem / SECS_PER_HOUR; tp->tm_hour = rem / SECS_PER_HOUR;
rem %= SECS_PER_HOUR; rem %= SECS_PER_HOUR;
tp->tm_min = rem / 60; tp->tm_min = rem / 60;
tp->tm_sec = rem % 60; tp->tm_sec = rem % 60;
/* January 1, 1970 was a Thursday. */ /* January 1, 1970 was a Thursday. */
tp->tm_wday = (4 + days) % 7; tp->tm_wday = (4 + days) % 7;
if (tp->tm_wday < 0) if (tp->tm_wday < 0)
tp->tm_wday += 7; tp->tm_wday += 7;
y = 1970; y = 1970;
#define DIV(a, b) ((a) / (b) - ((a) % (b) < 0)) #define DIV(a, b) ((a) / (b) - ((a) % (b) < 0))
#define LEAPS_THRU_END_OF(y) (DIV (y, 4) - DIV (y, 100) + DIV (y, 400)) #define LEAPS_THRU_END_OF(y) (DIV (y, 4) - DIV (y, 100) + DIV (y, 400))
while (days < 0 || days >= (leapyear (y) ? 366 : 365)) while (days < 0 || days >= (leapyear (y) ? 366 : 365))
{ {
/* Guess a corrected year, assuming 365 days per year. */ /* Guess a corrected year, assuming 365 days per year. */
long int yg = y + days / 365 - (days % 365 < 0); long int yg = y + days / 365 - (days % 365 < 0);
/* Adjust DAYS and Y to match the guessed year. */ /* Adjust DAYS and Y to match the guessed year. */
days -= ((yg - y) * 365 days -= ((yg - y) * 365
+ LEAPS_THRU_END_OF (yg - 1) + LEAPS_THRU_END_OF (yg - 1)
- LEAPS_THRU_END_OF (y - 1)); - LEAPS_THRU_END_OF (y - 1));
y = yg; y = yg;
} }
tp->tm_year = y - 1900; tp->tm_year = y - 1900;
if (tp->tm_year != y - 1900) if (tp->tm_year != y - 1900)
{ {
/* The year cannot be represented due to overflow. */ /* The year cannot be represented due to overflow. */
// __set_errno (EOVERFLOW); // __set_errno (EOVERFLOW);
return 0; return 0;
} }
tp->tm_yday = days; tp->tm_yday = days;
ip = __mon_yday[leapyear(y)]; ip = __mon_yday[leapyear(y)];
for (y = 11; days < (long int) ip[y]; --y) for (y = 11; days < (long int) ip[y]; --y)
continue; continue;
days -= ip[y]; days -= ip[y];
tp->tm_mon = y; tp->tm_mon = y;
tp->tm_mday = days + 1; tp->tm_mday = days + 1;
return 1; return 1;
} }
/* time_r function implementations */ /* time_r function implementations */
// G: No time zones in our implementation so just do the converion from gtime_t to struct tm // G: No time zones in our implementation so just do the converion from gtime_t to struct tm
struct gtm * struct gtm *
__localtime_r (gtime_t * t, struct gtm * tp) __localtime_r (gtime_t * t, struct gtm * tp)
{ {
__offtime(t, 0, tp); __offtime(t, 0, tp);
return tp; return tp;
} }
/* Return an integer value measuring (YEAR1-YDAY1 HOUR1:MIN1:SEC1) - /* Return an integer value measuring (YEAR1-YDAY1 HOUR1:MIN1:SEC1) -
(YEAR0-YDAY0 HOUR0:MIN0:SEC0) in seconds, assuming that the clocks (YEAR0-YDAY0 HOUR0:MIN0:SEC0) in seconds, assuming that the clocks
were not adjusted between the time stamps. were not adjusted between the time stamps.
The YEAR values uses the same numbering as TP->tm_year. Values 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 need not be in the usual range. However, YEAR1 must not be less
than 2 * INT_MIN or greater than 2 * INT_MAX. than 2 * INT_MIN or greater than 2 * INT_MAX.
The result may overflow. It is the caller's responsibility to The result may overflow. It is the caller's responsibility to
detect overflow. */ detect overflow. */
static inline gtime_t static inline gtime_t
ydhms_diff (long int year1, long int yday1, int hour1, int min1, int sec1, ydhms_diff (long int year1, long int yday1, int hour1, int min1, int sec1,
int year0, int yday0, int hour0, int min0, int sec0) int year0, int yday0, int hour0, int min0, int sec0)
{ {
verify (C99_integer_division, -1 / 2 == 0); verify (C99_integer_division, -1 / 2 == 0);
verify (long_int_year_and_yday_are_wide_enough, verify (long_int_year_and_yday_are_wide_enough,
INT_MAX <= LONG_MAX / 2 || TIME_T_MAX <= UINT_MAX); INT_MAX <= LONG_MAX / 2 || TIME_T_MAX <= UINT_MAX);
/* Compute intervening leap days correctly even if year is negative. /* Compute intervening leap days correctly even if year is negative.
Take care to avoid integer overflow here. */ Take care to avoid integer overflow here. */
int a4 = SHR (year1, 2) + SHR (TM_YEAR_BASE, 2) - ! (year1 & 3); 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 b4 = SHR (year0, 2) + SHR (TM_YEAR_BASE, 2) - ! (year0 & 3);
int a100 = a4 / 25 - (a4 % 25 < 0); int a100 = a4 / 25 - (a4 % 25 < 0);
int b100 = b4 / 25 - (b4 % 25 < 0); int b100 = b4 / 25 - (b4 % 25 < 0);
int a400 = SHR (a100, 2); int a400 = SHR (a100, 2);
int b400 = SHR (b100, 2); int b400 = SHR (b100, 2);
int intervening_leap_days = (a4 - b4) - (a100 - b100) + (a400 - b400); int intervening_leap_days = (a4 - b4) - (a100 - b100) + (a400 - b400);
/* Compute the desired time in gtime_t precision. Overflow might /* Compute the desired time in gtime_t precision. Overflow might
occur here. */ occur here. */
gtime_t tyear1 = year1; gtime_t tyear1 = year1;
gtime_t years = tyear1 - year0; gtime_t years = tyear1 - year0;
gtime_t days = 365 * years + yday1 - yday0 + intervening_leap_days; gtime_t days = 365 * years + yday1 - yday0 + intervening_leap_days;
gtime_t hours = 24 * days + hour1 - hour0; gtime_t hours = 24 * days + hour1 - hour0;
gtime_t minutes = 60 * hours + min1 - min0; gtime_t minutes = 60 * hours + min1 - min0;
gtime_t seconds = 60 * minutes + sec1 - sec0; gtime_t seconds = 60 * minutes + sec1 - sec0;
return seconds; return seconds;
} }
/* Return a gtime_t value corresponding to (YEAR-YDAY HOUR:MIN:SEC), /* Return a gtime_t value corresponding to (YEAR-YDAY HOUR:MIN:SEC),
assuming that *T corresponds to *TP and that no clock adjustments assuming that *T corresponds to *TP and that no clock adjustments
occurred between *TP and the desired time. occurred between *TP and the desired time.
If TP is null, return a value not equal to *T; this avoids false matches. 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 If overflow occurs, yield the minimal or maximal value, except do not
yield a value equal to *T. */ yield a value equal to *T. */
static gtime_t static gtime_t
guess_time_tm (long int year, long int yday, int hour, int min, int sec, guess_time_tm (long int year, long int yday, int hour, int min, int sec,
gtime_t *t, struct gtm *tp) gtime_t *t, struct gtm *tp)
{ {
if (tp) if (tp)
{ {
gtime_t d = ydhms_diff (year, yday, hour, min, sec, gtime_t d = ydhms_diff (year, yday, hour, min, sec,
tp->tm_year, tp->tm_yday, tp->tm_year, tp->tm_yday,
tp->tm_hour, tp->tm_min, tp->tm_sec); tp->tm_hour, tp->tm_min, tp->tm_sec);
gtime_t t1 = *t + d; gtime_t t1 = *t + d;
if ((t1 < *t) == (TYPE_SIGNED (gtime_t) ? d < 0 : TIME_T_MAX / 2 < d)) if ((t1 < *t) == (TYPE_SIGNED (gtime_t) ? d < 0 : TIME_T_MAX / 2 < d))
return t1; return t1;
} }
/* Overflow occurred one way or another. Return the nearest result /* Overflow occurred one way or another. Return the nearest result
that is actually in range, except don't report a zero difference that is actually in range, except don't report a zero difference
if the actual difference is nonzero, as that would cause a false if the actual difference is nonzero, as that would cause a false
match; and don't oscillate between two values, as that would match; and don't oscillate between two values, as that would
confuse the spring-forward gap detector. */ confuse the spring-forward gap detector. */
return (*t < TIME_T_MIDPOINT return (*t < TIME_T_MIDPOINT
? (*t <= TIME_T_MIN + 1 ? *t + 1 : TIME_T_MIN) ? (*t <= TIME_T_MIN + 1 ? *t + 1 : TIME_T_MIN)
: (TIME_T_MAX - 1 <= *t ? *t - 1 : TIME_T_MAX)); : (TIME_T_MAX - 1 <= *t ? *t - 1 : TIME_T_MAX));
} }
/* Use CONVERT to convert *T to a broken down time in *TP. /* Use CONVERT to convert *T to a broken down time in *TP.
If *T is out of range for conversion, adjust it so that If *T is out of range for conversion, adjust it so that
it is the nearest in-range value and then convert that. */ it is the nearest in-range value and then convert that. */
static struct gtm * static struct gtm *
ranged_convert (struct gtm *(*convert) (gtime_t *, struct gtm *), ranged_convert (struct gtm *(*convert) (gtime_t *, struct gtm *),
gtime_t *t, struct gtm *tp) gtime_t *t, struct gtm *tp)
{ {
struct gtm *r = convert (t, tp); struct gtm *r = convert (t, tp);
if (!r && *t) if (!r && *t)
{ {
gtime_t bad = *t; gtime_t bad = *t;
gtime_t ok = 0; gtime_t ok = 0;
/* BAD is a known unconvertible gtime_t, and OK is a known good one. /* 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 Use binary search to narrow the range between BAD and OK until
they differ by 1. */ they differ by 1. */
while (bad != ok + (bad < 0 ? -1 : 1)) while (bad != ok + (bad < 0 ? -1 : 1))
{ {
gtime_t mid = *t = (bad < 0 gtime_t mid = *t = (bad < 0
? bad + ((ok - bad) >> 1) ? bad + ((ok - bad) >> 1)
: ok + ((bad - ok) >> 1)); : ok + ((bad - ok) >> 1));
r = convert (t, tp); r = convert (t, tp);
if (r) if (r)
ok = mid; ok = mid;
else else
bad = mid; bad = mid;
} }
if (!r && ok) if (!r && ok)
{ {
/* The last conversion attempt failed; /* The last conversion attempt failed;
revert to the most recent successful attempt. */ revert to the most recent successful attempt. */
*t = ok; *t = ok;
r = convert (t, tp); r = convert (t, tp);
} }
} }
return r; return r;
} }
/* Convert *TP to a gtime_t value, inverting /* Convert *TP to a gtime_t value, inverting
the monotonic and mostly-unit-linear conversion function CONVERT. the monotonic and mostly-unit-linear conversion function CONVERT.
Use *OFFSET to keep track of a guess at the offset of the result, 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. compared to what the result would be for UTC without leap seconds.
If *OFFSET's guess is correct, only one CONVERT call is needed. If *OFFSET's guess is correct, only one CONVERT call is needed.
This function is external because it is used also by timegm.c. */ This function is external because it is used also by timegm.c. */
gtime_t gtime_t
__mktime_internal (struct gtm *tp, __mktime_internal (struct gtm *tp,
struct gtm *(*convert) (gtime_t *, struct gtm *), struct gtm *(*convert) (gtime_t *, struct gtm *),
gtime_t *offset) gtime_t *offset)
{ {
gtime_t t, gt, t0, t1, t2; gtime_t t, gt, t0, t1, t2;
struct gtm tm; struct gtm tm;
/* The maximum number of probes (calls to CONVERT) should be enough /* The maximum number of probes (calls to CONVERT) should be enough
to handle any combinations of time zone rule changes, solar time, to handle any combinations of time zone rule changes, solar time,
leap seconds, and oscillations around a spring-forward gap. leap seconds, and oscillations around a spring-forward gap.
POSIX.1 prohibits leap seconds, but some hosts have them anyway. */ POSIX.1 prohibits leap seconds, but some hosts have them anyway. */
int remaining_probes = 6; int remaining_probes = 6;
/* Time requested. Copy it in case CONVERT modifies *TP; this can /* Time requested. Copy it in case CONVERT modifies *TP; this can
occur if TP is localtime's returned value and CONVERT is localtime. */ occur if TP is localtime's returned value and CONVERT is localtime. */
int sec = tp->tm_sec; int sec = tp->tm_sec;
int min = tp->tm_min; int min = tp->tm_min;
int hour = tp->tm_hour; int hour = tp->tm_hour;
int mday = tp->tm_mday; int mday = tp->tm_mday;
int mon = tp->tm_mon; int mon = tp->tm_mon;
int year_requested = tp->tm_year; int year_requested = tp->tm_year;
/* Ensure that mon is in range, and set year accordingly. */ /* Ensure that mon is in range, and set year accordingly. */
int mon_remainder = mon % 12; int mon_remainder = mon % 12;
int negative_mon_remainder = mon_remainder < 0; int negative_mon_remainder = mon_remainder < 0;
int mon_years = mon / 12 - negative_mon_remainder; int mon_years = mon / 12 - negative_mon_remainder;
long int lyear_requested = year_requested; long int lyear_requested = year_requested;
long int year = lyear_requested + mon_years; long int year = lyear_requested + mon_years;
/* The other values need not be in range: /* The other values need not be in range:
the remaining code handles minor overflows correctly, the remaining code handles minor overflows correctly,
assuming int and gtime_t arithmetic wraps around. assuming int and gtime_t arithmetic wraps around.
Major overflows are caught at the end. */ Major overflows are caught at the end. */
/* Calculate day of year from year, month, and day of month. /* Calculate day of year from year, month, and day of month.
The result need not be in range. */ The result need not be in range. */
int mon_yday = ((__mon_yday[leapyear (year)] int mon_yday = ((__mon_yday[leapyear (year)]
[mon_remainder + 12 * negative_mon_remainder]) [mon_remainder + 12 * negative_mon_remainder])
- 1); - 1);
long int lmday = mday; long int lmday = mday;
long int yday = mon_yday + lmday; long int yday = mon_yday + lmday;
gtime_t guessed_offset = *offset; gtime_t guessed_offset = *offset;
int sec_requested = sec; int sec_requested = sec;
/* /*
if (LEAP_SECONDS_POSSIBLE) if (LEAP_SECONDS_POSSIBLE)
{ {
// Handle out-of-range seconds specially, // Handle out-of-range seconds specially,
// since ydhms_tm_diff assumes every minute has 60 seconds. // since ydhms_tm_diff assumes every minute has 60 seconds.
if (sec < 0) if (sec < 0)
sec = 0; sec = 0;
if (59 < sec) if (59 < sec)
sec = 59; sec = 59;
} }
*/ */
/* Invert CONVERT by probing. First assume the same offset as last /* Invert CONVERT by probing. First assume the same offset as last
time. */ time. */
t0 = ydhms_diff (year, yday, hour, min, sec, t0 = ydhms_diff (year, yday, hour, min, sec,
EPOCH_YEAR - TM_YEAR_BASE, 0, 0, 0, - guessed_offset); EPOCH_YEAR - TM_YEAR_BASE, 0, 0, 0, - guessed_offset);
if (TIME_T_MAX / INT_MAX / 366 / 24 / 60 / 60 < 3) if (TIME_T_MAX / INT_MAX / 366 / 24 / 60 / 60 < 3)
{ {
/* gtime_t isn't large enough to rule out overflows, so check /* gtime_t isn't large enough to rule out overflows, so check
for major overflows. A gross check suffices, since if t0 for major overflows. A gross check suffices, since if t0
has overflowed, it is off by a multiple of TIME_T_MAX - has overflowed, it is off by a multiple of TIME_T_MAX -
TIME_T_MIN + 1. So ignore any component of the difference TIME_T_MIN + 1. So ignore any component of the difference
that is bounded by a small value. */ that is bounded by a small value. */
/* Approximate log base 2 of the number of time units per /* Approximate log base 2 of the number of time units per
biennium. A biennium is 2 years; use this unit instead of biennium. A biennium is 2 years; use this unit instead of
years to avoid integer overflow. For example, 2 average years to avoid integer overflow. For example, 2 average
Gregorian years are 2 * 365.2425 * 24 * 60 * 60 seconds, Gregorian years are 2 * 365.2425 * 24 * 60 * 60 seconds,
which is 63113904 seconds, and rint (log2 (63113904)) is which is 63113904 seconds, and rint (log2 (63113904)) is
26. */ 26. */
int ALOG2_SECONDS_PER_BIENNIUM = 26; int ALOG2_SECONDS_PER_BIENNIUM = 26;
int ALOG2_MINUTES_PER_BIENNIUM = 20; int ALOG2_MINUTES_PER_BIENNIUM = 20;
int ALOG2_HOURS_PER_BIENNIUM = 14; int ALOG2_HOURS_PER_BIENNIUM = 14;
int ALOG2_DAYS_PER_BIENNIUM = 10; int ALOG2_DAYS_PER_BIENNIUM = 10;
int LOG2_YEARS_PER_BIENNIUM = 1; int LOG2_YEARS_PER_BIENNIUM = 1;
int approx_requested_biennia = int approx_requested_biennia =
(SHR (year_requested, LOG2_YEARS_PER_BIENNIUM) (SHR (year_requested, LOG2_YEARS_PER_BIENNIUM)
- SHR (EPOCH_YEAR - TM_YEAR_BASE, LOG2_YEARS_PER_BIENNIUM) - SHR (EPOCH_YEAR - TM_YEAR_BASE, LOG2_YEARS_PER_BIENNIUM)
+ SHR (mday, ALOG2_DAYS_PER_BIENNIUM) + SHR (mday, ALOG2_DAYS_PER_BIENNIUM)
+ SHR (hour, ALOG2_HOURS_PER_BIENNIUM) + SHR (hour, ALOG2_HOURS_PER_BIENNIUM)
+ SHR (min, ALOG2_MINUTES_PER_BIENNIUM) + SHR (min, ALOG2_MINUTES_PER_BIENNIUM)
+ (LEAP_SECONDS_POSSIBLE + (LEAP_SECONDS_POSSIBLE
? 0 ? 0
: SHR (sec, ALOG2_SECONDS_PER_BIENNIUM))); : SHR (sec, ALOG2_SECONDS_PER_BIENNIUM)));
int approx_biennia = SHR (t0, ALOG2_SECONDS_PER_BIENNIUM); int approx_biennia = SHR (t0, ALOG2_SECONDS_PER_BIENNIUM);
int diff = approx_biennia - approx_requested_biennia; int diff = approx_biennia - approx_requested_biennia;
int abs_diff = diff < 0 ? - diff : diff; int abs_diff = diff < 0 ? - diff : diff;
/* IRIX 4.0.5 cc miscalculates TIME_T_MIN / 3: it erroneously /* IRIX 4.0.5 cc miscalculates TIME_T_MIN / 3: it erroneously
gives a positive value of 715827882. Setting a variable gives a positive value of 715827882. Setting a variable
first then doing math on it seems to work. first then doing math on it seems to work.
(ghazi@caip.rutgers.edu) */ (ghazi@caip.rutgers.edu) */
gtime_t time_t_max = TIME_T_MAX; gtime_t time_t_max = TIME_T_MAX;
gtime_t time_t_min = TIME_T_MIN; gtime_t time_t_min = TIME_T_MIN;
gtime_t overflow_threshold = gtime_t overflow_threshold =
(time_t_max / 3 - time_t_min / 3) >> ALOG2_SECONDS_PER_BIENNIUM; (time_t_max / 3 - time_t_min / 3) >> ALOG2_SECONDS_PER_BIENNIUM;
if (overflow_threshold < abs_diff) if (overflow_threshold < abs_diff)
{ {
/* Overflow occurred. Try repairing it; this might work if /* Overflow occurred. Try repairing it; this might work if
the time zone offset is enough to undo the overflow. */ the time zone offset is enough to undo the overflow. */
gtime_t repaired_t0 = -1 - t0; gtime_t repaired_t0 = -1 - t0;
approx_biennia = SHR (repaired_t0, ALOG2_SECONDS_PER_BIENNIUM); approx_biennia = SHR (repaired_t0, ALOG2_SECONDS_PER_BIENNIUM);
diff = approx_biennia - approx_requested_biennia; diff = approx_biennia - approx_requested_biennia;
abs_diff = diff < 0 ? - diff : diff; abs_diff = diff < 0 ? - diff : diff;
if (overflow_threshold < abs_diff) if (overflow_threshold < abs_diff)
return -1; return -1;
guessed_offset += repaired_t0 - t0; guessed_offset += repaired_t0 - t0;
t0 = repaired_t0; t0 = repaired_t0;
} }
} }
/* Repeatedly use the error to improve the guess. */ /* Repeatedly use the error to improve the guess. */
for (t = t1 = t2 = t0; for (t = t1 = t2 = t0;
(gt = guess_time_tm (year, yday, hour, min, sec, &t, (gt = guess_time_tm (year, yday, hour, min, sec, &t,
ranged_convert (convert, &t, &tm)), ranged_convert (convert, &t, &tm)),
t != gt); t != gt);
t1 = t2, t2 = t, t = gt) t1 = t2, t2 = t, t = gt)
if (t == t1 && t != t2) if (t == t1 && t != t2)
goto offset_found; goto offset_found;
else if (--remaining_probes == 0) else if (--remaining_probes == 0)
return -1; return -1;
offset_found: offset_found:
*offset = guessed_offset + t - t0; *offset = guessed_offset + t - t0;
if (LEAP_SECONDS_POSSIBLE && sec_requested != tm.tm_sec) if (LEAP_SECONDS_POSSIBLE && sec_requested != tm.tm_sec)
{ {
/* Adjust time to reflect the tm_sec requested, not the normalized value. /* 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. */ Also, repair any damage from a false match due to a leap second. */
int sec_adjustment = (sec == 0 && tm.tm_sec == 60) - sec; int sec_adjustment = (sec == 0 && tm.tm_sec == 60) - sec;
t1 = t + sec_requested; t1 = t + sec_requested;
t2 = t1 + sec_adjustment; t2 = t1 + sec_adjustment;
if (((t1 < t) != (sec_requested < 0)) if (((t1 < t) != (sec_requested < 0))
| ((t2 < t1) != (sec_adjustment < 0)) | ((t2 < t1) != (sec_adjustment < 0))
| ! convert (&t2, &tm)) | ! convert (&t2, &tm))
return -1; return -1;
t = t2; t = t2;
} }
*tp = tm; *tp = tm;
return t; return t;
} }
/* Convert *TP to a gtime_t value. */ /* Convert *TP to a gtime_t value. */
gtime_t gtime_t
gmktime (struct gtm *tp) gmktime (struct gtm *tp)
{ {
// no time zone stuff. Just do the math ;) // no time zone stuff. Just do the math ;)
static gtime_t localtime_offset; static gtime_t localtime_offset;
return __mktime_internal (tp, __localtime_r, &localtime_offset); return __mktime_internal (tp, __localtime_r, &localtime_offset);
} }
/* Fill a (struct tm) TP* from a given gtime_t time stamp */ /* Fill a (struct tm) TP* from a given gtime_t time stamp */
gtime_t gtime_t
filltm(gtime_t *t, struct gtm *tp) filltm(gtime_t *t, struct gtm *tp)
{ {
return __offtime(t, 0, tp); return __offtime(t, 0, tp);
} }
gtime_t g_rtcTime; gtime_t g_rtcTime;
uint8_t g_ms100 = 0; // global to allow time set function to reset to zero uint8_t g_ms100 = 0; // global to allow time set function to reset to zero
void gettime(struct gtm * tm) void gettime(struct gtm * tm)
{ {
filltm(&g_rtcTime, tm); // create a struct tm date/time structure from global unix time stamp filltm(&g_rtcTime, tm); // create a struct tm date/time structure from global unix time stamp
} }

74
radio/src/serial.cpp
View file

@ -18,40 +18,40 @@
* GNU General Public License for more details. * GNU General Public License for more details.
*/ */
#include "opentx.h" #include "opentx.h"
#include "serial.h" #include "serial.h"
#include <stdarg.h> #include <stdarg.h>
#include <stdio.h> #include <stdio.h>
#define PRINTF_BUFFER_SIZE 128 #define PRINTF_BUFFER_SIZE 128
void serialPutc(char c) void serialPutc(char c)
{ {
#if defined(USB_SERIAL) #if defined(USB_SERIAL)
usbSerialPutc(c); usbSerialPutc(c);
#else #else
serial2Putc(c); serial2Putc(c);
#endif #endif
} }
void serialPrintf(const char * format, ...) void serialPrintf(const char * format, ...)
{ {
va_list arglist; va_list arglist;
char tmp[PRINTF_BUFFER_SIZE+1]; char tmp[PRINTF_BUFFER_SIZE+1];
va_start(arglist, format); va_start(arglist, format);
vsnprintf(tmp, PRINTF_BUFFER_SIZE, format, arglist); vsnprintf(tmp, PRINTF_BUFFER_SIZE, format, arglist);
tmp[PRINTF_BUFFER_SIZE] = '\0'; tmp[PRINTF_BUFFER_SIZE] = '\0';
va_end(arglist); va_end(arglist);
const char *t = tmp; const char *t = tmp;
while (*t) { while (*t) {
serialPutc(*t++); serialPutc(*t++);
} }
} }
void serialCrlf() void serialCrlf()
{ {
serialPutc('\r'); serialPutc('\r');
serialPutc('\n'); serialPutc('\n');
} }

2228
radio/src/switches.cpp
View file

File diff suppressed because it is too large Load diff

418
radio/src/tasks_arm.cpp
View file

@ -1,209 +1,209 @@
/* /*
* Copyright (C) OpenTX * Copyright (C) OpenTX
* *
* Based on code named * Based on code named
* th9x - http://code.google.com/p/th9x * th9x - http://code.google.com/p/th9x
* er9x - http://code.google.com/p/er9x * er9x - http://code.google.com/p/er9x
* gruvin9x - http://code.google.com/p/gruvin9x * gruvin9x - http://code.google.com/p/gruvin9x
* *
* License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
* *
* This program is free software; you can redistribute it and/or modify * 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 * it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation. * published by the Free Software Foundation.
* *
* This program is distributed in the hope that it will be useful, * This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of * but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details. * GNU General Public License for more details.
*/ */
#include "opentx.h" #include "opentx.h"
OS_TID menusTaskId; OS_TID menusTaskId;
// menus stack must be aligned to 8 bytes otherwise printf for %f does not work! // menus stack must be aligned to 8 bytes otherwise printf for %f does not work!
TaskStack<MENUS_STACK_SIZE> _ALIGNED(8) menusStack; TaskStack<MENUS_STACK_SIZE> _ALIGNED(8) menusStack;
OS_TID mixerTaskId; OS_TID mixerTaskId;
TaskStack<MIXER_STACK_SIZE> mixerStack; TaskStack<MIXER_STACK_SIZE> mixerStack;
OS_TID audioTaskId; OS_TID audioTaskId;
TaskStack<AUDIO_STACK_SIZE> audioStack; TaskStack<AUDIO_STACK_SIZE> audioStack;
#if defined(BLUETOOTH) #if defined(BLUETOOTH)
OS_TID btTaskId; OS_TID btTaskId;
TaskStack<BLUETOOTH_STACK_SIZE> bluetoothStack; TaskStack<BLUETOOTH_STACK_SIZE> bluetoothStack;
#endif #endif
OS_MutexID audioMutex; OS_MutexID audioMutex;
OS_MutexID mixerMutex; OS_MutexID mixerMutex;
enum TaskIndex { enum TaskIndex {
MENU_TASK_INDEX, MENU_TASK_INDEX,
MIXER_TASK_INDEX, MIXER_TASK_INDEX,
AUDIO_TASK_INDEX, AUDIO_TASK_INDEX,
CLI_TASK_INDEX, CLI_TASK_INDEX,
BLUETOOTH_TASK_INDEX, BLUETOOTH_TASK_INDEX,
TASK_INDEX_COUNT, TASK_INDEX_COUNT,
MAIN_TASK_INDEX = 255 MAIN_TASK_INDEX = 255
}; };
template<int SIZE> template<int SIZE>
void TaskStack<SIZE>::paint() void TaskStack<SIZE>::paint()
{ {
for (uint32_t i=0; i<SIZE; i++) { for (uint32_t i=0; i<SIZE; i++) {
stack[i] = 0x55555555; stack[i] = 0x55555555;
} }
} }
uint16_t getStackAvailable(void * address, uint16_t size) uint16_t getStackAvailable(void * address, uint16_t size)
{ {
uint32_t * array = (uint32_t *)address; uint32_t * array = (uint32_t *)address;
uint16_t i = 0; uint16_t i = 0;
while (i < size && array[i] == 0x55555555) { while (i < size && array[i] == 0x55555555) {
i++; i++;
} }
return i*4; return i*4;
#if defined(CLI) #if defined(CLI)
cliStackPaint(); cliStackPaint();
#endif #endif
} }
void stackPaint() void stackPaint()
{ {
menusStack.paint(); menusStack.paint();
mixerStack.paint(); mixerStack.paint();
audioStack.paint(); audioStack.paint();
#if defined(CLI) #if defined(CLI)
cliStack.paint(); cliStack.paint();
#endif #endif
} }
#if defined(CPUSTM32) && !defined(SIMU) #if defined(CPUSTM32) && !defined(SIMU)
uint16_t stackSize() uint16_t stackSize()
{ {
return ((unsigned char *)&_estack - (unsigned char *)&_main_stack_start) / 4; return ((unsigned char *)&_estack - (unsigned char *)&_main_stack_start) / 4;
} }
uint16_t stackAvailable() uint16_t stackAvailable()
{ {
return getStackAvailable(&_main_stack_start, stackSize()); return getStackAvailable(&_main_stack_start, stackSize());
} }
#endif #endif
void mixerTask(void * pdata) void mixerTask(void * pdata)
{ {
s_pulses_paused = true; s_pulses_paused = true;
while(1) { while(1) {
#if defined(SIMU) #if defined(SIMU)
if (main_thread_running == 0) if (main_thread_running == 0)
return; return;
#endif #endif
if (!s_pulses_paused) { if (!s_pulses_paused) {
uint16_t t0 = getTmr2MHz(); uint16_t t0 = getTmr2MHz();
CoEnterMutexSection(mixerMutex); CoEnterMutexSection(mixerMutex);
doMixerCalculations(); doMixerCalculations();
CoLeaveMutexSection(mixerMutex); CoLeaveMutexSection(mixerMutex);
#if defined(FRSKY) || defined(MAVLINK) #if defined(FRSKY) || defined(MAVLINK)
telemetryWakeup(); telemetryWakeup();
#endif #endif
if (heartbeat == HEART_WDT_CHECK) { if (heartbeat == HEART_WDT_CHECK) {
wdt_reset(); wdt_reset();
heartbeat = 0; heartbeat = 0;
} }
t0 = getTmr2MHz() - t0; t0 = getTmr2MHz() - t0;
if (t0 > maxMixerDuration) maxMixerDuration = t0 ; if (t0 > maxMixerDuration) maxMixerDuration = t0 ;
} }
CoTickDelay(1); // 2ms for now CoTickDelay(1); // 2ms for now
} }
} }
#define MENU_TASK_PERIOD_TICKS 10 // 20ms #define MENU_TASK_PERIOD_TICKS 10 // 20ms
void menusTask(void * pdata) void menusTask(void * pdata)
{ {
opentxInit(); opentxInit();
#if defined(PWR_BUTTON_DELAY) #if defined(PWR_BUTTON_DELAY)
while (1) { while (1) {
uint32_t pwr_check = pwrCheck(); uint32_t pwr_check = pwrCheck();
if (pwr_check == e_power_off) { if (pwr_check == e_power_off) {
break; break;
} }
else if (pwr_check == e_power_press) { else if (pwr_check == e_power_press) {
CoTickDelay(MENU_TASK_PERIOD_TICKS); CoTickDelay(MENU_TASK_PERIOD_TICKS);
continue; continue;
} }
#else #else
while (pwrCheck() != e_power_off) { while (pwrCheck() != e_power_off) {
#endif #endif
U64 start = CoGetOSTime(); U64 start = CoGetOSTime();
perMain(); perMain();
// TODO remove completely massstorage from sky9x firmware // TODO remove completely massstorage from sky9x firmware
U32 runtime = (U32)(CoGetOSTime() - start); U32 runtime = (U32)(CoGetOSTime() - start);
// deduct the thread run-time from the wait, if run-time was more than // deduct the thread run-time from the wait, if run-time was more than
// desired period, then skip the wait all together // desired period, then skip the wait all together
if (runtime < MENU_TASK_PERIOD_TICKS) { if (runtime < MENU_TASK_PERIOD_TICKS) {
CoTickDelay(MENU_TASK_PERIOD_TICKS - runtime); CoTickDelay(MENU_TASK_PERIOD_TICKS - runtime);
} }
#if defined(SIMU) #if defined(SIMU)
if (main_thread_running == 0) if (main_thread_running == 0)
return; return;
#endif #endif
} }
#if defined(PCBTARANIS) && defined(REV9E) #if defined(PCBTARANIS) && defined(REV9E)
toplcdOff(); toplcdOff();
#endif #endif
BACKLIGHT_OFF(); BACKLIGHT_OFF();
#if defined(PCBHORUS) #if defined(PCBHORUS)
ledOff(); ledOff();
#endif #endif
#if defined(COLORLCD) || defined(PCBTARANIS) #if defined(COLORLCD) || defined(PCBTARANIS)
drawSleepBitmap(); drawSleepBitmap();
#else #else
lcdClear(); lcdClear();
displayPopup(STR_SHUTDOWN); displayPopup(STR_SHUTDOWN);
#endif #endif
opentxClose(); opentxClose();
boardOff(); // Only turn power off if necessary boardOff(); // Only turn power off if necessary
} }
extern void audioTask(void* pdata); extern void audioTask(void* pdata);
void tasksStart() void tasksStart()
{ {
CoInitOS(); CoInitOS();
#if defined(CLI) #if defined(CLI)
cliStart(); cliStart();
#endif #endif
#if defined(BLUETOOTH) #if defined(BLUETOOTH)
btTaskId = CoCreateTask(btTask, NULL, 15, &bluetoothStack.stack[BLUETOOTH_STACK_SIZE-1], BLUETOOTH_STACK_SIZE); btTaskId = CoCreateTask(btTask, NULL, 15, &bluetoothStack.stack[BLUETOOTH_STACK_SIZE-1], BLUETOOTH_STACK_SIZE);
#endif #endif
mixerTaskId = CoCreateTask(mixerTask, NULL, 5, &mixerStack.stack[MIXER_STACK_SIZE-1], MIXER_STACK_SIZE); 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); menusTaskId = CoCreateTask(menusTask, NULL, 10, &menusStack.stack[MENUS_STACK_SIZE-1], MENUS_STACK_SIZE);
#if !defined(SIMU) #if !defined(SIMU)
// TODO move the SIMU audio in this task // TODO move the SIMU audio in this task
audioTaskId = CoCreateTask(audioTask, NULL, 7, &audioStack.stack[AUDIO_STACK_SIZE-1], AUDIO_STACK_SIZE); audioTaskId = CoCreateTask(audioTask, NULL, 7, &audioStack.stack[AUDIO_STACK_SIZE-1], AUDIO_STACK_SIZE);
#endif #endif
audioMutex = CoCreateMutex(); audioMutex = CoCreateMutex();
mixerMutex = CoCreateMutex(); mixerMutex = CoCreateMutex();
CoStartOS(); CoStartOS();
} }

476
radio/src/templates.cpp
View file

@ -18,241 +18,241 @@
* GNU General Public License for more details. * GNU General Public License for more details.
*/ */
/* /*
* *
* ============================================================ * ============================================================
* Templates file * Templates file
* *
* eccpm * eccpm
* crow * crow
* throttle cut * throttle cut
* flaperon * flaperon
* elevon * elevon
* v-tail * v-tail
* throttle hold * throttle hold
* Aileron Differential * Aileron Differential
* Spoilers * Spoilers
* Snap Roll * Snap Roll
* ELE->Flap * ELE->Flap
* Flap->ELE * Flap->ELE
* *
*/ */
#include "opentx.h" #include "opentx.h"
#if defined(PCBTARANIS) #if defined(PCBTARANIS)
#pragma message("Templates with virtual inputs (FrSky Taranis) are not implemented!") #pragma message("Templates with virtual inputs (FrSky Taranis) are not implemented!")
#endif #endif
MixData* setDest(uint8_t dch, uint8_t src, bool clear=false) MixData* setDest(uint8_t dch, uint8_t src, bool clear=false)
{ {
uint8_t i = 0; uint8_t i = 0;
MixData * mix; MixData * mix;
while (1) { while (1) {
mix = mixAddress(i); mix = mixAddress(i);
if (mix->srcRaw && mix->destCh <= dch) { if (mix->srcRaw && mix->destCh <= dch) {
if (clear && mix->destCh == dch) { if (clear && mix->destCh == dch) {
deleteExpoMix(0, i); deleteExpoMix(0, i);
} }
else { else {
if (++i==MAX_MIXERS) { if (++i==MAX_MIXERS) {
// TODO should return null pointer but needs to be tested then // TODO should return null pointer but needs to be tested then
mix = mixAddress(0); mix = mixAddress(0);
break; break;
} }
} }
} }
else { else {
break; break;
} }
} }
memmove(mix+1, mix, (MAX_MIXERS-(i+1))*sizeof(MixData) ); memmove(mix+1, mix, (MAX_MIXERS-(i+1))*sizeof(MixData) );
memclear(mix, sizeof(MixData)); memclear(mix, sizeof(MixData));
mix->destCh = dch; mix->destCh = dch;
mix->srcRaw = src; mix->srcRaw = src;
mix->weight = 100; mix->weight = 100;
return mix; return mix;
} }
void mixSetWeight(MixData* md, int8_t weight) void mixSetWeight(MixData* md, int8_t weight)
{ {
u_int8int16_t tmp; u_int8int16_t tmp;
tmp.word=weight; tmp.word=weight;
MD_UNION_TO_WEIGHT(tmp,md); MD_UNION_TO_WEIGHT(tmp,md);
// MD_SETWEIGHT(md,weight); doesn't matter here in code cost compiler optimizes this anyway // MD_SETWEIGHT(md,weight); doesn't matter here in code cost compiler optimizes this anyway
} }
#if defined(PCBTARANIS) #if defined(PCBTARANIS)
#define TMPL_INPUT(x) (MIXSRC_FIRST_INPUT+channel_order(x)-1) #define TMPL_INPUT(x) (MIXSRC_FIRST_INPUT+channel_order(x)-1)
#else #else
#define clearInputs() #define clearInputs()
#define defaultInputs() #define defaultInputs()
#define TMPL_INPUT(x) (MIXSRC_Rud+x-1) #define TMPL_INPUT(x) (MIXSRC_Rud+x-1)
#endif #endif
void clearMixes() void clearMixes()
{ {
memset(g_model.mixData, 0, sizeof(g_model.mixData)); // clear all mixes memset(g_model.mixData, 0, sizeof(g_model.mixData)); // clear all mixes
} }
void clearCurves() void clearCurves()
{ {
memclear(g_model.curves, sizeof(g_model.curves) + sizeof(g_model.points)); // clear all curves 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[]) void setCurve(uint8_t c, const pm_int8_t ar[])
{ {
int8_t * cv = curveAddress(c); int8_t * cv = curveAddress(c);
for (uint8_t i=0; i<5; i++) { for (uint8_t i=0; i<5; i++) {
cv[i] = pgm_read_byte(&ar[i]); cv[i] = pgm_read_byte(&ar[i]);
} }
} }
void setLogicalSwitch(uint8_t idx, uint8_t func, int8_t v1, int8_t v2) void setLogicalSwitch(uint8_t idx, uint8_t func, int8_t v1, int8_t v2)
{ {
LogicalSwitchData *cs = lswAddress(idx-1); LogicalSwitchData *cs = lswAddress(idx-1);
cs->func = func; cs->func = func;
cs->v1 = v1; cs->v1 = v1;
cs->v2 = v2; cs->v2 = v2;
} }
const pm_int8_t heli_ar1[] PROGMEM = {-100, 20, 30, 70, 90}; 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_ar2[] PROGMEM = {80, 70, 60, 70, 100};
const pm_int8_t heli_ar3[] PROGMEM = {100, 90, 80, 90, 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_ar4[] PROGMEM = {-30, -15, 0, 50, 100};
const pm_int8_t heli_ar5[] PROGMEM = {-100, -50, 0, 50, 100}; const pm_int8_t heli_ar5[] PROGMEM = {-100, -50, 0, 50, 100};
void applyTemplate(uint8_t idx) void applyTemplate(uint8_t idx)
{ {
MixData *md; MixData *md;
//CC(STK) -> vSTK //CC(STK) -> vSTK
//ICC(vSTK) -> STK //ICC(vSTK) -> STK
#define ICC(x) icc[(x)-1] #define ICC(x) icc[(x)-1]
uint8_t icc[4] = {0}; uint8_t icc[4] = {0};
for (uint8_t i=0; i<4; i++) { //generate inverse array for (uint8_t i=0; i<4; i++) { //generate inverse array
for(uint8_t j=0; j<4; j++) for(uint8_t j=0; j<4; j++)
if(CC(i+1)==j+MIXSRC_Rud) icc[j]=i; if(CC(i+1)==j+MIXSRC_Rud) icc[j]=i;
} }
switch (idx) { switch (idx) {
case TMPL_CLEAR_MIXES: case TMPL_CLEAR_MIXES:
case TMPL_SIMPLE_4CH: case TMPL_SIMPLE_4CH:
case TMPL_HELI_SETUP: case TMPL_HELI_SETUP:
clearMixes(); clearMixes();
break; break;
} }
switch (idx) { switch (idx) {
// Simple 4-Ch // Simple 4-Ch
case TMPL_SIMPLE_4CH: case TMPL_SIMPLE_4CH:
defaultInputs(); defaultInputs();
setDest(ICC(STK_RUD), TMPL_INPUT(STK_RUD)); setDest(ICC(STK_RUD), TMPL_INPUT(STK_RUD));
setDest(ICC(STK_ELE), TMPL_INPUT(STK_ELE)); setDest(ICC(STK_ELE), TMPL_INPUT(STK_ELE));
setDest(ICC(STK_THR), TMPL_INPUT(STK_THR)); setDest(ICC(STK_THR), TMPL_INPUT(STK_THR));
setDest(ICC(STK_AIL), TMPL_INPUT(STK_AIL)); setDest(ICC(STK_AIL), TMPL_INPUT(STK_AIL));
break; break;
// Sticky-T-Cut // Sticky-T-Cut
case TMPL_STI_THR_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(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_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); 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; 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(11, LS_FUNC_VNEG, STK_THR, -99);
setLogicalSwitch(12, LS_FUNC_VPOS, MIXSRC_CH14, 0); setLogicalSwitch(12, LS_FUNC_VPOS, MIXSRC_CH14, 0);
break; break;
// V-Tail // V-Tail
case TMPL_V_TAIL: case TMPL_V_TAIL:
defaultInputs(); defaultInputs();
setDest(ICC(STK_RUD), TMPL_INPUT(STK_RUD), true); setDest(ICC(STK_RUD), TMPL_INPUT(STK_RUD), true);
md=setDest(ICC(STK_RUD), TMPL_INPUT(STK_ELE)); mixSetWeight(md, -100); 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_RUD), true);
setDest(ICC(STK_ELE), TMPL_INPUT(STK_ELE)); setDest(ICC(STK_ELE), TMPL_INPUT(STK_ELE));
break; break;
// Elevon\\Delta // Elevon\\Delta
case TMPL_ELEVON_DELTA: case TMPL_ELEVON_DELTA:
defaultInputs(); defaultInputs();
setDest(ICC(STK_ELE), MIXSRC_Ele, true); setDest(ICC(STK_ELE), MIXSRC_Ele, true);
setDest(ICC(STK_ELE), MIXSRC_Ail); setDest(ICC(STK_ELE), MIXSRC_Ail);
setDest(ICC(STK_AIL), MIXSRC_Ele, true); setDest(ICC(STK_AIL), MIXSRC_Ele, true);
md=setDest(ICC(STK_AIL), MIXSRC_Ail); mixSetWeight(md, -100); md=setDest(ICC(STK_AIL), MIXSRC_Ail); mixSetWeight(md, -100);
break; break;
// eCCPM // eCCPM
case TMPL_ECCPM: case TMPL_ECCPM:
md=setDest(ICC(STK_ELE), MIXSRC_Ele, true); md->weight= 72; 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_ELE), MIXSRC_Thr); md->weight= 55;
md=setDest(ICC(STK_AIL), MIXSRC_Ele, true); mixSetWeight(md, -36); 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_Ail); md->weight= 62;
md=setDest(ICC(STK_AIL), MIXSRC_Thr); md->weight= 55; md=setDest(ICC(STK_AIL), MIXSRC_Thr); md->weight= 55;
md=setDest(5, MIXSRC_Ele, true); mixSetWeight(md, -36); md=setDest(5, MIXSRC_Ele, true); mixSetWeight(md, -36);
md=setDest(5, MIXSRC_Ail); mixSetWeight(md, -62); md=setDest(5, MIXSRC_Ail); mixSetWeight(md, -62);
md=setDest(5, MIXSRC_Thr); md->weight= 55; md=setDest(5, MIXSRC_Thr); md->weight= 55;
break; break;
// Heli Setup // Heli Setup
case TMPL_HELI_SETUP: case TMPL_HELI_SETUP:
clearCurves(); clearCurves();
//Set up Mixes //Set up Mixes
// 3 cyclic channels // 3 cyclic channels
md=setDest(0, MIXSRC_CYC1); // md->weight=100; md=setDest(0, MIXSRC_CYC1); // md->weight=100;
md=setDest(1, MIXSRC_CYC2); // md->weight=100; md=setDest(1, MIXSRC_CYC2); // md->weight=100;
md=setDest(2, MIXSRC_CYC3); // md->weight=100; md=setDest(2, MIXSRC_CYC3); // md->weight=100;
// rudder // rudder
md=setDest(3, MIXSRC_Rud); // md->weight=100; md=setDest(3, MIXSRC_Rud); // md->weight=100;
// throttle // throttle
#if defined(PCBTARANIS) #if defined(PCBTARANIS)
// TODO // TODO
#else #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_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_ID1; mixSetCurve(md, 1); md->carryTrim=TRIM_OFF;
md=setDest(4, MIXSRC_Thr); md->swtch=SWSRC_ID2; mixSetCurve(md, 2); md->carryTrim=TRIM_OFF; md=setDest(4, MIXSRC_Thr); md->swtch=SWSRC_ID2; mixSetCurve(md, 2); md->carryTrim=TRIM_OFF;
#endif #endif
md=setDest(4, MIXSRC_MAX); mixSetWeight(md, -100); md->swtch=SWSRC_THR; md->mltpx=MLTPX_REP; md=setDest(4, MIXSRC_MAX); mixSetWeight(md, -100); md->swtch=SWSRC_THR; md->mltpx=MLTPX_REP;
// gyro gain // gyro gain
md=setDest(5, MIXSRC_MAX); md->weight= 30; md->swtch=-SWSRC_GEA; md=setDest(5, MIXSRC_MAX); md->weight= 30; md->swtch=-SWSRC_GEA;
md=setDest(5, MIXSRC_MAX); mixSetWeight(md, -30); md->swtch= SWSRC_GEA; md=setDest(5, MIXSRC_MAX); mixSetWeight(md, -30); md->swtch= SWSRC_GEA;
// collective // collective
#if defined(PCBTARANIS) #if defined(PCBTARANIS)
// TODO // TODO
#else #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_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_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; md=setDest(10, MIXSRC_Thr); /*md->weight= 100;*/ md->swtch=SWSRC_ID2; mixSetCurve(md, 5); md->carryTrim=TRIM_OFF;
#endif #endif
g_model.swashR.collectiveSource = MIXSRC_CH11; g_model.swashR.collectiveSource = MIXSRC_CH11;
g_model.swashR.type = SWASH_TYPE_120; g_model.swashR.type = SWASH_TYPE_120;
// curves // curves
setCurve(0, heli_ar1); setCurve(0, heli_ar1);
setCurve(1, heli_ar2); setCurve(1, heli_ar2);
setCurve(2, heli_ar3); setCurve(2, heli_ar3);
setCurve(3, heli_ar4); setCurve(3, heli_ar4);
setCurve(4, heli_ar5); setCurve(4, heli_ar5);
setCurve(5, heli_ar5); setCurve(5, heli_ar5);
break; break;
// Servo Test // Servo Test
case TMPL_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; 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); setLogicalSwitch(1, LS_FUNC_VNEG, MIXSRC_LAST_CH, 0);
break; break;
default: default:
break; break;
} }
storageDirty(EE_MODEL); storageDirty(EE_MODEL);
} }

1424
radio/src/translations.cpp
View file

File diff suppressed because it is too large Load diff

272
radio/src/vario.cpp
View file

@ -18,139 +18,139 @@
* GNU General Public License for more details. * GNU General Public License for more details.
*/ */
#include "opentx.h" #include "opentx.h"
#if defined(CPUARM) #if defined(CPUARM)
void varioWakeup() void varioWakeup()
{ {
if (isFunctionActive(FUNCTION_VARIO)) { if (isFunctionActive(FUNCTION_VARIO)) {
int varioFreq, varioDuration, varioPause=0; int varioFreq, varioDuration, varioPause=0;
uint8_t varioFlags; uint8_t varioFlags;
int verticalSpeed = 0; int verticalSpeed = 0;
if (g_model.frsky.varioSource) { if (g_model.frsky.varioSource) {
uint8_t item = g_model.frsky.varioSource-1; uint8_t item = g_model.frsky.varioSource-1;
if (item < MAX_SENSORS) { if (item < MAX_SENSORS) {
verticalSpeed = telemetryItems[item].value * g_model.telemetrySensors[item].getPrecMultiplier(); verticalSpeed = telemetryItems[item].value * g_model.telemetrySensors[item].getPrecMultiplier();
} }
} }
int varioCenterMin = (int)g_model.frsky.varioCenterMin * 10 - 50; int varioCenterMin = (int)g_model.frsky.varioCenterMin * 10 - 50;
int varioCenterMax = (int)g_model.frsky.varioCenterMax * 10 + 50; int varioCenterMax = (int)g_model.frsky.varioCenterMax * 10 + 50;
int varioMax = (10+(int)g_model.frsky.varioMax) * 100; int varioMax = (10+(int)g_model.frsky.varioMax) * 100;
int varioMin = (-10+(int)g_model.frsky.varioMin) * 100; int varioMin = (-10+(int)g_model.frsky.varioMin) * 100;
if (verticalSpeed > varioMax) if (verticalSpeed > varioMax)
verticalSpeed = varioMax; verticalSpeed = varioMax;
else if (verticalSpeed < varioMin) else if (verticalSpeed < varioMin)
verticalSpeed = varioMin; verticalSpeed = varioMin;
if (verticalSpeed <= varioCenterMin) { 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); 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 varioDuration = 80; // continuous beep: we will enter again here before the tone ends
varioFlags = PLAY_BACKGROUND|PLAY_NOW; varioFlags = PLAY_BACKGROUND|PLAY_NOW;
} }
else if (verticalSpeed >= varioCenterMax || !g_model.frsky.varioCenterSilent) { 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); 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)); 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) if (verticalSpeed >= varioCenterMax || varioCenterMin == varioCenterMax)
varioDuration = varioPeriod / 5; varioDuration = varioPeriod / 5;
else else
varioDuration = varioPeriod * (85 - (((verticalSpeed-varioCenterMin) * 25) / (varioCenterMax-varioCenterMin))) / 100; varioDuration = varioPeriod * (85 - (((verticalSpeed-varioCenterMin) * 25) / (varioCenterMax-varioCenterMin))) / 100;
varioPause = varioPeriod - varioDuration; varioPause = varioPeriod - varioDuration;
varioFlags = PLAY_BACKGROUND; varioFlags = PLAY_BACKGROUND;
} }
else { else {
return; return;
} }
AUDIO_VARIO(varioFreq, varioDuration, varioPause, varioFlags); AUDIO_VARIO(varioFreq, varioDuration, varioPause, varioFlags);
} }
} }
#elif defined(FRSKY) #elif defined(FRSKY)
void varioWakeup() void varioWakeup()
{ {
static tmr10ms_t s_varioTmr; static tmr10ms_t s_varioTmr;
tmr10ms_t tmr10ms = get_tmr10ms(); tmr10ms_t tmr10ms = get_tmr10ms();
if (isFunctionActive(FUNCTION_VARIO)) { if (isFunctionActive(FUNCTION_VARIO)) {
#if defined(AUDIO) #if defined(AUDIO)
cli(); cli();
int16_t verticalSpeed = frskyData.hub.varioSpeed; int16_t verticalSpeed = frskyData.hub.varioSpeed;
sei(); sei();
#if defined(PCBSTD) #if defined(PCBSTD)
int16_t varioCenterMax = (int16_t)g_model.frsky.varioCenterMax * 10 + 50; int16_t varioCenterMax = (int16_t)g_model.frsky.varioCenterMax * 10 + 50;
if (verticalSpeed >= varioCenterMax) { if (verticalSpeed >= varioCenterMax) {
verticalSpeed = verticalSpeed - varioCenterMax; verticalSpeed = verticalSpeed - varioCenterMax;
int16_t varioMax = (10+(int16_t)g_model.frsky.varioMax) * 100; int16_t varioMax = (10+(int16_t)g_model.frsky.varioMax) * 100;
if (verticalSpeed > varioMax) verticalSpeed = varioMax; if (verticalSpeed > varioMax) verticalSpeed = varioMax;
verticalSpeed = (verticalSpeed * 10) / ((varioMax-varioCenterMax) / 100); verticalSpeed = (verticalSpeed * 10) / ((varioMax-varioCenterMax) / 100);
if ((int16_t)(s_varioTmr-tmr10ms) < 0) { if ((int16_t)(s_varioTmr-tmr10ms) < 0) {
uint8_t varioFreq = (verticalSpeed * 10 + 16000) >> 8; uint8_t varioFreq = (verticalSpeed * 10 + 16000) >> 8;
uint8_t varioDuration = (1600 - verticalSpeed) / 100; uint8_t varioDuration = (1600 - verticalSpeed) / 100;
s_varioTmr = tmr10ms + (varioDuration*2); s_varioTmr = tmr10ms + (varioDuration*2);
AUDIO_VARIO(varioFreq, varioDuration); AUDIO_VARIO(varioFreq, varioDuration);
} }
} }
#else #else
int varioCenterMin = (int)g_model.frsky.varioCenterMin * 10 - 50; int varioCenterMin = (int)g_model.frsky.varioCenterMin * 10 - 50;
int varioCenterMax = (int)g_model.frsky.varioCenterMax * 10 + 50; int varioCenterMax = (int)g_model.frsky.varioCenterMax * 10 + 50;
int varioMax = (10+(int)g_model.frsky.varioMax) * 100; int varioMax = (10+(int)g_model.frsky.varioMax) * 100;
int varioMin = (-10+(int)g_model.frsky.varioMin) * 100; int varioMin = (-10+(int)g_model.frsky.varioMin) * 100;
if (verticalSpeed < varioCenterMin || (verticalSpeed > varioCenterMax && (int16_t)(s_varioTmr - tmr10ms) < 0)) { if (verticalSpeed < varioCenterMin || (verticalSpeed > varioCenterMax && (int16_t)(s_varioTmr - tmr10ms) < 0)) {
if (verticalSpeed > varioMax) if (verticalSpeed > varioMax)
verticalSpeed = varioMax; verticalSpeed = varioMax;
else if (verticalSpeed < varioMin) else if (verticalSpeed < varioMin)
verticalSpeed = varioMin; verticalSpeed = varioMin;
uint8_t varioFreq, varioDuration; uint8_t varioFreq, varioDuration;
if (verticalSpeed > 0) { if (verticalSpeed > 0) {
varioFreq = (verticalSpeed * 4 + 8000) >> 7; varioFreq = (verticalSpeed * 4 + 8000) >> 7;
varioDuration = (8000 - verticalSpeed * 5) / 100; varioDuration = (8000 - verticalSpeed * 5) / 100;
} }
else { else {
varioFreq = (verticalSpeed * 3 + 8000) >> 7; varioFreq = (verticalSpeed * 3 + 8000) >> 7;
varioDuration = 20; varioDuration = 20;
} }
s_varioTmr = tmr10ms + (varioDuration/2); s_varioTmr = tmr10ms + (varioDuration/2);
AUDIO_VARIO(varioFreq, varioDuration); AUDIO_VARIO(varioFreq, varioDuration);
} }
#endif #endif
#elif defined(BUZZER) // && !defined(AUDIO) #elif defined(BUZZER) // && !defined(AUDIO)
int8_t verticalSpeed = limit((int16_t)-100, (int16_t)(frskyData.hub.varioSpeed/10), (int16_t)+100); int8_t verticalSpeed = limit((int16_t)-100, (int16_t)(frskyData.hub.varioSpeed/10), (int16_t)+100);
uint16_t interval; uint16_t interval;
if (verticalSpeed == 0) { if (verticalSpeed == 0) {
interval = 300; interval = 300;
} }
else { else {
if (verticalSpeed < 0) { if (verticalSpeed < 0) {
verticalSpeed = -verticalSpeed; verticalSpeed = -verticalSpeed;
warble = 1; warble = 1;
} }
interval = (uint8_t)200 / verticalSpeed; interval = (uint8_t)200 / verticalSpeed;
} }
if (g_tmr10ms - s_varioTmr > interval) { if (g_tmr10ms - s_varioTmr > interval) {
s_varioTmr = g_tmr10ms; s_varioTmr = g_tmr10ms;
if (warble) if (warble)
AUDIO_VARIO_DOWN(); AUDIO_VARIO_DOWN();
else else
AUDIO_VARIO_UP(); AUDIO_VARIO_UP();
} }
#endif #endif
} }
else { else {
s_varioTmr = tmr10ms; s_varioTmr = tmr10ms;
} }
} }
#endif #endif