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

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

166
radio/src/debug.cpp
View file

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

746
radio/src/logs.cpp
View file

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

372
radio/src/maths.cpp
View file

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

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

74
radio/src/serial.cpp
View file

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

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

476
radio/src/templates.cpp
View file

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

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