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